Skip to content

Commit

Permalink
send commands synchronously (#27)
Browse files Browse the repository at this point in the history 
* send commands synchronously

* refactoring

* small fixes

* batched tx / rx
  • Loading branch information
@codekansas
codekansas authored Oct 13, 2024
1 parent 26a8d53 commit 6656e3c
Show file tree
Hide file tree
Showing 4 changed files with 372 additions and 289 deletions.
20 changes: 10 additions & 10 deletions actuator/rust/bindings/bindings.pyi
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,13 @@ class PyRobstrideMotors:
def send_get_mode(self) -> dict[int, str]:
...

def send_set_zero(self, motor_ids:typing.Optional[typing.Sequence[int]]) -> dict[int, PyRobstrideMotorFeedback]:
def send_set_zero(self, motor_ids:typing.Optional[typing.Sequence[int]]) -> None:
...

def send_reset(self) -> dict[int, PyRobstrideMotorFeedback]:
def send_resets(self) -> None:
...

def send_start(self) -> dict[int, PyRobstrideMotorFeedback]:
def send_starts(self) -> None:
...

def send_motor_controls(self, motor_controls:typing.Mapping[int, PyRobstrideMotorControlParams]) -> dict[int, PyRobstrideMotorFeedback]:
Expand All @@ -54,31 +54,31 @@ class PyRobstrideMotors:

class PyRobstrideMotorsSupervisor:
def __new__(cls,port_name,motor_infos,verbose = ...,min_update_rate = ...,target_update_rate = ...): ...
def set_position(self, motor_id:int, position:float) -> None:
def set_position(self, motor_id:int, position:float) -> float:
...

def get_position(self, motor_id:int) -> float:
...

def set_velocity(self, motor_id:int, velocity:float) -> None:
def set_velocity(self, motor_id:int, velocity:float) -> float:
...

def get_velocity(self, motor_id:int) -> float:
...

def set_kp(self, motor_id:int, kp:float) -> None:
def set_kp(self, motor_id:int, kp:float) -> float:
...

def get_kp(self, motor_id:int) -> float:
...

def set_kd(self, motor_id:int, kd:float) -> None:
def set_kd(self, motor_id:int, kd:float) -> float:
...

def get_kd(self, motor_id:int) -> float:
...

def set_torque(self, motor_id:int, torque:float) -> None:
def set_torque(self, motor_id:int, torque:float) -> float:
...

def get_torque(self, motor_id:int) -> float:
Expand All @@ -102,10 +102,10 @@ class PyRobstrideMotorsSupervisor:
def set_params(self, motor_id:int, params:PyRobstrideMotorControlParams) -> None:
...

def get_total_commands(self) -> int:
def get_total_commands(self, motor_id:int) -> int:
...

def get_failed_commands(self) -> int:
def get_failed_commands(self, motor_id:int) -> int:
...

def reset_command_counters(self) -> None:
Expand Down
82 changes: 40 additions & 42 deletions actuator/rust/bindings/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,34 +44,22 @@ impl PyRobstrideMotors {
.collect()
}

fn send_set_zero(
&mut self,
motor_ids: Option<Vec<u8>>,
) -> PyResult<HashMap<u8, PyRobstrideMotorFeedback>> {
fn send_set_zero(&mut self, motor_ids: Option<Vec<u8>>) -> PyResult<()> {
self.inner
.send_set_zero(motor_ids.as_deref())
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?
.into_iter()
.map(|(k, v)| Ok((k, v.into())))
.collect()
.send_set_zeros(motor_ids.as_deref())
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn send_reset(&mut self) -> PyResult<HashMap<u8, PyRobstrideMotorFeedback>> {
fn send_resets(&mut self) -> PyResult<()> {
self.inner
.send_reset()
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?
.into_iter()
.map(|(k, v)| Ok((k, v.into())))
.collect()
.send_resets()
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn send_start(&mut self) -> PyResult<HashMap<u8, PyRobstrideMotorFeedback>> {
fn send_starts(&mut self) -> PyResult<()> {
self.inner
.send_start()
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?
.into_iter()
.map(|(k, v)| Ok((k, v.into())))
.collect()
.send_starts()
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn send_motor_controls(
Expand Down Expand Up @@ -254,9 +242,10 @@ impl PyRobstrideMotorsSupervisor {
Ok(PyRobstrideMotorsSupervisor { inner: controller })
}

fn set_position(&self, motor_id: u8, position: f32) -> PyResult<()> {
self.inner.set_position(motor_id, position);
Ok(())
fn set_position(&self, motor_id: u8, position: f32) -> PyResult<f32> {
self.inner
.set_position(motor_id, position)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_position(&self, motor_id: u8) -> PyResult<f32> {
Expand All @@ -265,9 +254,10 @@ impl PyRobstrideMotorsSupervisor {
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn set_velocity(&self, motor_id: u8, velocity: f32) -> PyResult<()> {
self.inner.set_velocity(motor_id, velocity);
Ok(())
fn set_velocity(&self, motor_id: u8, velocity: f32) -> PyResult<f32> {
self.inner
.set_velocity(motor_id, velocity)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_velocity(&self, motor_id: u8) -> PyResult<f32> {
Expand All @@ -276,9 +266,10 @@ impl PyRobstrideMotorsSupervisor {
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn set_kp(&self, motor_id: u8, kp: f32) -> PyResult<()> {
self.inner.set_kp(motor_id, kp);
Ok(())
fn set_kp(&self, motor_id: u8, kp: f32) -> PyResult<f32> {
self.inner
.set_kp(motor_id, kp)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_kp(&self, motor_id: u8) -> PyResult<f32> {
Expand All @@ -287,9 +278,10 @@ impl PyRobstrideMotorsSupervisor {
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn set_kd(&self, motor_id: u8, kd: f32) -> PyResult<()> {
self.inner.set_kd(motor_id, kd);
Ok(())
fn set_kd(&self, motor_id: u8, kd: f32) -> PyResult<f32> {
self.inner
.set_kd(motor_id, kd)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_kd(&self, motor_id: u8) -> PyResult<f32> {
Expand All @@ -298,9 +290,10 @@ impl PyRobstrideMotorsSupervisor {
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn set_torque(&self, motor_id: u8, torque: f32) -> PyResult<()> {
self.inner.set_torque(motor_id, torque);
Ok(())
fn set_torque(&self, motor_id: u8, torque: f32) -> PyResult<f32> {
self.inner
.set_torque(motor_id, torque)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_torque(&self, motor_id: u8) -> PyResult<f32> {
Expand All @@ -310,8 +303,9 @@ impl PyRobstrideMotorsSupervisor {
}

fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> {
self.inner.add_motor_to_zero(motor_id);
Ok(())
self.inner
.add_motor_to_zero(motor_id)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_latest_feedback(&self) -> HashMap<u8, PyRobstrideMotorFeedback> {
Expand Down Expand Up @@ -354,12 +348,16 @@ impl PyRobstrideMotorsSupervisor {
Ok(())
}

fn get_total_commands(&self) -> PyResult<u64> {
Ok(self.inner.get_total_commands())
fn get_total_commands(&self, motor_id: u8) -> PyResult<u64> {
self.inner
.get_total_commands(motor_id)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn get_failed_commands(&self) -> PyResult<u64> {
Ok(self.inner.get_failed_commands())
fn get_failed_commands(&self, motor_id: u8) -> PyResult<u64> {
self.inner
.get_failed_commands(motor_id)
.map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
}

fn reset_command_counters(&self) -> PyResult<()> {
Expand Down
14 changes: 8 additions & 6 deletions actuator/rust/robstride/src/bin/supervisor.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
continue;
}
let position: f32 = parts[1].parse()?;
controller.set_position(test_id, position);
let _ = controller.set_position(test_id, position);
println!("Set target position to {}", position);
}
"v" => {
Expand All @@ -95,23 +95,25 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
continue;
}
let velocity: f32 = parts[1].parse()?;
controller.set_velocity(test_id, velocity);
let _ = controller.set_velocity(test_id, velocity);
println!("Set target velocity to {}", velocity);
}
"t" => {
if parts.len() != 2 {
println!("Usage: t <torque>");
continue;
}
let torque: f32 = parts[1].parse()?;
controller.set_torque(test_id, torque);
let _ = controller.set_torque(test_id, torque);
println!("Set target torque to {}", torque);
}
"kp" => {
if parts.len() != 2 {
println!("Usage: kp <kp>");
continue;
}
let kp: f32 = parts[1].parse()?;
controller.set_kp(test_id, kp);
let _ = controller.set_kp(test_id, kp);
println!("Set KP for motor {} to {}", test_id, kp);
}
"kd" => {
Expand All @@ -120,11 +122,11 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
continue;
}
let kd: f32 = parts[1].parse()?;
controller.set_kd(test_id, kd);
let _ = controller.set_kd(test_id, kd);
println!("Set KD for motor {} to {}", test_id, kd);
}
"zero" | "z" => {
controller.add_motor_to_zero(test_id);
let _ = controller.add_motor_to_zero(test_id);
println!("Added motor {} to zero list", test_id);
}
"get_feedback" | "g" => {
Expand Down
Loading

0 comments on commit 6656e3c

Please sign in to comment.