diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 7ed66a2..ccf9ab6 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -14,37 +14,67 @@ use std::collections::HashMap; use robstride::{MotorType, MotorsSupervisor}; pub struct KBotActuator { - motors_supervisor: MotorsSupervisor, + motors_supervisors: HashMap, + motor_id_map: HashMap, } impl KBotActuator { pub fn new( _operations_service: Arc, - port: &str, - motor_infos: HashMap, + port_motor_map: HashMap<&str, HashMap>, verbose: Option, max_update_rate: Option, zero_on_init: Option, ) -> Result { - let motor_infos_u8 = motor_infos + let mut motor_id_map = HashMap::new(); + + let motors_supervisors = port_motor_map .into_iter() - .map(|(k, v)| { - let id = - u8::try_from(k).wrap_err_with(|| format!("Motor ID {} too large for u8", k))?; - Ok((id, v)) + .map(|(port, motor_infos)| { + for motor_id in motor_infos.keys() { + if motor_id_map.insert(*motor_id, (port.to_string(), *motor_id as u8)).is_some() { + return Err(eyre::eyre!("Duplicate motor ID: {}", motor_id)); + } + } + + let motor_infos_u8 = motor_infos + .into_iter() + .map(|(k, v)| { + let id = u8::try_from(k) + .wrap_err_with(|| format!("Motor ID {} too large for u8", k))?; + Ok((id, v)) + }) + .collect::>>()?; + + let supervisor = MotorsSupervisor::new( + port, + &motor_infos_u8, + verbose.unwrap_or(false), + max_update_rate.unwrap_or(100000) as f64, + zero_on_init.unwrap_or(false), + ) + .map_err(|e| eyre::eyre!("Failed to create motors supervisor for port {}: {}", port, e))?; + + Ok((port.to_string(), supervisor)) }) .collect::>>()?; - let motors_supervisor = MotorsSupervisor::new( - port, - &motor_infos_u8, - verbose.unwrap_or(false), - max_update_rate.unwrap_or(100000) as f64, - zero_on_init.unwrap_or(false), - ) - .map_err(|e| eyre::eyre!("Failed to create motors supervisor: {}", e))?; + Ok(KBotActuator { + motors_supervisors, + motor_id_map, + }) + } + + pub fn get_supervisor_for_motor(&self, motor_id: u32) -> Result<(&MotorsSupervisor, u8)> { + let (port, local_id) = self.motor_id_map + .get(&motor_id) + .ok_or_else(|| eyre::eyre!("Motor ID {} not found", motor_id))?; + + let supervisor = self.motors_supervisors + .get(port) + .ok_or_else(|| eyre::eyre!("Supervisor for port {} not found", port))?; - Ok(KBotActuator { motors_supervisor }) + Ok((supervisor, *local_id)) } } @@ -53,23 +83,33 @@ impl Actuator for KBotActuator { async fn command_actuators(&self, commands: Vec) -> Result> { let mut results = vec![]; for command in commands { + let motor_id = command.actuator_id as u32; + let (supervisor, local_id) = match self.get_supervisor_for_motor(motor_id) { + Ok(supervisor_info) => supervisor_info, + Err(e) => { + results.push(ActionResult { + actuator_id: command.actuator_id, + success: false, + error: Some(KosError { + code: ErrorCode::InvalidArgument as i32, + message: e.to_string(), + }), + }); + continue; + } + }; + let mut motor_result = vec![]; if let Some(position) = command.position { - let result = self - .motors_supervisor - .set_position(command.actuator_id as u8, position.to_radians() as f32); + let result = supervisor.set_position(local_id, position.to_radians() as f32); motor_result.push(result); } if let Some(velocity) = command.velocity { - let result = self - .motors_supervisor - .set_velocity(command.actuator_id as u8, velocity as f32); + let result = supervisor.set_velocity(local_id, velocity as f32); motor_result.push(result); } if let Some(torque) = command.torque { - let result = self - .motors_supervisor - .set_torque(command.actuator_id as u8, torque as f32); + let result = supervisor.set_torque(local_id, torque as f32); motor_result.push(result); } @@ -105,18 +145,24 @@ impl Actuator for KBotActuator { } async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result { - let motor_id = config.actuator_id as u8; - let mut results = vec![]; + let (supervisor, local_id) = match self.get_supervisor_for_motor(config.actuator_id as u32) { + Ok(supervisor_info) => supervisor_info, + Err(e) => return Ok(ActionResponse { + success: false, + error: Some(KosError { + code: ErrorCode::InvalidArgument as i32, + message: e.to_string(), + }), + }), + }; - // Configure KP if provided + let mut results = vec![]; if let Some(kp) = config.kp { - let result = self.motors_supervisor.set_kp(motor_id, kp as f32); + let result = supervisor.set_kp(local_id, kp as f32); results.push(result); } - - // Configure KD if provided if let Some(kd) = config.kd { - let result = self.motors_supervisor.set_kd(motor_id, kd as f32); + let result = supervisor.set_kd(local_id, kd as f32); results.push(result); } @@ -153,7 +199,10 @@ impl Actuator for KBotActuator { &self, _actuator_ids: Vec, ) -> Result> { - let feedback = self.motors_supervisor.get_latest_feedback(); + let feedback = self.motors_supervisors + .values() + .flat_map(|supervisor| supervisor.get_latest_feedback()) + .collect::>(); Ok(feedback .iter() .map(|(id, state)| ActuatorStateResponse { diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index e5a07ea..d480d80 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -74,17 +74,25 @@ impl Platform for KbotPlatform { Arc::new( KBotActuator::new( operations_service, - "/dev/ttyCH341USB1", HashMap::from([ - (1, MotorType::Type03), - (2, MotorType::Type03), - (3, MotorType::Type01), - (4, MotorType::Type01), - (5, MotorType::Type01), + ("/dev/ttyCH341USB0", HashMap::from([ + (1, MotorType::Type03), + (2, MotorType::Type03), + (3, MotorType::Type01), + (4, MotorType::Type01), + (5, MotorType::Type01), + ])), + ("/dev/ttyCH341USB1", HashMap::from([ + (11, MotorType::Type03), + (12, MotorType::Type03), + (13, MotorType::Type01), + (14, MotorType::Type01), + (15, MotorType::Type01), + ])), ]), None, None, - None, + True, ) .wrap_err("Failed to create actuator")?, ), @@ -98,14 +106,15 @@ impl Platform for KbotPlatform { ActuatorServiceImpl::new(Arc::new( KBotActuator::new( operations_service, - "/dev/ttyCH341USB0", HashMap::from([ - (1, MotorType::Type04), - (2, MotorType::Type04), - (3, MotorType::Type04), - (4, MotorType::Type04), - (5, MotorType::Type04), - (6, MotorType::Type01), + ("/dev/ttyCH341USB0", HashMap::from([ + (1, MotorType::Type04), + (2, MotorType::Type04), + (3, MotorType::Type04), + (4, MotorType::Type04), + (5, MotorType::Type04), + (6, MotorType::Type01), + ])), ]), None, None, diff --git a/pykos/pykos/__init__.py b/pykos/pykos/__init__.py index 44209bd..d442279 100644 --- a/pykos/pykos/__init__.py +++ b/pykos/pykos/__init__.py @@ -1,5 +1,3 @@ __version__ = "0.1.5" from pykos.client import KOS - -from . import services