Architecture v1

.gitignore

@@ -11,6 +11,10 @@ __pycache__/
 .ruff_cache/
 .dmypy.json
 
+# python protobuf
+kscaleos_client_py/kos/
+!kscaleos_client_py/kos/__init__.py
+
 # Rust
 target/
 Cargo.lock

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "protos/googleapis"]
+	path = proto/googleapis
+	url = https://github.com/googleapis/googleapis.git

Cargo.toml

@@ -1,15 +1,21 @@
 [workspace]
+resolver = "2"
 
 members = [
-    "kscaleos/bindings",
-    "kscaleos/kscaleos",
+    "kos_core",
+    #    "platforms/*",
+    "daemon",
 ]
-resolver = "2"
 
 [workspace.package]
-
-version = "0.1.0"
-authors = ["Wesley Maa <[email protected]>", "Pawel Budzianowski <[email protected]>", "Benjamin Bolte <[email protected]>"]
+version = "0.1.1"
+authors = [
+    "Benjamin Bolte <[email protected]>",
+    "Denys Bezmenov <[email protected]>",
+    "Jingxiang Mo <[email protected]>",
+    "Pawel Budzianowski <[email protected]>",
+    "Wesley Maa <[email protected]>",
+]
 edition = "2021"
 license = "MIT"
 repository = "https://github.com/kscalelabs/kscaleos"

daemon/Cargo.toml

@@ -0,0 +1,25 @@
+[package]
+name = "daemon"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+kos_core = { path = "../kos_core" }
+kscale_micro = { path = "../platforms/kscale_micro", optional = true }
+kscale_pro = { path = "../platforms/kscale_pro", optional = true }
+sim = { path = "../platforms/sim", optional = true }
+stub = { path = "../platforms/stub", optional = true }
+
+tokio = { version = "1", features = ["full"] }
+tonic = { version = "0.12", features = ["transport"] }
+eyre = "0.6"
+
+# Also can add external platforms here?
+# e.g. third_party_platform = { git = "https://github.com/thirdparty/platform", optional = true }
+
+[features]
+kscale_micro = ["dep:kscale_micro"]
+kscale_pro = ["dep:kscale_pro"]
+sim = ["dep:sim"]
+stub = ["dep:stub"]
+default = ["stub"]

daemon/src/main.rs

@@ -0,0 +1,42 @@
+// TODO: Implement daemon.
+// This will be the main process that will manage the robot.
+// It will run the gRPC server, and, if applicable,
+// the runtime loop (e.g. actuator polling, loaded model inference).
+
+use eyre::Result;
+
+use kos_core::kos_proto::imu::imu_service_server::ImuServiceServer;
+use kos_core::services::*;
+use std::sync::Arc;
+use tonic::transport::Server;
+
+#[tokio::main]
+async fn main() -> Result<(), Box<dyn std::error::Error>> {
+    let (imu) = {
+        #[cfg(feature = "sim")]
+        {
+            (sim::SimIMU::new())
+        }
+
+        #[cfg(feature = "stub")]
+        {
+            (stub::StubIMU::new())
+        }
+    };
+
+    let imu_service = IMUServiceImpl::new(Arc::new(imu));
+
+    // Add additional services here
+    // let another_service = AnotherService::new(...);
+
+    let addr = "0.0.0.0:50051".parse()?;
+
+    println!("Starting gRPC at {}", addr);
+
+    Server::builder()
+        .add_service(ImuServiceServer::new(imu_service))
+        .serve(addr)
+        .await?;
+
+    Ok(())
+}

kos_core/Cargo.toml

@@ -0,0 +1,19 @@
+[package]
+name = "kos_core"
+version = "0.1.0"
+edition = "2021"
+build = "build.rs"
+
+[dependencies]
+serde = { version = "1.0", features = ["derive"] }
+yaml-rust2 = "0.9"
+tonic = { version = "0.12", features = ["transport"] }
+prost = "0.13"
+prost-types = "0.13"
+async-trait = "0.1"
+rumqttc = "0.24"
+tokio = { version = "1", features = ["full"] }
+eyre = "0.6"
+
+[build-dependencies]
+tonic-build = "0.12"

kos_core/build.rs

