feat: pose refer

Cargo.toml

@@ -5,7 +5,7 @@ edition = "2021"
 license-file = "LICENSE"
 name = "lebai_sdk"
 repository = "https://github.com/lebai-robotics/lebai-sdk.rs"
-version = "0.2.13"
+version = "0.2.14"
 
 [workspace]
 members = [

proto/Cargo.toml

@@ -6,7 +6,7 @@ keywords = ["protobuf"]
 license = "MIT"
 name = "lebai-proto"
 repository = "https://github.com/lebai-robotics/lebai-sdk.rs"
-version = "3.1.4"
+version = "3.1.5"
 
 [dependencies]
 jsonrpsee = {version = "0.21", features = ["client-core", "macros"]}

src/lib.rs

@@ -108,13 +108,13 @@ pub mod lebai_sdk {
             self.0.pose_inverse(p).await
         }
         #[classmethod]
-        #[cmod::tags(args(pose))]
-        pub async fn save_pose(&self, name: String, pose: Option<Pose>, dir: Option<String>) -> Result<()> {
-            self.0.save_pose(name, pose, dir).await
+        #[cmod::tags(args(pose, refer))]
+        pub async fn save_pose(&self, name: String, pose: Option<Pose>, dir: Option<String>, refer: Option<JointPose>) -> Result<()> {
+            self.0.save_pose(name, pose, dir,refer).await
         }
         #[classmethod]
         #[cmod::tags(ret)]
-        pub async fn load_pose(&self, name: String, dir: Option<String>) -> Result<Option<Pose>> {
+        pub async fn load_pose(&self, name: String, dir: Option<String>) -> Result<Option<JointPose>> {
             self.0.load_pose(name, dir).await
         }
         #[classmethod]

src/rpc/posture.rs

@@ -44,29 +44,35 @@ impl Robot {
         Ok(pose.into())
     }
 
-    pub async fn save_pose(&self, name: String, pose: Option<Pose>, dir: Option<String>) -> Result<()> {
+    pub async fn save_pose(&self, name: String, pose: Option<Pose>, dir: Option<String>, refer: Option<JointPose>) -> Result<()> {
+        let pose = pose.map(|x| {
+            let mut x: posture::Pose = x.into();
+            x.joint = refer.map(Into::into);
+            x
+        });
         let req = SavePoseRequest {
             name,
             dir: dir.unwrap_or_default(),
-            data: pose.map(Into::into),
+            data: pose,
         };
         self.c.save_pose(Some(req)).await.map_err(|e| e.to_string())?;
         Ok(())
     }
-    pub async fn load_pose(&self, name: String, dir: Option<String>) -> Result<Option<Pose>> {
+    pub async fn load_pose(&self, name: String, dir: Option<String>) -> Result<Option<JointPose>> {
         let req = LoadRequest {
             name,
             dir: dir.unwrap_or_default(),
         };
-        let pose = self.c.load_pose(Some(req)).await.map_err(|e| e.to_string())?;
+        let mut pose = self.c.load_pose(Some(req)).await.map_err(|e| e.to_string())?;
         let pose = match pose.kind() {
             pose::Kind::Unknown => None,
             pose::Kind::Cartesian => {
-                let req = PoseRequest { pose: Some(pose) };
-                let pose = self.c.get_forward_kin(Some(req)).await.map_err(|e| e.to_string())?;
-                Some(Pose::Cart(pose.into()))
+                let refer = pose.joint.take();
+                let req = GetInverseKinRequest { pose: Some(pose), refer };
+                let pose = self.c.get_inverse_kin(Some(req)).await.map_err(|e| e.to_string())?;
+                Some(pose.into())
             }
-            pose::Kind::Joint => Some(Pose::Joint(pose.joint.unwrap_or_default().into())),
+            pose::Kind::Joint => Some(pose.joint.unwrap_or_default().into()),
         };
         Ok(pose)
     }