feat:add get pose rpc service

config/cabin_robot.xml

@@ -20,10 +20,10 @@
             </Camera>
 
             <DexHand>
-<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">-->
+<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
 <!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
 <!--                </RH56DFTP>-->
-<!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" port="6000">-->
+<!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
 <!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
 <!--                </RH56DFTP>-->
             </DexHand>
@@ -40,7 +40,7 @@
                           bufferSize="50"
                           verbose="false">
                     <CanManger id="" devId="">
-                        <LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
+                        <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
                             <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@@ -58,7 +58,7 @@
                             <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
                             <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
                         </RightArmCan>
-                        <HeadCan id = " " devId = " " channelId ="2" enable="true">
+                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
                             <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@@ -94,7 +94,7 @@
             </Microphone>
 
             <Speaker>
-<!--                <ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="500000" volume="80"/>-->
+                <ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="100"/>
             </Speaker>
 
             <Canbus>

config/robot_description/hc_description/dual_arm.urdf

@@ -574,7 +574,7 @@
   </link>
 
   <joint name="R_FINGER_TIP_FIXED" type="fixed">
-    <origin xyz="0 -0.270 -0.005" rpy="0 0 -1.5707963267"/>
+    <origin xyz="0.00684256  -0.284077 0.00801525" rpy="0 0 0"/>
     <parent link="R_WRIST_R_S"/>
     <child link="R_FINGER_TIP"/>
     <axis xyz="0 0 1"/>

example/robot_wrapper.cpp

@@ -85,6 +85,23 @@ public:
         return values;
     }
 
+    void move(const std::string &base_link, const std::string &ee_link,
+           double x, double y, double z, double rx, double ry, double rz,
+           double vel, double acc) {
+        checkRobotInit();
+
+        cmvr::msgs::Pose3d pose;
+        pose.mutable_position()->set_x(x);
+        pose.mutable_position()->set_y(y);
+        pose.mutable_position()->set_z(z);
+
+        pose.mutable_euler()->set_rx(rx);
+        pose.mutable_euler()->set_ry(ry);
+        pose.mutable_euler()->set_rz(rz);
+
+        robot_->moveJ(base_link, ee_link, pose,vel,acc);
+    }
+
     std::vector<double> ik(const std::string& side,
                            const std::string &base_link,
                            const std::string &ee_link,
@@ -117,6 +134,15 @@ public:
         }
     }
 
+
+    std::tuple<double, double, double, double, double, double> fk(const std::string &base_link, const std::string &ee_link) {
+        checkRobotInit();
+        auto pose = robot_->fk(base_link, ee_link);
+
+        // 返回一个元组，包含 (x, y, z, rx, ry, rz)
+        return std::make_tuple(pose.position().x(), pose.position().y(),  pose.position().z(),  pose.euler().rx(), pose.euler().ry(), pose.euler().ry());
+    }
+
 private:
     static DeviceManager* dmgr_;
     static std::shared_ptr<AbstractRobot> robot_;
@@ -166,5 +192,12 @@ PYBIND11_MODULE(robot_wrapper, m) {
              py::arg("side"), py::arg("base_link"), py::arg("ee_link"),
              py::arg("x"), py::arg("y"), py::arg("z"),
              py::arg("rx"), py::arg("ry"), py::arg("rz"),
-             "Compute inverse kinematics and return 7 joint values for the given side");
+             "Compute inverse kinematics and return 7 joint values for the given side")
+        .def("moveJ", &PyRobotWrapper::move, "Move robot in joint space",
+            py::arg("base_link"), py::arg("ee_link"),
+            py::arg("x"), py::arg("y"), py::arg("z"),
+            py::arg("rx"), py::arg("ry"), py::arg("rz"),
+            py::arg("vel") = 0.5, py::arg("acc") = 0.1)
+        .def("fk", &PyRobotWrapper::fk, "Compute forward kinematics",
+             py::arg("base_link"), py::arg("ee_link"));
 }

