From e537194897bc16aa2b52a0fba6b68fecaaa02b4f Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 17 Mar 2026 09:11:29 +0800 Subject: [PATCH] update camera encoder --- cmvr-es/common/CMakeLists.txt | 5 + cmvr-es/common/config/cabin_robot.xml | 74 ++++---- .../config_helper/src/config_setting.cpp | 18 +- .../realsense_camera/src/realsense_camera.cpp | 36 +++- cmvr-es/devices/robot/abstract_robot.h | 3 + .../humanoid_robot/include/humanoid_robot.h | 7 +- .../humanoid_robot/src/humanoid_robot.cpp | 31 ++++ .../include/grpc_humanoid_robot_service.h | 10 +- .../service/grpc/src/grpc_camera_service.cpp | 26 +-- .../grpc/src/grpc_humanoid_robot_service.cpp | 162 +++++++++++++++++- protos/cmvr/api/humanoid_robot_command.proto | 42 +++++ protos/cmvr/api/humanoid_robot_service.proto | 3 + 12 files changed, 350 insertions(+), 67 deletions(-) diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 1f4f1267..5e4dca8f 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -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) diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index fa833a13..ecc9ae62 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -32,44 +32,44 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 2e32d6ed..187dbca8 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -2,18 +2,18 @@ #include -// 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", diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 43aa315d..55b5e3e9 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -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& 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& 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**)¶ms) >= 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; diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 50315346..c963ae0d 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -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 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& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true) = 0; protected: int dof_{}; RobotState state_{}; diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 740176e7..634eb6a2 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -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& time_points, std::vector& distance_ratios); - + bool getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose); + bool fk(const std::vector& 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 is_left_arm_busy_{false}, is_right_arm_busy_{false}; std::atomic is_head_busy_{false}, is_waist_busy_{false}; - + //矩阵转换 + std::shared_ptr pinocchio_ik_{nullptr}; }; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 081f4702..23c18a3e 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -38,6 +38,10 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { CSC_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); + pinocchio_ik_ = make_shared(cfg.getAttrString("urdf"),cfg.getAttrString("baseLink"),"R_WRIST_R_S"); + pinocchio_ik_->init(); + + upd_timer_ = make_shared(); upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), [this] { update_state_(); }); @@ -2101,6 +2105,33 @@ cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std: return math_pose; } +template +bool HumanoidRobot::getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose) +{ + auto q_map = getJointQ(); + std::vector 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 +bool HumanoidRobot::fk(const std::vector& 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>; diff --git a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h index 2b631e57..b9f0b9f9 100644 --- a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h +++ b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h @@ -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 pinocchio_ik_{nullptr}; }; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index e039b168..fb033cc0 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -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(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]); } diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index 4be0d3b9..eb8a45de 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -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("/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(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(request->header().device_id()); + + // 解析关节命令 + std::vector 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 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; +} \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto index 13273a10..75804fb1 100644 --- a/protos/cmvr/api/humanoid_robot_command.proto +++ b/protos/cmvr/api/humanoid_robot_command.proto @@ -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位姿矩阵 + } } \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto index a4d52f50..b046eb92 100644 --- a/protos/cmvr/api/humanoid_robot_service.proto +++ b/protos/cmvr/api/humanoid_robot_service.proto @@ -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); }