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

update camera encoder

Browse Source
This commit is contained in:
linbo 2026-03-17 09:11:29 +08:00
parent 62a90c3e64
commit e537194897
12 changed files with 350 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cmvr-es/common/CMakeLists.txt
View File

@ -4,6 +4,7 @@
file(GLOB SRC
${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/RealSenseCapture.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp
${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp
@ -23,6 +24,10 @@ target_link_libraries(common PUBLIC
avutil
avcodec
swscale
realsense2
opencv_core
opencv_imgproc
opencv_videoio
)
add_library(cmvr_es::common ALIAS common)

74
cmvr-es/common/config/cabin_robot.xml
View File

@ -32,44 +32,44 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />-->
<!-- <Neck/>-->
<Humanoid id="hc01" dof="14"
urdf="/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
baseLink="PELVIS_S"
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
bufferSize="50"
verbose="false">
<CanManger id="" devId="">
<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"/>
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<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="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"/>
</HeadCan>
<WaistCan id = " " devId = " " channelId ="3" enable="false">
<Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</WaistCan>
</CanManger>
<!-- <Humanoid id="hc01" dof="14"-->
<!-- urdf="/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"-->
<!-- baseLink="PELVIS_S"-->
<!-- jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"-->
<!-- linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"-->
<!-- bufferSize="50"-->
<!-- verbose="false">-->
<!-- <CanManger id="" devId="">-->
<!-- <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"/>-->
<!-- <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- </LeftArmCan>-->
<!-- <RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">-->
<!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <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="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"/>-->
<!-- </HeadCan>-->
<!-- <WaistCan id = " " devId = " " channelId ="3" enable="false">-->
<!-- <Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- </WaistCan>-->
<!-- </CanManger>-->
</Humanoid>
<!-- </Humanoid>-->
</Robot>
<BioHead>

18
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -2,18 +2,18 @@
#include <unistd.h>
// static std::string basePath()
// {
// char* cwd = ::getcwd(nullptr, 0);
// if (!cwd) return {};
// std::string s(cwd);
// std::free(cwd);
// return s + "/config/";
// }
static std::string basePath()
{
return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
char* cwd = ::getcwd(nullptr, 0);
if (!cwd) return {};
std::string s(cwd);
std::free(cwd);
return s + "/config/";
}
// static std::string basePath()
// {
// return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
// }
DEFINE_string(pinocchio_qp_ik_solver_config_file,
basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt",

36
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -218,10 +218,10 @@ void RealsenseCamera::init() {
}
// 初始化深度图编码器（示例参数：640x480，15fps，H.265）
if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) {
LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!";
throw runtime_error("Failed to init depth encoder!");
}
// if (!initSingleEncoder(depthEncoder_, codec_, width_, height_, fps_)) {
// LOG(ERROR) << "[RealsenseCamera] (start): Failed to init depth encoder!";
// throw runtime_error("Failed to init depth encoder!");
// }
LOG(INFO) << "[RealsenseCamera] (init): realsense camera initialized at index " << serial_; // 记录初始化成功日志
}
catch (const exception &error){
@ -895,6 +895,23 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
} else if (codec->id == AV_CODEC_ID_HEVC) {
// 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
// H.265 的强制设置
// 使用 x265-params 字符串设置所有参数
char x265_params[256];
snprintf(x265_params, sizeof(x265_params),
"keyint=%d:" // 关键帧间隔
"min-keyint=%d:" // 最小关键帧间隔
"no-open-gop=1:" // 禁用开放GOP
"bframes=0:" // 禁用B帧
"log-level=none", // 日志级别
10, 10); // 设置keyint和min-keyint为10
av_opt_set(ctx->priv_data, "x265-params", x265_params, 0);
// 或者分开设置（如果支持）
av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
}
// 6. 设置像素格式（不变）
@ -938,6 +955,17 @@ bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& enco
return false;
}
// 添加调试信息，确认设置生效
std::cout << "Encoder settings:" << std::endl;
std::cout << " GOP size: " << ctx->gop_size << std::endl;
if (codec->id == AV_CODEC_ID_HEVC) {
char* params = nullptr;
if (av_opt_get(ctx->priv_data, "x265-params", 0, (uint8_t**)&params) >= 0) {
std::cout << " x265-params: " << params << std::endl;
av_free(params);
}
}
std::cout << "Successfully initialized " << codec_name << " encoder ( "
<< width << "x" << height << "@" << fps << "fps )" << std::endl;
return true;

3
cmvr-es/devices/robot/abstract_robot.h
View File

@ -170,6 +170,9 @@ namespace cmvr::device{
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
virtual bool getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose) = 0;
virtual bool fk(const std::vector<double>& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true) = 0;
protected:
int dof_{};
RobotState state_{};

7
cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h
View File

@ -33,6 +33,7 @@
#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
#include "planner/joint_space_planner/include/joint_space_planner.h"
#include "ik_solver/include/pinocchio_qp_ik_solver.h"
namespace cmvr::device{
@ -144,7 +145,8 @@ namespace cmvr::device{
std::vector<double>& time_points,
std::vector<double>& distance_ratios);
bool getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose);
bool fk(const std::vector<double>& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true);
private:
std::string id_;
int upd_freq_;
@ -201,7 +203,8 @@ namespace cmvr::device{
std::atomic<bool> is_left_arm_busy_{false}, is_right_arm_busy_{false};
std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
//矩阵转换
std::shared_ptr<PinocchioQpIKSolver> pinocchio_ik_{nullptr};
};

31
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
View File

@ -38,6 +38,10 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
pinocchio_ik_ = make_shared<PinocchioQpIKSolver>(cfg.getAttrString("urdf"),cfg.getAttrString("baseLink"),"R_WRIST_R_S");
pinocchio_ik_->init();
upd_timer_ = make_shared<FDTimer>();
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
[this] { update_state_(); });
@ -2101,6 +2105,33 @@ cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std:
return math_pose;
}
template<int DOF>
bool HumanoidRobot<DOF>::getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose)
{
auto q_map = getJointQ();
std::vector<double> joints_angle = {
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"]
};
bool is_tcp = ee_link == "R_FINGER_TIP";
pinocchio_ik_->fk(joints_angle, cur_pose, is_tcp);
return true;
}
template<int DOF>
bool HumanoidRobot<DOF>::fk(const std::vector<double>& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp)
{
pinocchio_ik_->fk(joints_angle, cur_pose, is_tcp);
return true;
}
template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>;
template class cmvr::device::HumanoidRobot<20>;