include/service/grpc_humanoid_robot_service.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "device_manager/device_manager.h"
-#include "cmvr/api/humanoid_robot.grpc.pb.h"
+#include "cmvr/api/humanoid_robot_service.grpc.pb.h"
 
 namespace cmvr {
     namespace service {
@@ -27,6 +27,7 @@ namespace cmvr {
             grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override;
 
             grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override;
+            grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override;
         private:
             device::DeviceManager& dmgr_;
         };

protos/cmvr/api/humanoid_robot_command.proto

@@ -54,6 +54,19 @@ message MoveJ{
 
 }
 
+message GetPose{
+  message Request{
+    CommandHeader.Request header = 1;
+    string base_link = 2;
+    string ee_link = 3;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+    Pose3D pose = 2;
+  }
+}
+
 message MoveL{
 
   message Request{

protos/cmvr/api/humanoid_robot_service.proto

@@ -13,4 +13,5 @@ service HumanoidRobotService{
   rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response);
   rpc speedL(SpeedL.Request) returns (SpeedL.Response);
   rpc getJointState(JointRequest) returns (JointResponse);
+  rpc getPose(GetPose.Request) returns (GetPose.Response);
 }

python/vision_servo/follow_target.py

@@ -326,170 +326,147 @@ def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target):
     return P_base_target
 
 
-if __name__ == "__main__":
-    pipeline, align = init_realsense()
-    try:
-        # 初始化机器人
-        robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
-
-        # 移动到初始关节位姿
-
-        init_joint = [-0.304106 , 1.30538 , 1.4465  ,1.93647 , -2.84955  ,-0.116586   ,0.123911]
-        # init_joint = [0 ,0 , 0  ,0 , 0 ,0   ,0]
-        robot.moveJ("right", init_joint)
-
-        js = robot.getJointQ('right')
-        print("当前关节角:", js)
-
-        # 获取末端在基座下的位姿
-        # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
-        T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
-        print("末端位姿 T_base_ee:\n", T_base_ee)
-        # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
-        # print("末端位姿 T_base_ee:\n", T_base_ee)
-        #
-        # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP")
-        # print("末端位姿 T_base_ee:\n", T_base_ee)
-
-        # while True:
-        #     time.sleep(1)
-
-
-
-        # 获取红点在相机坐标系下的坐标
-        P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png")
-        if P_cam_target is None:
-            raise RuntimeError("未检测到红点，无法计算目标点")
-        print("红点在相机坐标系下:", P_cam_target)
-
-        # P_cam_target = np.array([0.0716422, 0.0482324, 0.325])
-
-        # "--frame-id",
-        # "R_FINGER_TIP",
-        # "--child-frame-id",
-        # "camera_color_optical_frame",
-        # "--x",
-        # "-0.132714",
-        # "--y",
-        # "0.00161894",
-        # "--z",
-        # "0.0760576",
-        # "--qx",
-        # "0.512312",
-        # "--qy",
-        # "-0.503306",
-        # "--qz",
-        # "0.493563",
-        # "--qw",
-        # "-0.490525",
-        # # "--roll",
-        # # "0.18103",
-        # # "--pitch",
-        # # "1.60289",
-        # # "--yaw",
-        # # "-1.76387",
-
-        # 手眼标定结果：四元数和平移
-        # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596])
-        # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921])
-
-        q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525])
-        t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576])
-        R_ee_cam = R.from_quat(q_ee_cam).as_matrix()
-        print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam)
-
-
-        # 构造相机相对于 末端的变换矩阵
-        T_ee_cam = np.eye(4)
-        T_ee_cam[:3, :3] = R_ee_cam
-        T_ee_cam[:3, 3] = t_ee_cam
-
-        # 相机相对于 基座的 变换矩阵
-        T_base_cam = T_base_ee @ T_ee_cam
-
-        # 相机下红点 -> 基座下红点
-        P_base_target = np.append(P_cam_target, 1)  # 齐次坐标
-        P_base_target = (T_base_cam @ P_base_target)[:3]
-        print("红点在基座坐标系下:", P_base_target)
-
-
-        # while True:
-        #     time.sleep(1)
-
-        # ----------------- IK 求解 -----------------
-        # 目标位姿 为 相机 相对于 基座的 姿态
-        # R_base_target = T_base_ee[:3, :3] @ R_ee_cam
-        R_base_target = T_base_ee[:3, :3]
-        rot = R.from_matrix(R_base_target)
-        target_euler = rot.as_euler('xyz', degrees=True)
-        d = rot.as_euler('xyz', degrees=False)
-        target_rx, target_ry, target_rz = target_euler
-        quat = rot.as_quat()
-        print("目标旋转欧拉角 (XYZ 度):", target_euler)
-        print("目标旋转欧拉角 (XYZ 弧度):", d)
-        print("目标旋转四元数 [x,y,z,w]:", quat)
-
-        # while True:
-        #     time.sleep(10)
-
-        # IK 求解
-        ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz,
-                              base_link="PELVIS_S", target_link="R_FINGER_TIP")
-        if not ok:
-            raise RuntimeError("IK 求解失败")
-
-        # 移动到目标点
-        robot.moveJ("right", rq)
-        print("已到达目标点，按 Ctrl+C 回到初始位姿...")
-
-        while True:
-            time.sleep(1)
-
-    except Exception as e:
-        print("捕获异常:", e)
-
-    finally:
-        # 回到初始位姿
-        robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805,
-                              -1.498103103566999, 0.039690803641003906, -0.08653132466712826])
-        pipeline.stop()
-        align = None
-
-
 # if __name__ == "__main__":