@@ -0,0 +1,39 @@
+use std::env;
+use std::path::PathBuf;
+
+fn main() {
+    // Path to the Protobuf files
+    let proto_root = "../proto";
+
+    // Where to output the compiled Rust files
+    let out_dir = PathBuf::from(env::var("OUT_DIR").unwrap());
+
+    // List of Protobuf files
+    let protos = [
+        "kos/common.proto",
+        "kos/actuator.proto",
+        "kos/imu.proto",
+        "kos/inference.proto",
+        "kos/process_manager.proto",
+        "kos/system.proto",
+        "google/longrunning/operations.proto",
+    ];
+
+    let includes = [proto_root, &format!("{}/googleapis", proto_root)];
+
+    // Create the output directory
+    std::fs::create_dir_all(out_dir.join("kos")).expect("Failed to create output directory");
+
+    // Configure and compile Protobuf files
+    tonic_build::configure()
+        .build_server(true)
+        .out_dir(out_dir.join("kos"))
+        .compile_protos(&protos, &includes)
+        .expect("Failed to compile protos");
+
+    // Re-run the build script if any of the proto files change
+    for proto in &protos {
+        println!("cargo:rerun-if-changed={}/kos/{}", proto_root, proto);
+    }
+    println!("cargo:rerun-if-changed={}", proto_root);
+}

kos_core/src/config.rs

@@ -0,0 +1,3 @@
+// TODO: Implement config loading.
+// Config should include embodiment information (e.g. limb names, actuator names),
+// as well as hardware parameters (e.g. serial port names, motor types, PID gains).

kos_core/src/grpc_interface.rs

@@ -0,0 +1,39 @@
+pub mod kos {
+    pub mod actuator {
+        tonic::include_proto!("kos/kos.actuator");
+    }
+
+    pub mod common {
+        tonic::include_proto!("kos/kos.common");
+    }
+
+    pub mod imu {
+        tonic::include_proto!("kos/kos.imu");
+    }
+
+    pub mod inference {
+        tonic::include_proto!("kos/kos.inference");
+    }
+
+    pub mod process_manager {
+        tonic::include_proto!("kos/kos.processmanager");
+    }
+
+    pub mod system {
+        tonic::include_proto!("kos/kos.system");
+    }
+}
+
+pub mod google {
+    pub mod longrunning {
+        tonic::include_proto!("kos/google.longrunning");
+    }
+
+    pub mod api {
+        tonic::include_proto!("kos/google.api");
+    }
+
+    pub mod rpc {
+        tonic::include_proto!("kos/google.rpc");
+    }
+}

kos_core/src/hal.rs

@@ -0,0 +1,33 @@
+pub use crate::grpc_interface::google::longrunning::Operation;
+pub use crate::grpc_interface::kos;
+pub use crate::grpc_interface::kos::common::ActionResponse;
+pub use crate::kos_proto::{actuator::*, common::ActionResult, imu::*};
+use async_trait::async_trait;
+use eyre::Result;
+
+#[async_trait]
+pub trait Actuator: Send + Sync {
+    async fn command_actuators(&self, commands: Vec<ActuatorCommand>) -> Result<Vec<ActionResult>>;
+    async fn configure_actuator(&self, config: ConfigureActuatorRequest) -> Result<()>;
+    async fn calibrate_actuator(&self, request: CalibrateActuatorRequest) -> Result<()>;
+    async fn get_actuators_state(
+        &self,
+        actuator_ids: Vec<u32>,
+    ) -> Result<Vec<ActuatorStateResponse>>;
+}
+
+#[async_trait]
+pub trait IMU: Send + Sync {
+    async fn get_values(&self) -> Result<ImuValuesResponse>;
+    async fn calibrate(&self) -> Result<CalibrationStatus>;
+    async fn zero(&self, duration: std::time::Duration) -> Result<ActionResponse>;
+    async fn get_euler(&self) -> Result<EulerAnglesResponse>;
+    async fn get_quaternion(&self) -> Result<QuaternionResponse>;
+}
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum CalibrationStatus {
+    Calibrating,
+    Calibrated,
+    Timeout,
+}

kos_core/src/lib.rs

@@ -0,0 +1,33 @@
+#![allow(unknown_lints)]
+#![allow(clippy::doc_lazy_continuation)]
+
+pub mod config;
+mod grpc_interface;
+pub mod hal;
+pub mod process_manager;
+pub mod services;
+pub mod telemetry;
+
+pub use grpc_interface::google as google_proto;
+pub use grpc_interface::kos as kos_proto;
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn test_config_loading() {
+        let yaml = r#"
+        limbs:
+            LeftArm:
+                port_name: /dev/ttyUSB0
+                motor_configs:
+                    1:
+                        motor_type: Type01
+                        kp: 50.0
+                        kd: 1.0
+        "#;
+        let config: Config = serde_yaml::from_str(yaml).expect("Failed to parse YAML");
+        assert_eq!(config.limbs.len(), 1);
+        assert_eq!(config.limbs.contains_key("LeftArm"), true);
+    }
+}

