Merge pull request #4 from kscalelabs/kos-kbot-dev

made actuators work I guess w/ Wesley of course

.github/CODEOWNERS

@@ -1,17 +1,17 @@
 # Core components
-/kos_core/ @codekansas @hatomist
-/daemon/ @codekansas @hatomist
+/kos_core/ @codekansas @hatomist @WT-MM
+/daemon/ @codekansas @hatomist @WT-MM
 
 # Platform-specific code
 /platforms/kscale_micro/ @codekansas @hatomist
 /platforms/kscale_pro/ @codekansas @hatomist @WT-MM
-/platforms/sim/ @codekansas @hatomist
+/platforms/sim/ @codekansas @hatomist @WT-MM
 /platforms/stub/ @codekansas @hatomist
 
 # Python client
-/pykos/ @codekansas @hatomist
+/pykos/ @codekansas @hatomist @WT-MM
 
 # Build and CI configuration
-/.github/ @codekansas @hatomist
-/toolchain/ @codekansas @hatomist
-*.toml @codekansas @hatomist
+/.github/ @codekansas @hatomist @WT-MM
+/toolchain/ @codekansas @hatomist @WT-MM
+*.toml @codekansas @hatomist @WT-MM

daemon/src/main.rs

@@ -42,10 +42,10 @@ async fn run_server(
     platform: &(dyn Platform + Send + Sync),
     operations_service: Arc<OperationsServiceImpl>,
 ) -> Result<(), Box<dyn std::error::Error>> {
-    let addr = "[::1]:50051".parse()?;
+    let addr = "0.0.0.0:50051".parse()?;
     let mut server_builder = Server::builder();
 
-    let services = platform.create_services(operations_service.clone());
+    let services = platform.create_services(operations_service.clone())?;
 
     let operations_service = OperationsServer::new(operations_service);
 

kos_core/src/hal.rs

@@ -5,6 +5,7 @@ pub use crate::kos_proto::{actuator::*, common::ActionResult, imu::*};
 use async_trait::async_trait;
 use eyre::Result;
 use std::fmt::Display;
+
 #[async_trait]
 pub trait Actuator: Send + Sync {
     async fn command_actuators(&self, commands: Vec<ActuatorCommand>) -> Result<Vec<ActionResult>>;

kos_core/src/lib.rs

@@ -40,7 +40,10 @@ pub trait Platform {
     fn name(&self) -> &'static str;
     fn serial(&self) -> String;
     fn initialize(&mut self, operations_service: Arc<OperationsServiceImpl>) -> eyre::Result<()>;
-    fn create_services(&self, operations_service: Arc<OperationsServiceImpl>) -> Vec<ServiceEnum>;
+    fn create_services(
+        &self,
+        operations_service: Arc<OperationsServiceImpl>,
+    ) -> eyre::Result<Vec<ServiceEnum>>;
     fn shutdown(&mut self) -> eyre::Result<()>;
 }
 

platforms/kbot/Cargo.toml

@@ -10,5 +10,7 @@ description = "KOS platform for KBot"
 [dependencies]
 kos_core = { version = "0.1.1", path = "../../kos_core" }
 eyre = "0.6"
+tracing = "0.1"
+async-trait = "0.1"
 robstride = "0.2.8"
 imu = "0.1.4"

platforms/kbot/src/actuator.rs

@@ -1,64 +1,151 @@
+use crate::{Arc, Operation, OperationsServiceImpl};
 use async_trait::async_trait;
-use eyre::Result;
-use kos_core::google_proto::longrunning::Operation;
-use kos_core::services::OperationsServiceImpl;
+use eyre::{Result, WrapErr};
 use kos_core::{
     hal::{ActionResponse, Actuator, ActuatorCommand, CalibrateActuatorRequest},
-    kos_proto::{actuator::*, common::ActionResult},
+    kos_proto::{
+        actuator::*,
+        common::Error as KosError,
+        common::{ActionResult, ErrorCode},
+    },
 };
 use std::collections::HashMap;
-use std::sync::Arc;
 
-use robstride::{MotorMode, MotorType, Motors, MotorsSupervisor};
+use robstride::{MotorType, MotorsSupervisor};
 
-pub struct KscaleProActuator {
+pub struct KBotActuator {
     motors_supervisor: MotorsSupervisor,
 }
 
-impl KscaleProActuator {
+impl KBotActuator {
     pub fn new(
+        _operations_service: Arc<OperationsServiceImpl>,
         port: &str,
         motor_infos: HashMap<u32, MotorType>,
         verbose: Option<bool>,
         max_update_rate: Option<u32>,
         zero_on_init: Option<bool>,
-    ) -> Self {
+    ) -> Result<Self> {
+        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 motors_supervisor = MotorsSupervisor::new(
             port,
-            &motor_infos,
+            &motor_infos_u8,
             verbose.unwrap_or(false),
-            max_update_rate.unwrap_or(100000),
+            max_update_rate.unwrap_or(100000) as f64,
             zero_on_init.unwrap_or(false),
         )
-        .unwrap();
+        .map_err(|e| eyre::eyre!("Failed to create motors supervisor: {}", e))?;
 
-        KscaleProActuator {
-            motors_supervisor,
-        }
+        Ok(KBotActuator { motors_supervisor })
     }
 }
 
 #[async_trait]
-impl Actuator for KscaleProActuator {
-    async fn command_actuators(
-        &self,
-        _commands: Vec<ActuatorCommand>,
-    ) -> Result<Vec<ActionResult>> {
-        Ok(vec![])
+impl Actuator for KBotActuator {
+    async fn command_actuators(&self, commands: Vec<ActuatorCommand>) -> Result<Vec<ActionResult>> {
+        let mut results = vec![];
+        for command in commands {
+            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);
+                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);
+                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);
+                motor_result.push(result);
+            }
+
+            let success = motor_result.iter().all(|r| r.is_ok());
+            let error = if !success {
+                Some(KosError {
+                    code: if motor_result
+                        .iter()
+                        .any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound))
+                    {
+                        ErrorCode::InvalidArgument as i32
+                    } else {
+                        ErrorCode::HardwareFailure as i32
+                    },
+                    message: motor_result
+                        .iter()
+                        .filter_map(|r| r.as_ref().err())
+                        .map(|e| e.to_string())
+                        .collect::<Vec<_>>()
+                        .join("; "),
+                })
+            } else {
+                None
+            };
+
+            results.push(ActionResult {
+                actuator_id: command.actuator_id,
+                success,
+                error,
+            });
+        }
+        Ok(results)
     }
 
-    async fn configure_actuator(
-        &self,
-        _config: ConfigureActuatorRequest,
-    ) -> Result<ActionResponse> {
-        Ok(ActionResponse {
-            success: true,
-            error: None,
-        })
+    async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result<ActionResponse> {
+        let motor_id = config.actuator_id as u8;
+        let mut results = vec![];
+
+        // Configure KP if provided
+        if let Some(kp) = config.kp {
+            let result = self.motors_supervisor.set_kp(motor_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);
+            results.push(result);
+        }
+
+        let success = results.iter().all(|r| r.is_ok());
+        let error = if !success {
+            Some(kos_core::kos_proto::common::Error {
+                code: if results
+                    .iter()
+                    .any(|r| matches!(r, Err(e) if e.kind() == std::io::ErrorKind::NotFound))
+                {
+                    ErrorCode::InvalidArgument as i32
+                } else {
+                    ErrorCode::HardwareFailure as i32
+                },
+                message: results
+                    .iter()
+                    .filter_map(|r| r.as_ref().err())
+                    .map(|e| e.to_string())
+                    .collect::<Vec<_>>()
+                    .join("; "),
+            })
+        } else {
+            None
+        };
+
+        Ok(ActionResponse { success, error })
     }
 
-    async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result<Operation> {
+    async fn calibrate_actuator(&self, _request: CalibrateActuatorRequest) -> Result<Operation> {
         Ok(Operation::default())
     }
 
@@ -70,11 +157,11 @@ impl Actuator for KscaleProActuator {
         Ok(feedback
             .iter()
             .map(|(id, state)| ActuatorStateResponse {
-                actuator_id: *id,
-                online: state.mode==MotorMode.Motor,
-                position: state.position,
-                velocity: state.velocity,
-                torque: state.torque,
+                actuator_id: u32::from(*id),
+                online: matches!(state.mode, robstride::MotorMode::Motor),
+                position: Some(state.position.to_degrees() as f64),
+                velocity: Some(state.velocity as f64),
+                torque: Some(state.torque as f64),
                 temperature: None,
                 voltage: None,
                 current: None,