-#
+#     pipeline, align = init_realsense()
 #     try:
 #         # 初始化机器人
 #         robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
+#
+#         # 移动到初始关节位姿
+#
+#         init_joint = [-0.304106 , 1.30538 , 1.4465  ,1.93647 , -2.84955  ,-0.116586   ,0.123911]
+#         # init_joint = [0 ,0 , 0  ,0 , 0 ,0   ,0]
+#         robot.moveJ("right", init_joint)
+#
 #         js = robot.getJointQ('right')
-#         # js = np.zeros(7)
+#         print("当前关节角:", js)
-#         # js[6] = 0.8
 #
-#         T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
+#         # 获取末端在基座下的位姿
-#         T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#         # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#         T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#         print("末端位姿 T_base_ee:\n", T_base_ee)
+#         # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#         # print("末端位姿 T_base_ee:\n", T_base_ee)
+#         #
+#         # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP")
+#         # print("末端位姿 T_base_ee:\n", T_base_ee)
 #
-#         print(T_base_cam - T_base_ee)
+#         # while True:
-#         T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S")
+#         #     time.sleep(1)
-#         print(T_ee1_ee2)
 #
 #
-#         T_ee2_ee1 = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP")
+#
-#         print(T_ee2_ee1)
+#         # 获取红点在相机坐标系下的坐标
+#         P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png")
+#         if P_cam_target is None:
+#             raise RuntimeError("未检测到红点，无法计算目标点")
+#         print("红点在相机坐标系下:", P_cam_target)
+#
+#         # P_cam_target = np.array([0.0716422, 0.0482324, 0.325])
+#
+#         # "--frame-id",
+#         # "R_FINGER_TIP",
+#         # "--child-frame-id",
+#         # "camera_color_optical_frame",
+#         # "--x",
+#         # "-0.132714",
+#         # "--y",
+#         # "0.00161894",
+#         # "--z",
+#         # "0.0760576",
+#         # "--qx",
+#         # "0.512312",
+#         # "--qy",
+#         # "-0.503306",
+#         # "--qz",
+#         # "0.493563",
+#         # "--qw",
+#         # "-0.490525",
+#         # # "--roll",
+#         # # "0.18103",
+#         # # "--pitch",
+#         # # "1.60289",
+#         # # "--yaw",
+#         # # "-1.76387",
+#
+#         # 手眼标定结果：四元数和平移
+#         # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596])
+#         # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921])
+#
+#         q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525])
+#         t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576])
+#         R_ee_cam = R.from_quat(q_ee_cam).as_matrix()
+#         print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam)
 #
 #