10
cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
View File

@ -6,7 +6,7 @@
#include "device_manager/include/device_manager.h"
#include "cmvr/api/humanoid_robot_service.grpc.pb.h"
#include "ik_solver/include/pinocchio_qp_ik_solver.h"
namespace cmvr {
namespace service {
class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service {
@ -34,8 +34,16 @@ namespace cmvr {
grpc::Status servoJ(grpc::ServerContext* context, const cmvr::api::ServoJ_Request* request, cmvr::api::ServoJ_Response* response) override;
grpc::Status getPoseMatrix(grpc::ServerContext* context, const cmvr::api::GetPoseMatrix_Request* request, cmvr::api::GetPoseMatrix_Response* response) override;
grpc::Status computeForwardKinematics(grpc::ServerContext* context, const cmvr::api::ComputeForwardKinematics_Request* request, cmvr::api::ComputeForwardKinematics_Response* response) override;
private:
device::DeviceManager& dmgr_;
//矩阵转换
std::shared_ptr<PinocchioQpIKSolver> pinocchio_ik_{nullptr};
};

26
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -87,7 +87,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
{
try {
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
// LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
cv::Mat image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
Rs2Intrinsics intrinsics = {0};
@ -95,9 +95,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx);
response->mutable_intrinsics()->set_cx(intrinsics.fx);
response->mutable_intrinsics()->set_cy(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
@ -146,9 +146,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx);
response->mutable_intrinsics()->set_cx(intrinsics.fx);
response->mutable_intrinsics()->set_cy(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
@ -200,9 +200,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fx);
response->mutable_intrinsics()->set_cx(intrinsics.fx);
response->mutable_intrinsics()->set_cy(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fy);
response->mutable_intrinsics()->set_cx(intrinsics.cx);
response->mutable_intrinsics()->set_cy(intrinsics.cy);
for (int i = 0; i < 5 ; i++) {
response->mutable_intrinsics()->add_coeffs(intrinsics.coeffs[i]);
}
@ -454,9 +454,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
response.mutable_color_frame()->set_height(frame_data.height);
response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
for (int i = 0; i < 5 ; i++) {
response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
}

162
cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
View File

@ -14,7 +14,14 @@ using namespace cmvr::api;
using google::protobuf::util::TimeUtil;
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance()){}
gRPCHumanoidRobotServiceImpl::gRPCHumanoidRobotServiceImpl():dmgr_(DeviceManager::getInstance())
{
pinocchio_ik_ = std::make_shared<PinocchioQpIKSolver>("/home/linbo/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
,"PELVIS_S"
,"R_WRIST_R_S");
pinocchio_ik_->init();
}
grpc::Status gRPCHumanoidRobotServiceImpl::torqueOff(grpc::ServerContext *context,
@ -270,3 +277,156 @@ grpc::Status gRPCHumanoidRobotServiceImpl::servoJ(grpc::ServerContext* context
return ret;
}
// 在 gRPC 服务实现文件中添加以下代码
grpc::Status gRPCHumanoidRobotServiceImpl::getPoseMatrix(grpc::ServerContext* context
, const cmvr::api::GetPoseMatrix_Request* request
, cmvr::api::GetPoseMatrix_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
// 获取机械臂设备
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
// 获取请求参数
std::string base_link = request->base_link();
std::string ee_link = request->ee_link();
// 调用机械臂的 getPoseMatrix 方法获取当前位姿矩阵
Eigen::Matrix4d cur_pose;
bool success = robot->getPoseMatrix(base_link, ee_link, cur_pose);
if (success) {
// 设置成功状态
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
// 填充4x4矩阵到响应
auto* matrix = response->mutable_matrix();
matrix->set_m00(cur_pose(0, 0));
matrix->set_m01(cur_pose(0, 1));
matrix->set_m02(cur_pose(0, 2));
matrix->set_m03(cur_pose(0, 3));
matrix->set_m10(cur_pose(1, 0));
matrix->set_m11(cur_pose(1, 1));
matrix->set_m12(cur_pose(1, 2));
matrix->set_m13(cur_pose(1, 3));
matrix->set_m20(cur_pose(2, 0));
matrix->set_m21(cur_pose(2, 1));
matrix->set_m22(cur_pose(2, 2));
matrix->set_m23(cur_pose(2, 3));
matrix->set_m30(cur_pose(3, 0));
matrix->set_m31(cur_pose(3, 1));
matrix->set_m32(cur_pose(3, 2));
matrix->set_m33(cur_pose(3, 3));
// 可选：记录日志
// LOG(INFO) << "getPoseMatrix success for device: " << request->header().device_id();
} else {
// 获取位姿失败
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("Failed to get pose matrix");
ret = grpc::Status(grpc::StatusCode::INTERNAL, "Failed to get pose matrix");
}
} 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::computeForwardKinematics(grpc::ServerContext* context
, const cmvr::api::ComputeForwardKinematics_Request* request
, cmvr::api::ComputeForwardKinematics_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
// // 获取机械臂设备
// auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
// 解析关节命令
std::vector<JointPoint> cmds{};
for (const auto& jc : request->joints()) {
cmds.emplace_back(jc.joint_name(), jc.rad(), jc.vel());
}
// 如果没有关节命令，返回错误
if (cmds.empty()) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("No joint commands provided");
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, "No joint commands provided");
}
// 获取请求参数
std::string base_link = request->base_link();
std::string ee_link = request->ee_link();
bool is_tcp = true; // 默认使用TCP
// 从关节命令中提取关节角度
std::vector<double> joints_angle;
for (const auto& cmd : cmds) {
joints_angle.push_back(cmd.rad); // 使用 rad 作为关节角度
}
// 调用机械臂的 fk 方法计算正运动学
Eigen::Matrix4d cur_pose;
bool success = pinocchio_ik_->fk(joints_angle, cur_pose, is_tcp);
if (success) {
// 设置成功状态
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
// 填充4x4矩阵到响应
auto* matrix = response->mutable_matrix();
matrix->set_m00(cur_pose(0, 0));
matrix->set_m01(cur_pose(0, 1));
matrix->set_m02(cur_pose(0, 2));
matrix->set_m03(cur_pose(0, 3));
matrix->set_m10(cur_pose(1, 0));
matrix->set_m11(cur_pose(1, 1));
matrix->set_m12(cur_pose(1, 2));
matrix->set_m13(cur_pose(1, 3));
matrix->set_m20(cur_pose(2, 0));
matrix->set_m21(cur_pose(2, 1));
matrix->set_m22(cur_pose(2, 2));
matrix->set_m23(cur_pose(2, 3));
matrix->set_m30(cur_pose(3, 0));
matrix->set_m31(cur_pose(3, 1));
matrix->set_m32(cur_pose(3, 2));
matrix->set_m33(cur_pose(3, 3));
// 可选：记录日志
// LOG(INFO) << "computeForwardKinematics success for device: " << request->header().device_id();
} else {
// 正运动学计算失败
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message("Forward kinematics calculation failed");
ret = grpc::Status(grpc::StatusCode::INTERNAL, "Forward kinematics calculation failed");
}
} 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;
}

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

@ -40,6 +40,17 @@ enum RobotJointIndexDirection{
ROTATE_Y = 9; // 绕Y轴旋转
ROTATE_Z = 10; // 绕Z轴旋转
}
// 4x4 齐次变换矩阵
message TransformMatrix4x4 {
double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4;
double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8;
double m20 = 9; double m21 = 10; double m22 = 11; double m23 = 12;
double m30 = 13; double m31 = 14; double m32 = 15; double m33 = 16;
}
message MoveJ{
message Request{
CommandHeader.Request header = 1;
@ -157,4 +168,35 @@ message ServoJ{
CommandHeader.Feedback header= 1;
}
}
// 获取当前位姿的4x4矩阵
message GetPoseMatrix {
message Request {
CommandHeader.Request header = 1;
string base_link = 2; // 基坐标系
string ee_link = 3; // 末端连杆名称
}
message Response {
CommandHeader.Feedback header = 1;
TransformMatrix4x4 matrix = 2; // 4x4 位姿矩阵
}
}
// 根据关节角度计算正运动学位姿矩阵
message ComputeForwardKinematics {
message Request {
CommandHeader.Request header = 1;
string base_link = 2; // 基坐标系
string ee_link = 3; // 末端连杆名称
repeated JointCmd joints = 4; // 关节角度
}
message Response {
CommandHeader.Feedback header = 1;
TransformMatrix4x4 matrix = 2; // 计算出的4x4位姿矩阵
}
}

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

@ -16,4 +16,7 @@ service HumanoidRobotService{
rpc getPose(GetPose.Request) returns (GetPose.Response);
rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);
rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response);
}