fix: kin_data

lebai-robotics · Jun 7, 2023 · f80641f · f80641f
1 parent aa60ea8
commit f80641f
Show file tree

Hide file tree

Showing 8 changed files with 104 additions and 54 deletions.
diff --git a/proto/src/kinematic.rs b/proto/src/kinematic.rs
@@ -0,0 +1,40 @@
+use crate::lebai::kinematic;
+use crate::posture::{CartesianPose, JointPose};
+use serde::{Deserialize, Serialize};
+
+#[derive(Deserialize, Serialize)]
+pub struct KinData {
+    pub actual_joint_pose: JointPose,
+    pub actual_joint_speed: JointPose,
+    pub actual_joint_acc: JointPose,
+    pub actual_joint_torque: JointPose,
+    pub target_joint_pose: JointPose,
+    pub target_joint_speed: JointPose,
+    pub target_joint_acc: JointPose,
+    pub target_joint_torque: JointPose,
+
+    pub actual_tcp_pose: CartesianPose,
+    pub target_tcp_pose: CartesianPose,
+
+    pub actual_flange_pose: CartesianPose,
+}
+
+impl From<kinematic::KinData> for KinData {
+    fn from(p: kinematic::KinData) -> Self {
+        Self {
+            actual_joint_pose: JointPose(p.actual_joint_pose),
+            actual_joint_speed: JointPose(p.actual_joint_speed),
+            actual_joint_acc: JointPose(p.actual_joint_acc),
+            actual_joint_torque: JointPose(p.actual_joint_torque),
+            target_joint_pose: JointPose(p.target_joint_pose),
+            target_joint_speed: JointPose(p.target_joint_speed),
+            target_joint_acc: JointPose(p.target_joint_acc),
+            target_joint_torque: JointPose(p.target_joint_torque),
+
+            actual_tcp_pose: p.actual_tcp_pose.unwrap_or_default().into(),
+            target_tcp_pose: p.target_tcp_pose.unwrap_or_default().into(),
+
+            actual_flange_pose: p.actual_flange_pose.unwrap_or_default().into(),
+        }
+    }
+}
diff --git a/proto/src/lib.rs b/proto/src/lib.rs
@@ -1,4 +1,5 @@
 pub mod io;
+pub mod kinematic;
 pub mod led;
 pub mod modbus;
 pub mod posture;

diff --git a/src/common.rs b/src/common.rs
@@ -8,7 +8,7 @@ static TIMESTAMP: Lazy<Box<dyn Fn() -> Duration + Send + Sync>> = Lazy::new(|| {
     Box::new(move || time.elapsed() + diff)
 });
 