-#         # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
+#         # 构造相机相对于 末端的变换矩阵
+#         T_ee_cam = np.eye(4)
+#         T_ee_cam[:3, :3] = R_ee_cam
+#         T_ee_cam[:3, 3] = t_ee_cam
 #
-#         # robot.moveJ("right", [-0.344938,  0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
+#         # 相机相对于 基座的 变换矩阵
-#         # js = robot.getJointQ('right')
+#         T_base_cam = T_base_ee @ T_ee_cam
-#         # print("js = ")
+#
-#         # print(js)
+#         # 相机下红点 -> 基座下红点
+#         P_base_target = np.append(P_cam_target, 1)  # 齐次坐标
+#         P_base_target = (T_base_cam @ P_base_target)[:3]
+#         print("红点在基座坐标系下:", P_base_target)
 #
 #
+#         # while True:
+#         #     time.sleep(1)
+#
+#         # ----------------- IK 求解 -----------------
+#         # 目标位姿 为 相机 相对于 基座的 姿态
+#         # R_base_target = T_base_ee[:3, :3] @ R_ee_cam
+#         R_base_target = T_base_ee[:3, :3]
+#         rot = R.from_matrix(R_base_target)
+#         target_euler = rot.as_euler('xyz', degrees=True)
+#         d = rot.as_euler('xyz', degrees=False)
+#         target_rx, target_ry, target_rz = target_euler
+#         quat = rot.as_quat()
+#         print("目标旋转欧拉角 (XYZ 度):", target_euler)
+#         print("目标旋转欧拉角 (XYZ 弧度):", d)
+#         print("目标旋转四元数 [x,y,z,w]:", quat)
+#
+#         # while True:
+#         #     time.sleep(10)
+#
+#         # IK 求解
+#         ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz,
+#                               base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#         if not ok:
+#             raise RuntimeError("IK 求解失败")
+#
+#         # 移动到目标点
+#         robot.moveJ("right", rq)
 #         print("已到达目标点，按 Ctrl+C 回到初始位姿...")
+#
 #         while True:
 #             time.sleep(1)
-#     except SystemExit:
+#
-#         print("安全退出程序...")
+#     except Exception as e:
-#     #
+#         print("捕获异常:", e)
-#     # finally:
+#
-#     #     robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
+#     finally:
+#         # 回到初始位姿
+#         robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805,
+#                               -1.498103103566999, 0.039690803641003906, -0.08653132466712826])
+#         pipeline.stop()
+#         align = None
+
+
+if __name__ == "__main__":
+
+
+        # 初始化机器人
+        robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
+        # js = robot.getJointQ('right')
+        js = np.zeros(7)
+        js = np.array([-0.054383226063586795, 1.4797969033848364, -1.4685445467885516, -0.03790717127687202, -0.2797606954361809, 0.1916896945079446, -0.17680932146902423])
+        # js[6] = 0.8
+
+        T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
+        # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
+        print(T_base_ee)
+

python/vision_servo/robot_warpper_test.py

@@ -10,33 +10,38 @@ sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
 # 导入模块
 from robot_wrapper import Robot
 
+# R_WRIST_R_S
 # 初始化机器人（传入配置文件路径和机器人名称）
 robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
 
-
+# robot.torqueOn()
-
+#
-
+#
-
+# x,y,z,rx,ry,rz = robot.fk(base_link="PELVIS_S", ee_link="R_FINGER_TIP")
+# # robot->move(base_link="PELVIS_S", target_link="R_FINGER_TIP")
+#
+#
+# print(x, y, z, rx, ry, rz, sep=',')
 #
 #
 # while True:
 #     for j in JOINT_POSITIONS:
 #         robot.moveJ('right', j)