kos_core/src/process_manager.rs

@@ -0,0 +1,2 @@
+// TODO: Implement process manager.
+// This will manage life cycle of non rust services (e.g. gstreamer, mosquitto etc)

kos_core/src/services/actuator.rs

@@ -0,0 +1,17 @@
+use crate::hal::Actuator;
+use crate::kos_proto::actuator::*;
+use tonic::{Request, Response, Status};
+
+pub struct ActuatorService {
+    actuator: Box<dyn Actuator>,
+}
+
+// #[tonic::async_trait]
+// impl ActuatorService {
+//     async fn command_actuators(
+//         &self,
+//         request: Request<CommandActuatorsRequest>,
+//     ) -> Result<Response<CommandActuatorsResponse>, Status> {
+//         todo!()
+//     }
+// }

kos_core/src/services/imu.rs

@@ -0,0 +1,87 @@
+use crate::grpc_interface::google::longrunning::Operation;
+use crate::hal::IMU;
+use crate::kos_proto::common::ActionResponse;
+use crate::kos_proto::imu::imu_service_server::{ImuService, ImuServiceServer};
+use crate::kos_proto::imu::*;
+use eyre::OptionExt;
+use std::sync::Arc;
+use tonic::{Request, Response, Status};
+
+pub struct IMUServiceImpl {
+    imu: Arc<dyn IMU>,
+}
+
+impl IMUServiceImpl {
+    pub fn new(imu: Arc<dyn IMU>) -> Self {
+        Self { imu }
+    }
+}
+
+#[tonic::async_trait]
+impl ImuService for IMUServiceImpl {
+    async fn get_values(
+        &self,
+        _request: Request<()>,
+    ) -> Result<Response<ImuValuesResponse>, Status> {
+        let values = self
+            .imu
+            .get_values()
+            .await
+            .map_err(|e| Status::internal(format!("Failed to get IMU values, {:?}", e)))?;
+        Ok(Response::new(values))
+    }
+
+    async fn calibrate(&self, _request: Request<()>) -> Result<Response<Operation>, Status> {
+        let _status = self
+            .imu
+            .calibrate()
+            .await
+            .map_err(|e| Status::internal(format!("Failed to calibrate IMU, {:?}", e)))?;
+
+        Ok(Response::new(Operation {
+            name: "operations/calibrate_imu/0".to_string(),
+            metadata: None,
+            done: false,
+            result: None,
+        }))
+    }
+
+    async fn zero(
+        &self,
+        request: Request<ZeroImuRequest>,
+    ) -> Result<Response<ActionResponse>, Status> {
+        let duration = request
+            .into_inner()
+            .duration
+            .ok_or_eyre("Duration is required")
+            .map_err(|_| Status::internal("Failed to parse duration"))?;
+
+        let duration = std::time::Duration::from_nanos(duration.nanos as u64)
+            + std::time::Duration::from_secs(duration.seconds as u64);
+
+        let response = self
+            .imu
+            .zero(duration)
+            .await
+            .map_err(|e| Status::internal(format!("Failed to zero IMU, {:?}", e)))?;
+        Ok(Response::new(response))
+    }
+
+    async fn get_euler(
+        &self,
+        _request: Request<()>,
+    ) -> Result<Response<EulerAnglesResponse>, Status> {
+        Ok(Response::new(self.imu.get_euler().await.map_err(|e| {
+            Status::internal(format!("Failed to get euler, {:?}", e))
+        })?))
+    }
+
+    async fn get_quaternion(
+        &self,
+        _request: Request<()>,
+    ) -> Result<Response<QuaternionResponse>, Status> {
+        Ok(Response::new(self.imu.get_quaternion().await.map_err(
+            |e| Status::internal(format!("Failed to get quaternion, {:?}", e)),
+        )?))
+    }
+}

kos_core/src/services/mod.rs

@@ -0,0 +1,5 @@
+mod actuator;
+mod imu;
+
+pub use actuator::*;
+pub use imu::*;