-pub static VERSION: &'static str = env!("CARGO_PKG_VERSION");
+pub static VERSION: &str = env!("CARGO_PKG_VERSION");
 
 pub fn timestamp() -> Result<u64> {
     Ok(TIMESTAMP().as_millis() as u64)

diff --git a/src/lib.rs b/src/lib.rs
@@ -9,9 +9,9 @@ mod runtime;
 pub mod lebai_sdk {
     use super::*;
     use cmod::Result;
+    use proto::kinematic::KinData;
     use proto::lebai::claw::Claw;
     use proto::lebai::dynamic::Payload;
-    use proto::lebai::kinematic::KinData;
     use proto::lebai::posture::Position;
     use proto::lebai::system::RobotState;
     use proto::led::LedStyle;
@@ -39,16 +39,16 @@ pub mod lebai_sdk {
     #[cmod::function]
     #[cmod::tags(args(ms))]
     pub async fn sleep_ms(ms: u64) -> Result<()> {
-        return common::sleep_ms(ms).await;
+        common::sleep_ms(ms).await
     }
 
     #[cmod::function]
     #[cmod::tags(ret)]
     pub async fn discover_devices(time: u32) -> Result<Vec<proto::lebai::multi_devices::DeviceInfo>> {
         #[cfg(not(feature = "mdns"))]
         {
-            drop(time);
-            return Err("not support".into());
+            let _time = time;
+            Err("not support".into())
         }
         #[cfg(feature = "mdns")]
         mdns::discover_devices(time).await
@@ -206,7 +206,7 @@ pub mod lebai_sdk {
         }
         #[classmethod]
         #[cmod::tags(args(device))]
-        pub async fn set_ao(&self, device: String, pin: u32, value: u32) -> Result<()> {
+        pub async fn set_ao(&self, device: String, pin: u32, value: f64) -> Result<()> {
             self.0.set_ao(device, pin, value).await
         }
         #[classmethod]

diff --git a/src/rpc/dynamic.rs b/src/rpc/dynamic.rs
@@ -3,40 +3,9 @@ use cmod::Result;
 use proto::google::protobuf::Empty;
 use proto::lebai::db::LoadRequest;
 use proto::lebai::dynamic::*;
-use proto::lebai::kinematic::*;
 use proto::lebai::posture::Position;
-use proto::posture::CartesianPose;
 
 impl Robot {
-    pub(crate) async fn load_tcp(&self, name: String, dir: Option<String>) -> Result<CartesianPose> {
-        let req = LoadRequest {
-            name,
-            dir: dir.unwrap_or_default(),
-        };
-        let resp = self.c.load_tcp(Some(req)).await.map_err(|e| e.to_string())?;
-        Ok(resp.into())
-    }
-    pub(crate) async fn set_tcp(&self, pose: CartesianPose) -> Result<()> {
-        let _ = self.c.set_tcp(Some(pose.into())).await.map_err(|e| e.to_string())?;
-        Ok(())
-    }
-    pub(crate) async fn get_tcp(&self) -> Result<CartesianPose> {
-        let resp = self.c.get_tcp(Some(Empty {})).await.map_err(|e| e.to_string())?;
-        Ok(resp.into())
-    }
-    pub(crate) async fn set_velocity_factor(&self, speed_factor: i32) -> Result<()> {
-        let req = KinFactor { speed_factor };
-        let _ = self.c.set_kin_factor(Some(req)).await.map_err(|e| e.to_string())?;
-        Ok(())
-    }
-    pub(crate) async fn get_velocity_factor(&self) -> Result<i32> {
-        let resp = self.c.get_kin_factor(Some(Empty {})).await.map_err(|e| e.to_string())?;
-        Ok(resp.speed_factor)
-    }
-    pub(crate) async fn get_kin_data(&self) -> Result<KinData> {
-        let resp = self.c.get_kin_data(Some(Empty {})).await.map_err(|e| e.to_string())?;
-        Ok(resp)
-    }
     pub(crate) async fn load_payload(&self, name: String, dir: Option<String>) -> Result<Payload> {
         let req = LoadRequest {
             name,

diff --git a/src/rpc/io.rs b/src/rpc/io.rs
@@ -6,85 +6,85 @@ impl Robot {
     pub(crate) async fn set_do(&self, device: String, pin: u32, value: u32) -> Result<()> {
         let req = SetDoPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            value: value.into(),
+            pin,
+            value,
         };
         let _ = self.c.set_do(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(())
     }
     pub(crate) async fn get_do(&self, device: String, pin: u32) -> Result<u32> {
         let req = GetDioPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
+            pin,
         };
         let resp = self.c.get_do(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.value)
     }
     pub(crate) async fn get_dos(&self, device: String, pin: u32, num: u32) -> Result<Vec<u32>> {
         let req = GetDioPinsRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            count: num.into(),
+            pin,
+            count: num,
         };
         let resp = self.c.get_dos(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.values)
     }
     pub(crate) async fn get_di(&self, device: String, pin: u32) -> Result<u32> {
         let req = GetDioPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
+            pin,
         };
         let resp = self.c.get_di(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.value)
     }
     pub(crate) async fn get_dis(&self, device: String, pin: u32, num: u32) -> Result<Vec<u32>> {
         let req = GetDioPinsRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            count: num.into(),
+            pin,
+            count: num,
         };
         let resp = self.c.get_dis(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.values)
     }
-    pub(crate) async fn set_ao(&self, device: String, pin: u32, value: u32) -> Result<()> {
+    pub(crate) async fn set_ao(&self, device: String, pin: u32, value: f64) -> Result<()> {
         let req = SetAoPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            value: value.into(),
+            pin,
+            value,
         };
         let _ = self.c.set_ao(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(())
     }
     pub(crate) async fn get_ao(&self, device: String, pin: u32) -> Result<f64> {
         let req = GetAioPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
+            pin,
         };
         let resp = self.c.get_ao(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.value)
     }
     pub(crate) async fn get_aos(&self, device: String, pin: u32, num: u32) -> Result<Vec<f64>> {
         let req = GetAioPinsRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            count: num.into(),
+            pin,
+            count: num,
         };
         let resp = self.c.get_aos(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.values)
     }
     pub(crate) async fn get_ai(&self, device: String, pin: u32) -> Result<f64> {
         let req = GetAioPinRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
+            pin,
         };
         let resp = self.c.get_ai(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.value)
     }
     pub(crate) async fn get_ais(&self, device: String, pin: u32, num: u32) -> Result<Vec<f64>> {
         let req = GetAioPinsRequest {
             device: IoDevice::from(&*device).into(),
-            pin: pin.into(),
-            count: num.into(),
+            pin,
+            count: num,
         };
         let resp = self.c.get_ais(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(resp.values)

diff --git a/src/rpc/kinematic.rs b/src/rpc/kinematic.rs
@@ -0,0 +1,39 @@
+use super::Robot;
+use cmod::Result;
+use proto::google::protobuf::Empty;
+use proto::kinematic::KinData;
+use proto::lebai::db::LoadRequest;
+use proto::lebai::kinematic::*;
+use proto::posture::CartesianPose;
+
+impl Robot {
+    pub(crate) async fn load_tcp(&self, name: String, dir: Option<String>) -> Result<CartesianPose> {
+        let req = LoadRequest {
+            name,
+            dir: dir.unwrap_or_default(),
+        };
+        let resp = self.c.load_tcp(Some(req)).await.map_err(|e| e.to_string())?;
+        Ok(resp.into())
+    }
+    pub(crate) async fn set_tcp(&self, pose: CartesianPose) -> Result<()> {
+        let _ = self.c.set_tcp(Some(pose.into())).await.map_err(|e| e.to_string())?;
+        Ok(())
+    }
+    pub(crate) async fn get_tcp(&self) -> Result<CartesianPose> {
+        let resp = self.c.get_tcp(Some(Empty {})).await.map_err(|e| e.to_string())?;
+        Ok(resp.into())
+    }
+    pub(crate) async fn set_velocity_factor(&self, speed_factor: i32) -> Result<()> {
+        let req = KinFactor { speed_factor };
+        let _ = self.c.set_kin_factor(Some(req)).await.map_err(|e| e.to_string())?;
+        Ok(())
+    }
+    pub(crate) async fn get_velocity_factor(&self) -> Result<i32> {
+        let resp = self.c.get_kin_factor(Some(Empty {})).await.map_err(|e| e.to_string())?;
+        Ok(resp.speed_factor)
+    }
+    pub(crate) async fn get_kin_data(&self) -> Result<KinData> {
+        let resp = self.c.get_kin_data(Some(Empty {})).await.map_err(|e| e.to_string())?;
+        Ok(resp.into())
+    }
+}
diff --git a/src/rpc/mod.rs b/src/rpc/mod.rs
@@ -1,6 +1,7 @@
 pub mod claw;
 pub mod dynamic;
 pub mod io;
+pub mod kinematic;
 pub mod led;
 pub mod modbus;
 pub mod motion;