-# robot.torqueOn()
+
 # time.sleep(10)
 # js = robot.getJointQ('right')
 # print(js)
 # 控制左臂关节
-# robot.calibrateZeroQ("R_WRIST_Y")
+robot.calibrateZeroQ("R_WRIST_Y")
 # robot.calibrateZeroQ("R_WRIST_R")
-# time.sleep(3)
+time.sleep(3)
-robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
+# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
 # # robot.moveJ("right", [-0.344938,  0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
 # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
 # #
-time.sleep(10)
+# time.sleep(10)
 
-# # robot.torqueOff("R_WRIST_R")
+# robot.torqueOff("R_WRIST_R")
 # time.sleep(10)
 # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
 # time.sleep(20)

src/devices/motor/ti5_motor/ti5_motor.h

@@ -32,11 +32,12 @@ namespace  cmvr {
                 if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
                     auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
                     canopen_protocol->configPdo(node_id_);
-                    canopen_protocol->configProfile(node_id_,2000,2000,2000);
+                    canopen_protocol->configProfile(node_id_,4000,8000,8000);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
                     canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE);
                     canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06);
                     canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F);
+                    canopen_protocol->setLimitQd(node_id_,6.0);
                 }
             }
         };

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -1587,6 +1587,7 @@ cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const st
                 q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
                 q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
 
+        LOG(INFO) << "q_current: " << q;
         // 更新状态并计算前向运动学
         m_state_->SetQ(q);
         m_robot_->ComputeForwardKinematics(m_state_);

src/devices/robot/humanoid_robot/humanoid_robot_test.cpp

@@ -443,12 +443,14 @@ TEST(HumanoidRobotTest, MoveIKTest) {
     auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP");
 
     cmvr::msgs::Pose3d pose;
-    pose.mutable_position()->set_x(0.591101);
+    pose.mutable_position()->set_x(0.047436);
-    pose.mutable_position()->set_y(-0.159451);
+    pose.mutable_position()->set_y(-0.2884);
-    pose.mutable_position()->set_z(-0.206554);
+    pose.mutable_position()->set_z(-0.605183);
-    pose.mutable_euler()->set_rx(0.070589);
+    pose.mutable_euler()->set_rx(-1.191976);
-    pose.mutable_euler()->set_ry(0.117212);
+    pose.mutable_euler()->set_ry(1.397317);
-    pose.mutable_euler()->set_rz(0.0916384);
+    pose.mutable_euler()->set_rz(2.721208);
+    // Pose: {'x': 0.047436, 'y': -0.2884, 'z': -0.605183, 'rx': -1.191976, 'ry': 1.397317, 'rz': 2.721208}
+
 
     // 打印关节状态线程
     std::thread joint_state_thread([&]() {

src/service/grpc_humanoid_robot_service.cpp

@@ -153,6 +153,40 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context,
     return ret;
 }
 
+grpc::Status gRPCHumanoidRobotServiceImpl::getPose(grpc::ServerContext *context,
+    const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) {
+
+    grpc::Status ret = grpc::Status::OK;
+
+    try {
+        auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
+
+        std::vector<cmvr::device::JointState> states{};
+        auto pose = robot->fk(request->base_link(),request->ee_link());
+
+        response->mutable_header()->set_success(true);
+        response->mutable_header()->set_error_message("");
+        response->mutable_pose()->set_x(pose.position().x());
+        response->mutable_pose()->set_y(pose.position().y());
+        response->mutable_pose()->set_z(pose.position().z());
+        response->mutable_pose()->set_rx(pose.euler().rx());
+        response->mutable_pose()->set_ry(pose.euler().ry());
+        response->mutable_pose()->set_rz(pose.euler().rz());
+
+
+
+    }catch (const std::exception& e) {
+        response->mutable_header()->set_success(false);
+        response->mutable_header()->set_error_message(e.what());
+        ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+    *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
+    return ret;
+
+
+}
+
+
 grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context,
     const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {