Skip to content

Commit

Permalink
moves but dies
Browse files Browse the repository at this point in the history
  • Loading branch information
WT-MM committed Dec 22, 2024
1 parent 3e682be commit c5315db
Show file tree
Hide file tree
Showing 2 changed files with 32 additions and 13 deletions.
22 changes: 11 additions & 11 deletions actuator/bindings/src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -55,12 +55,12 @@ struct PyRobstrideActuatorCommand {
#[pymethods]
impl PyRobstrideActuatorCommand {
#[new]
fn new(actuator_id: u32) -> Self {
fn new(actuator_id: u32, position: Option<f64>, velocity: Option<f64>, torque: Option<f64>) -> Self {
Self {
actuator_id,
position: None,
velocity: None,
torque: None,
position,
velocity,
torque,
}
}
}
Expand Down Expand Up @@ -89,15 +89,15 @@ struct PyRobstrideConfigureRequest {
#[pymethods]
impl PyRobstrideConfigureRequest {
#[new]
fn new(actuator_id: u32) -> Self {
fn new(actuator_id: u32, kp: Option<f64>, kd: Option<f64>, max_torque: Option<f64>, torque_enabled: Option<bool>, zero_position: Option<bool>, new_actuator_id: Option<u32>) -> Self {
Self {
actuator_id,
kp: None,
kd: None,
max_torque: None,
torque_enabled: None,
zero_position: None,
new_actuator_id: None,
kp,
kd,
max_torque,
torque_enabled,
zero_position,
new_actuator_id,
}
}
}
Expand Down
23 changes: 21 additions & 2 deletions examples/supervisor.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,9 @@
import argparse
import logging
import time
import math

from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand
from actuator import RobstrideActuatorConfig, RobstrideSupervisor, RobstrideActuatorCommand, RobstrideConfigureRequest

def setup_logging() -> None:
"""Set up logging configuration."""
Expand All @@ -29,6 +30,11 @@ def main() -> None:
parser.add_argument("--motor-type", type=int, default=2)
parser.add_argument("--polling-interval", type=float, default=0.001)
parser.add_argument("--verbose", action="store_true")

parser.add_argument("--test-sinusoidal", action="store_true", help="Enable sinusoidal motion testing")
parser.add_argument("--sin-period", type=float, default=20.0, help="Period of sinusoidal motion in seconds")
parser.add_argument("--sin-amplitude", type=float, default=10.0, help="Amplitude of sinusoidal motion in degrees")

args = parser.parse_args()

if args.verbose:
Expand All @@ -48,6 +54,12 @@ def main() -> None:
logger.error(f"Failed to initialize supervisor: {e}")
return

start_time = time.time()

if args.test_sinusoidal:
logger.info(f"Configuring motor {args.motor_id} for sinusoidal motion")
supervisor.configure_actuator(RobstrideConfigureRequest(args.motor_id, kp=5.0, kd=1.0, max_torque=5.0, torque_enabled=True))

try:
while True:
states = supervisor.get_actuators_state([args.motor_id])
Expand All @@ -60,9 +72,16 @@ def main() -> None:
logger.info(f" Velocity: {state.velocity:.2f}°/s")
logger.info(f" Torque: {state.torque:.2f}")
logger.info(f" Temperature: {state.temperature:.2f}°C")

if args.test_sinusoidal:
elapsed_time = time.time() - start_time
target_position = args.sin_amplitude * math.sin(2 * math.pi * elapsed_time / args.sin_period)
cmd = RobstrideActuatorCommand(args.motor_id, position=target_position_rad)
supervisor.command_actuators([cmd])
logger.info(f" Target Position: {target_position:.2f}°")
else:
logger.warning("No motor states received")
time.sleep(1)
time.sleep(0.1)
except KeyboardInterrupt:
logger.info("\nShutting down gracefully...")

Expand Down

0 comments on commit c5315db

Please sign in to comment.