fix: load_frame

lebai-robotics · Nov 24, 2023 · 54b5364 · 54b5364
1 parent b662a2a
commit 54b5364
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 22 deletions.
diff --git a/src/rpc/mod.rs b/src/rpc/mod.rs
@@ -4,8 +4,8 @@ pub mod io;
 pub mod kinematic;
 pub mod led;
 pub mod modbus;
-pub mod plugin;
 pub mod motion;
+pub mod plugin;
 pub mod posture;
 pub mod serial;
 pub mod signal;

diff --git a/src/rpc/plugin.rs b/src/rpc/plugin.rs
@@ -4,11 +4,7 @@ use proto::lebai::plugin::*;
 use proto::lebai::CommandStdout;
 
 impl Robot {
-    pub(crate) async fn run_plugin_cmd(
-        &self,
-        name: String,
-        params: Option<Vec<String>>,
-    ) -> Result<CommandStdout> {
+    pub(crate) async fn run_plugin_cmd(&self, name: String, params: Option<Vec<String>>) -> Result<CommandStdout> {
         let req = RunPluginCmdRequest {
             name,
             params: params.unwrap_or_default(),

diff --git a/src/rpc/posture.rs b/src/rpc/posture.rs
@@ -61,25 +61,24 @@ impl Robot {
         Ok(pose)
     }
     pub async fn load_frame(&self, name: String, dir: Option<String>) -> Result<CartesianPose> {
-        let req = LoadRequest {
+        let frame_index = LoadRequest {
             name,
             dir: dir.unwrap_or_default(),
         };
-        let pose = self.c.load_frame(Some(req)).await.map_err(|e| e.to_string())?;
-        let mut ret = CartesianPose::default();
-        match pose.rotation_kind() {
-            cartesian_frame::Kind::Base => {}
-            cartesian_frame::Kind::Flange => todo!(),
-            cartesian_frame::Kind::LastFlange => todo!(),
-            cartesian_frame::Kind::Tcp => todo!(),
-            cartesian_frame::Kind::LastTcp => todo!(),
-            cartesian_frame::Kind::Custom => {
-                let rot = pose.rotation.unwrap_or_default().euler_zyx.unwrap_or_default();
-                ret.rx = rot.x;
-                ret.ry = rot.y;
-                ret.rz = rot.z;
-            }
+        let req = PoseRequest {
+            pose: Some(posture::Pose {
+                kind: posture::pose::Kind::Cartesian as i32,
+                cart_frame_index: Some(frame_index),
+                ..Default::default()
+            }),
         };
-        Ok(ret)
+        let pose = self.c.get_forward_kin(Some(req)).await.map_err(|e| e.to_string())?;
+        let rot = pose.rotation.unwrap_or_default().euler_zyx.unwrap_or_default();
+        Ok(CartesianPose {
+            rx: rot.x,
+            ry: rot.y,
+            rz: rot.z,
+            ..Default::default()
+        })
     }
 }