Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix kactuator #15

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Fix kactuator #15

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b8ae92c
make work with multiple buses
WT-MM Nov 25, 2024
e7560ae
update initializ
WT-MM Nov 25, 2024
9d9c108
fix other
WT-MM Nov 25, 2024
580a1ed
Merge branch 'master' into fix-kactuator
WT-MM Nov 25, 2024
f6f60e2
remove import
WT-MM Nov 25, 2024
d658005
two arms
WT-MM Nov 25, 2024
79ce91c
update ids
WT-MM Nov 25, 2024
2f2dc75
zero on init
WT-MM Nov 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
117 changes: 83 additions & 34 deletions platforms/kbot/src/actuator.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,37 +14,67 @@ use std::collections::HashMap;
use robstride::{MotorType, MotorsSupervisor};

pub struct KBotActuator {
motors_supervisor: MotorsSupervisor,
motors_supervisors: HashMap<String, MotorsSupervisor>,
motor_id_map: HashMap<u32, (String, u8)>,
}

impl KBotActuator {
pub fn new(
_operations_service: Arc<OperationsServiceImpl>,
port: &str,
motor_infos: HashMap<u32, MotorType>,
port_motor_map: HashMap<&str, HashMap<u32, MotorType>>,
verbose: Option<bool>,
max_update_rate: Option<u32>,
zero_on_init: Option<bool>,
) -> Result<Self> {
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::<Result<HashMap<_, _>>>()?;

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::<Result<HashMap<_, _>>>()?;

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))
}
}

Expand All @@ -53,23 +83,33 @@ impl Actuator for KBotActuator {
async fn command_actuators(&self, commands: Vec<ActuatorCommand>) -> Result<Vec<ActionResult>> {
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);
}

Expand Down Expand Up @@ -105,18 +145,24 @@ impl Actuator for KBotActuator {
}

async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result<ActionResponse> {
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);
}

Expand Down Expand Up @@ -153,7 +199,10 @@ impl Actuator for KBotActuator {
&self,
_actuator_ids: Vec<u32>,
) -> Result<Vec<ActuatorStateResponse>> {
let feedback = self.motors_supervisor.get_latest_feedback();
let feedback = self.motors_supervisors
.values()
.flat_map(|supervisor| supervisor.get_latest_feedback())
.collect::<HashMap<_, _>>();
Ok(feedback
.iter()
.map(|(id, state)| ActuatorStateResponse {
Expand Down
37 changes: 23 additions & 14 deletions platforms/kbot/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")?,
),
Expand All @@ -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,
Expand Down
2 changes: 0 additions & 2 deletions pykos/pykos/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,3 @@
__version__ = "0.1.5"

from pykos.client import KOS

from . import services
Loading