From f80641f5c739d875f4ae0bf733d5dcb9f2e834ac Mon Sep 17 00:00:00 2001 From: rise0chen Date: Wed, 7 Jun 2023 07:17:26 +0000 Subject: [PATCH] fix: kin_data --- proto/src/kinematic.rs | 40 ++++++++++++++++++++++++++++++++++++++++ proto/src/lib.rs | 1 + src/common.rs | 2 +- src/lib.rs | 10 +++++----- src/rpc/dynamic.rs | 31 ------------------------------- src/rpc/io.rs | 34 +++++++++++++++++----------------- src/rpc/kinematic.rs | 39 +++++++++++++++++++++++++++++++++++++++ src/rpc/mod.rs | 1 + 8 files changed, 104 insertions(+), 54 deletions(-) create mode 100644 proto/src/kinematic.rs create mode 100644 src/rpc/kinematic.rs diff --git a/proto/src/kinematic.rs b/proto/src/kinematic.rs new file mode 100644 index 0000000..f8500cd --- /dev/null +++ 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 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 index 67acf37..77d1dc1 100644 --- 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 index ed8d282..3129929 100644 --- a/src/common.rs +++ b/src/common.rs @@ -8,7 +8,7 @@ static TIMESTAMP: Lazy 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 { Ok(TIMESTAMP().as_millis() as u64) diff --git a/src/lib.rs b/src/lib.rs index 9d00fb8..cb9476f 100644 --- 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,7 +39,7 @@ 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] @@ -47,8 +47,8 @@ pub mod lebai_sdk { pub async fn discover_devices(time: u32) -> Result> { #[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 index 901e4b2..d4fa2f2 100644 --- 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) -> Result { - 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 { - 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 { - 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 { - 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) -> Result { let req = LoadRequest { name, diff --git a/src/rpc/io.rs b/src/rpc/io.rs index 83e9e15..c43b845 100644 --- a/src/rpc/io.rs +++ b/src/rpc/io.rs @@ -6,8 +6,8 @@ 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(()) @@ -15,7 +15,7 @@ impl Robot { pub(crate) async fn get_do(&self, device: String, pin: u32) -> Result { 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) @@ -23,8 +23,8 @@ impl Robot { pub(crate) async fn get_dos(&self, device: String, pin: u32, num: u32) -> Result> { 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) @@ -32,7 +32,7 @@ impl Robot { pub(crate) async fn get_di(&self, device: String, pin: u32) -> Result { 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) @@ -40,17 +40,17 @@ impl Robot { pub(crate) async fn get_dis(&self, device: String, pin: u32, num: u32) -> Result> { 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(()) @@ -58,7 +58,7 @@ impl Robot { pub(crate) async fn get_ao(&self, device: String, pin: u32) -> Result { 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) @@ -66,8 +66,8 @@ impl Robot { pub(crate) async fn get_aos(&self, device: String, pin: u32, num: u32) -> Result> { 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) @@ -75,7 +75,7 @@ impl Robot { pub(crate) async fn get_ai(&self, device: String, pin: u32) -> Result { 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) @@ -83,8 +83,8 @@ impl Robot { pub(crate) async fn get_ais(&self, device: String, pin: u32, num: u32) -> Result> { 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 new file mode 100644 index 0000000..3913fd9 --- /dev/null +++ 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) -> Result { + 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 { + 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 { + 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 { + 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 index 376b428..f6691d2 100644 --- 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;