From d94f8ffb1a7afba417f6348c84a77c17f8bf9763 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Tue, 3 Dec 2024 06:40:52 +0000 Subject: [PATCH 1/6] zeroing, change ids --- platforms/kbot/src/actuator.rs | 10 ++++++++++ platforms/kbot/src/lib.rs | 3 +-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 22066f0..730121e 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -115,6 +115,16 @@ impl Actuator for KBotActuator { } } + if let Some(zero_position) = config.zero_position { + if zero_position { + supervisor.zero(motor_id).await?; + } + } + + if let Some(new_actuator_id) = config.new_actuator_id { + supervisor.change_id(motor_id, new_actuator_id as u8).await?; + } + let success = result.is_ok(); let error = result.err().map(|e| KosError { code: ErrorCode::HardwareFailure as i32, diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 6c8a91b..275bd6b 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -11,7 +11,7 @@ pub use hexmove::*; pub use process_manager::*; use async_trait::async_trait; -use eyre::{Result, WrapErr}; +use eyre::WrapErr; use kos_core::hal::Operation; use kos_core::kos_proto::actuator::actuator_service_server::ActuatorServiceServer; use kos_core::kos_proto::process_manager::process_manager_service_server::ProcessManagerServiceServer; @@ -19,7 +19,6 @@ use kos_core::{ services::{ActuatorServiceImpl, OperationsServiceImpl, ProcessManagerServiceImpl}, Platform, ServiceEnum, }; -use std::collections::HashMap; use std::future::Future; use std::pin::Pin; use std::sync::Arc; From be57f3b3e7e61ed1a218a9b5a1f71be24974a275 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 01:32:34 +0000 Subject: [PATCH 2/6] actuator types --- platforms/kbot/Cargo.toml | 2 +- platforms/kbot/src/actuator.rs | 10 ++++++---- platforms/kbot/src/lib.rs | 31 +++++++++++++++++++++++++------ 3 files changed, 32 insertions(+), 11 deletions(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index b613239..75e599b 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = "0.1" +robstridev2 = { version = "0.1", path = "../../../robstride" } gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20" diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 730121e..7bf48be 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -22,9 +22,11 @@ impl KBotActuator { pub async fn new( _operations_service: Arc, ports: Vec<&str>, - motors_timeout: Duration, + actuator_timeout: Duration, + polling_interval: Duration, + desired_actuator_types: &[(u8, robstridev2::ActuatorType)], ) -> Result { - let mut supervisor = Supervisor::new(motors_timeout)?; + let mut supervisor = Supervisor::new(actuator_timeout)?; for port in ports.clone() { if port.starts_with("/dev/tty") { @@ -44,13 +46,13 @@ impl KBotActuator { let mut supervisor_runner = supervisor.clone_controller(); let _supervisor_handle = tokio::spawn(async move { - if let Err(e) = supervisor_runner.run().await { + if let Err(e) = supervisor_runner.run(polling_interval).await { tracing::error!("Supervisor task failed: {}", e); } }); for port in ports.clone() { - supervisor.scan_bus(0xFD, port).await?; + supervisor.scan_bus(0xFD, port, desired_actuator_types).await?; } Ok(KBotActuator { diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 275bd6b..155bbd7 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -5,6 +5,7 @@ mod process_manager; mod hexmove; pub use actuator::*; +pub use robstridev2::ActuatorType; #[cfg(target_os = "linux")] pub use hexmove::*; @@ -65,9 +66,21 @@ impl Platform for KbotPlatform { .wrap_err("Failed to initialize GStreamer process manager")?; let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04), + (1, ActuatorType::RobStride04), // Left Hip + (2, robstridev2::ActuatorType::RobStride03), // Left Knee + (3, robstridev2::ActuatorType::RobStride03), // Right Hip + (4, robstridev2::ActuatorType::RobStride04), // Right Knee + (5, robstridev2::ActuatorType::RobStride02), // Torso + ], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( @@ -79,9 +92,15 @@ impl Platform for KbotPlatform { ]) } else { let actuator = - KBotActuator::new(operations_service, vec!["can0"], Duration::from_secs(1)) - .await - .wrap_err("Failed to create actuator")?; + KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04)], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( ActuatorServiceImpl::new(Arc::new(actuator)), From fa63742e64dba7d30be6b9663d133e5bfa5fed4a Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 01:54:00 +0000 Subject: [PATCH 3/6] k1 config --- platforms/kbot/src/lib.rs | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 155bbd7..32b96d8 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -71,12 +71,36 @@ impl Platform for KbotPlatform { vec!["can0"], Duration::from_secs(1), Duration::from_nanos(3_333_333), - &[(1, robstridev2::ActuatorType::RobStride04), - (1, ActuatorType::RobStride04), // Left Hip - (2, robstridev2::ActuatorType::RobStride03), // Left Knee - (3, robstridev2::ActuatorType::RobStride03), // Right Hip - (4, robstridev2::ActuatorType::RobStride04), // Right Knee - (5, robstridev2::ActuatorType::RobStride02), // Torso + &[ + // Right Leg + (10, ActuatorType::RobStride04), + (11, ActuatorType::RobStride03), + (12, ActuatorType::RobStride03), + (13, ActuatorType::RobStride04), + (14, ActuatorType::RobStride02), + + // Left Leg + (20, ActuatorType::RobStride04), + (21, ActuatorType::RobStride03), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride04), + (24, ActuatorType::RobStride02), + + // Right Arm + (30, ActuatorType::RobStride03), + (31, ActuatorType::RobStride03), + (32, ActuatorType::RobStride02), + (33, ActuatorType::RobStride02), + (34, ActuatorType::RobStride02), + (35, ActuatorType::RobStride00), + + // Left Arm + (40, ActuatorType::RobStride03), + (41, ActuatorType::RobStride03), + (42, ActuatorType::RobStride02), + (43, ActuatorType::RobStride02), + (44, ActuatorType::RobStride02), + (45, ActuatorType::RobStride00), ], ) .await From 273f3e17addfda11e192d1ae49bca00708e0f098 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 03:10:37 +0000 Subject: [PATCH 4/6] convert to proto units of measurement --- platforms/kbot/src/actuator.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 7bf48be..4505f6b 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -75,7 +75,7 @@ impl Actuator for KBotActuator { .position .map(|p| p.to_radians() as f32) .unwrap_or(0.0), - command.velocity.map(|v| v as f32).unwrap_or(0.0), + command.velocity.map(|v| v.to_radians() as f32).unwrap_or(0.0), command.torque.map(|t| t as f32).unwrap_or(0.0), ) .await; @@ -151,8 +151,8 @@ impl Actuator for KBotActuator { responses.push(ActuatorStateResponse { actuator_id: id, online: ts.elapsed().unwrap_or(Duration::from_secs(1)) < Duration::from_secs(1), - position: Some(feedback.angle as f64), - velocity: Some(feedback.velocity as f64), + position: Some(feedback.angle.to_degrees() as f64), + velocity: Some(feedback.velocity.to_degrees() as f64), torque: Some(feedback.torque as f64), temperature: Some(feedback.temperature as f64), voltage: None, From f29687bbd7093d114991698964d0dfdb53b6d1c5 Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 06:07:50 +0000 Subject: [PATCH 5/6] fmt --- platforms/kbot/src/actuator.rs | 13 +++-- platforms/kbot/src/lib.rs | 89 ++++++++++++++++------------------ 2 files changed, 52 insertions(+), 50 deletions(-) diff --git a/platforms/kbot/src/actuator.rs b/platforms/kbot/src/actuator.rs index 4505f6b..41a2d0d 100644 --- a/platforms/kbot/src/actuator.rs +++ b/platforms/kbot/src/actuator.rs @@ -52,7 +52,9 @@ impl KBotActuator { }); for port in ports.clone() { - supervisor.scan_bus(0xFD, port, desired_actuator_types).await?; + supervisor + .scan_bus(0xFD, port, desired_actuator_types) + .await?; } Ok(KBotActuator { @@ -75,7 +77,10 @@ impl Actuator for KBotActuator { .position .map(|p| p.to_radians() as f32) .unwrap_or(0.0), - command.velocity.map(|v| v.to_radians() as f32).unwrap_or(0.0), + command + .velocity + .map(|v| v.to_radians() as f32) + .unwrap_or(0.0), command.torque.map(|t| t as f32).unwrap_or(0.0), ) .await; @@ -124,7 +129,9 @@ impl Actuator for KBotActuator { } if let Some(new_actuator_id) = config.new_actuator_id { - supervisor.change_id(motor_id, new_actuator_id as u8).await?; + supervisor + .change_id(motor_id, new_actuator_id as u8) + .await?; } let success = result.is_ok(); diff --git a/platforms/kbot/src/lib.rs b/platforms/kbot/src/lib.rs index 32b96d8..92afb8d 100644 --- a/platforms/kbot/src/lib.rs +++ b/platforms/kbot/src/lib.rs @@ -65,46 +65,42 @@ impl Platform for KbotPlatform { KBotProcessManager::new(self.name().to_string(), self.serial()) .wrap_err("Failed to initialize GStreamer process manager")?; - let actuator = - KBotActuator::new( - operations_service, - vec!["can0"], - Duration::from_secs(1), - Duration::from_nanos(3_333_333), - &[ - // Right Leg - (10, ActuatorType::RobStride04), - (11, ActuatorType::RobStride03), - (12, ActuatorType::RobStride03), - (13, ActuatorType::RobStride04), - (14, ActuatorType::RobStride02), - - // Left Leg - (20, ActuatorType::RobStride04), - (21, ActuatorType::RobStride03), - (22, ActuatorType::RobStride03), - (23, ActuatorType::RobStride04), - (24, ActuatorType::RobStride02), - - // Right Arm - (30, ActuatorType::RobStride03), - (31, ActuatorType::RobStride03), - (32, ActuatorType::RobStride02), - (33, ActuatorType::RobStride02), - (34, ActuatorType::RobStride02), - (35, ActuatorType::RobStride00), - - // Left Arm - (40, ActuatorType::RobStride03), - (41, ActuatorType::RobStride03), - (42, ActuatorType::RobStride02), - (43, ActuatorType::RobStride02), - (44, ActuatorType::RobStride02), - (45, ActuatorType::RobStride00), + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[ + // Right Leg + (10, ActuatorType::RobStride04), + (11, ActuatorType::RobStride03), + (12, ActuatorType::RobStride03), + (13, ActuatorType::RobStride04), + (14, ActuatorType::RobStride02), + // Left Leg + (20, ActuatorType::RobStride04), + (21, ActuatorType::RobStride03), + (22, ActuatorType::RobStride03), + (23, ActuatorType::RobStride04), + (24, ActuatorType::RobStride02), + // Right Arm + (30, ActuatorType::RobStride03), + (31, ActuatorType::RobStride03), + (32, ActuatorType::RobStride02), + (33, ActuatorType::RobStride02), + (34, ActuatorType::RobStride02), + (35, ActuatorType::RobStride00), + // Left Arm + (40, ActuatorType::RobStride03), + (41, ActuatorType::RobStride03), + (42, ActuatorType::RobStride02), + (43, ActuatorType::RobStride02), + (44, ActuatorType::RobStride02), + (45, ActuatorType::RobStride00), ], ) .await - .wrap_err("Failed to create actuator")?; + .wrap_err("Failed to create actuator")?; Ok(vec![ ServiceEnum::Actuator(ActuatorServiceServer::new(ActuatorServiceImpl::new( @@ -115,16 +111,15 @@ impl Platform for KbotPlatform { )), ]) } else { - let actuator = - KBotActuator::new( - operations_service, - vec!["can0"], - Duration::from_secs(1), - Duration::from_nanos(3_333_333), - &[(1, robstridev2::ActuatorType::RobStride04)], - ) - .await - .wrap_err("Failed to create actuator")?; + let actuator = KBotActuator::new( + operations_service, + vec!["can0"], + Duration::from_secs(1), + Duration::from_nanos(3_333_333), + &[(1, robstridev2::ActuatorType::RobStride04)], + ) + .await + .wrap_err("Failed to create actuator")?; Ok(vec![ServiceEnum::Actuator(ActuatorServiceServer::new( ActuatorServiceImpl::new(Arc::new(actuator)), From c59bd751616c4ab4336392b4d99af1db7ec60e6e Mon Sep 17 00:00:00 2001 From: Denys Bezmenov Date: Wed, 4 Dec 2024 06:08:18 +0000 Subject: [PATCH 6/6] deps fix --- platforms/kbot/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/kbot/Cargo.toml b/platforms/kbot/Cargo.toml index 75e599b..71b61f4 100644 --- a/platforms/kbot/Cargo.toml +++ b/platforms/kbot/Cargo.toml @@ -13,7 +13,7 @@ eyre = "0.6" krec = "0.2" tracing = "0.1" async-trait = "0.1" -robstridev2 = { version = "0.1", path = "../../../robstride" } +robstridev2 = "0.2" gstreamer = "0.20" gstreamer-app = "0.20" gstreamer-video = "0.20"