SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat:add get pose rpc service

Browse Source
This commit is contained in:
lgv 2025-11-03 11:44:52 +08:00
parent 057984f440
commit 7d03c0eeaf
13 changed files with 243 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
config/cabin_robot.xml
View File

@ -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>

2
config/robot_description/hc_description/dual_arm.urdf
View File

@ -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"/>

35
example/robot_wrapper.cpp
View File

@ -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"));
}

3
include/service/grpc_humanoid_robot_service.h
View File

@ -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_;
};

13
protos/cmvr/api/humanoid_robot_command.proto
View File

@ -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{

1
protos/cmvr/api/humanoid_robot_service.proto
View File

@ -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);
}

BIN
python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc
View File

Binary file not shown.

277
python/vision_servo/follow_target.py
View File

@ -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)
# # js[6] = 0.8
# print("当前关节角:", js)
#
# 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)
# T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S")
# print(T_ee1_ee2)
# # while True:
# # time.sleep(1)
#
#
# 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')
# # print("js = ")
# # print(js)
# # 相机相对于 基座的 变换矩阵
# 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 SystemExit:
# print("安全退出程序...")
# #
# # finally:
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
#
# 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__":
# 初始化机器人
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)

25
python/vision_servo/robot_warpper_test.py
View File

@ -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)
robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
time.sleep(3)
# 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)

3
src/devices/motor/ti5_motor/ti5_motor.h
View File

@ -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);
}
}
};

1
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -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_);

14
src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
View File

@ -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_y(-0.159451);
pose.mutable_position()->set_z(-0.206554);
pose.mutable_euler()->set_rx(0.070589);
pose.mutable_euler()->set_ry(0.117212);
pose.mutable_euler()->set_rz(0.0916384);
pose.mutable_position()->set_x(0.047436);
pose.mutable_position()->set_y(-0.2884);
pose.mutable_position()->set_z(-0.605183);
pose.mutable_euler()->set_rx(-1.191976);
pose.mutable_euler()->set_ry(1.397317);
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([&]() {

34
src/service/grpc_humanoid_robot_service.cpp
View File

@ -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) {