diff --git a/README.md b/README.md index e553315a..e1bcdd0e 100644 --- a/README.md +++ b/README.md @@ -95,14 +95,15 @@ sudo make install ``` -### 5. librealsense2 install +### 5. librealsense2 2.55.1 install ```shell sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u -sudo apt-get install librealsense2-dkms -y -sudo apt-get install librealsense2-utils -y -sudo apt-get install librealsense2-dev -y -sudo apt-get install librealsense2-dbg -y +sudo apt-get install librealsense2-dkms=2.55.1-0~realsense.12473 -y +sudo apt-get install librealsense2-utils=2.55.1-0~realsense.12473 -y +sudo apt-get install librealsense2-dev=2.55.1-0~realsense.12473 -y +sudo apt-get install librealsense2-dbg=2.55.1-0~realsense.12473 -y + ``` ### 5. OSQPEigen install(build from source) diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index f7223171..62a0adf0 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -14,7 +14,7 @@ - + @@ -89,7 +89,7 @@ - + diff --git a/include/controller/touch_controller.h b/include/controller/touch_controller.h index 1d0c195f..9fa8dbb9 100644 --- a/include/controller/touch_controller.h +++ b/include/controller/touch_controller.h @@ -9,6 +9,8 @@ #include "devices/abstract_dexhand.h" #include "devices/abstract_robot.h" #include "cmvr/msgs/geometry.pb.h" +#include "librealsense2/rs.h" +#include "librealsense2/h/rs_frame.h" namespace cmvr { namespace ctrl { @@ -80,9 +82,14 @@ namespace cmvr { TouchController(){}; TouchController( std::shared_ptr robot,std::shared_ptr hand) :robot_(std::move(robot)),hand_(std::move(hand)), - pid_(std::make_shared(0.05, 0.001, 0.01, 50.0, 0.5, 1.0)){} + pid_(std::make_shared(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){} ~TouchController()=default; + + bool isArrive(double max_force); + + void touch(int u,int v ,double max_force){} + void touch(std::shared_ptr robot,const msgs::Pose3d pose,const msgs::Pose3d offset); void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); @@ -98,6 +105,8 @@ namespace cmvr { // 从压阻矩阵提取触控点与压力 bool extractTouch(const std::vector>& matrix,double& force, int& x, int& y); + + }; } } diff --git a/include/devices/abstract_camera.h b/include/devices/abstract_camera.h index 74770ef5..64447f24 100644 --- a/include/devices/abstract_camera.h +++ b/include/devices/abstract_camera.h @@ -4,6 +4,7 @@ #include #include "abstract_device.h" +#include namespace cmvr::device { struct Rs2Intrinsics @@ -58,6 +59,7 @@ namespace cmvr::device { virtual bool startStreaming() {return true;} virtual void stopStreaming() {} + virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};} protected: CameraState state_{}; void clear_error_() { diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index c160db13..4f6e89ed 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -93,6 +93,9 @@ namespace cmvr::device{ virtual void servoJ(std::vector &joints, double dt) { throw std::runtime_error("Not implemented"); } virtual void servoJ(std::vector &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); } + virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); } + virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); } + virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); } diff --git a/include/service/grpc_touch_service.h b/include/service/grpc_hlc_service.h similarity index 65% rename from include/service/grpc_touch_service.h rename to include/service/grpc_hlc_service.h index 388ec2dd..ecdd5c11 100644 --- a/include/service/grpc_touch_service.h +++ b/include/service/grpc_hlc_service.h @@ -4,14 +4,14 @@ #pragma once #include "device_manager/device_manager.h" -#include "cmvr/api/touch_service.grpc.pb.h" +#include "cmvr/api/hlc_service.grpc.pb.h" namespace cmvr { namespace service { - class gRPCTouchServiceImpl final : public api::TouchService::Service { + class gRPCHlcServiceImpl final : public api::HlcService::Service { public: - gRPCTouchServiceImpl(); - ~gRPCTouchServiceImpl() = default; + gRPCHlcServiceImpl(); + ~gRPCHlcServiceImpl() = default; grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; private: diff --git a/protos/cmvr/api/touch_service.proto b/protos/cmvr/api/hlc_command.proto similarity index 54% rename from protos/cmvr/api/touch_service.proto rename to protos/cmvr/api/hlc_command.proto index 3b0d6d1c..7c39ced9 100644 --- a/protos/cmvr/api/touch_service.proto +++ b/protos/cmvr/api/hlc_command.proto @@ -1,15 +1,19 @@ syntax = "proto3"; -package cmvr.api; -import "cmvr/api/common.proto"; -import "cmvr/msgs/geometry.proto"; +import "cmvr/api/common.proto"; + +package cmvr.api; message Touch{ message Request{ CommandHeader.Request header = 1; - cmvr.msgs.Pose3d pose = 2; - cmvr.msgs.Pose3d offset = 3; + // 触点坐标，unit: ms + int32 u = 2; // 水平坐标 + int32 v = 3; // 垂直坐标 + + // 触控时允许的最大作用力，unit: N + double max_force = 4; } message Response{ @@ -18,6 +22,3 @@ message Touch{ } -service TouchService{ - rpc touch(Touch.Request) returns (Touch.Response); -} \ No newline at end of file diff --git a/protos/cmvr/api/hlc_service.proto b/protos/cmvr/api/hlc_service.proto new file mode 100644 index 00000000..7e14032c --- /dev/null +++ b/protos/cmvr/api/hlc_service.proto @@ -0,0 +1,9 @@ +syntax = "proto3"; + +import "cmvr/api/hlc_command.proto"; + +package cmvr.api; + +service HlcService{ + rpc touch(Touch.Request) returns (Touch.Response); +} \ No newline at end of file diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index 6797c954..ad888b82 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -45,12 +45,12 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # robot.moveJ('right', j) robot.torqueOn() # 控制左臂关节 -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) -robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# time.sleep(10) +# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) # robot.torqueOff() # // {"R_SHOULDER_P", 0.00203898}, diff --git a/src/controller/touch_controller.cpp b/src/controller/touch_controller.cpp index 28dd5cc6..504200eb 100644 --- a/src/controller/touch_controller.cpp +++ b/src/controller/touch_controller.cpp @@ -91,4 +91,24 @@ bool TouchController::extractTouch(const std::vector > &ma } +bool TouchController::isArrive(double max_force) { + const auto& hand_data = hand_->getSensorData(); + double force = 0.0; + int x = 0, y = 0; + extractTouch(hand_data.index.tip.data, force, x, y); + + LOG(INFO) << "Force : " << force ; + + // 输出 3x3 数组 + LOG(INFO) << "Tip data (3x3):"; + for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) { + std::stringstream ss; + for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) { + ss << hand_data.index.tip.data[i][j] << "\t"; + } + LOG(INFO) << ss.str(); + } + + return force > max_force; +} diff --git a/src/controller/touch_controller_test.cpp b/src/controller/touch_controller_test.cpp index 02232431..f00f6e0f 100644 --- a/src/controller/touch_controller_test.cpp +++ b/src/controller/touch_controller_test.cpp @@ -39,15 +39,35 @@ TEST(TouchControllerTest,MyTest) { pose.mutable_euler()->set_rz(1.57); robot->moveJ("PELVIS_S","R_WRIST_R_S",pose); + // robot->seJ("PELVIS_S","R_WRIST_R_S",pose); + // robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5); + + cmvr::msgs::Pose3d delta_pose; + delta_pose.mutable_position()->set_x( 0.01); + delta_pose.mutable_position()->set_y(0); + delta_pose.mutable_position()->set_z(0); + + delta_pose.mutable_euler()->set_rx(0); + delta_pose.mutable_euler()->set_ry(0); + delta_pose.mutable_euler()->set_rz(0); + + + + + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02); Pose3d offset; TouchController controller(robot, hand); while (true) { - controller.touch(pose,offset,500); + // controller.touch(pose,offset,500); + if (controller.isArrive(500)) { + delta_pose.mutable_position()->set_x( -0.01); + robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05); + } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - } \ No newline at end of file diff --git a/src/devices/camera/realsense_camera/realsense_camera.cpp b/src/devices/camera/realsense_camera/realsense_camera.cpp index 079f5a20..b833bb79 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.cpp +++ b/src/devices/camera/realsense_camera/realsense_camera.cpp @@ -244,13 +244,28 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { auto frame = stream_frame_buffer_->pop(getImageIndex_); if (frame.has_value()) { color = frame.value().rgbImage.clone(); + { + std::lock_guard lock(frame_mtx_); + latest_depth_ = frame.value().depthImage.clone(); + } } else { rs2::frameset frames = get_frameset(false); auto color_frame = frames.get_color_frame(); - color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), CV_8UC3, const_cast(color_frame.get_data()), cv::Mat::AUTO_STEP).clone(); + auto depth_frame = frames.get_depth_frame(); + color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), + CV_8UC3, const_cast(color_frame.get_data()), + cv::Mat::AUTO_STEP).clone(); + + if (depth_frame) { + cv::Mat depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()), + CV_16UC1, const_cast(depth_frame.get_data()), + cv::Mat::AUTO_STEP).clone(); + std::lock_guard lock(frame_mtx_); + latest_depth_ = depth; + } } LOG(INFO) << "realsense get rgb frame successfully"; } @@ -260,7 +275,6 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) { } void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { - intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -272,8 +286,7 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { if (frame.has_value()) { depth = frame.value().depthImage.clone(); } - else - { + else { rs2::frameset frames; try { // 获取帧（带同步） @@ -282,23 +295,24 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { throw std::runtime_error("[RealsenseCamera] (getDepthImage): Failed to get frames: " + std::string(e.what())); } - // 提取深度帧 rs2::depth_frame depth_frame = frames.get_depth_frame(); if (!depth_frame) { throw std::runtime_error("[RealsenseCamera] (getDepthImage): No depth frame available"); } - // 创建深度图像（16位无符号整数，单位：毫米） - depth = cv::Mat(cv::Size(640, 480), CV_16UC1, const_cast(depth_frame.get_data()), cv::Mat::AUTO_STEP).clone(); + depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()), + CV_16UC1, const_cast(depth_frame.get_data()), + cv::Mat::AUTO_STEP).clone(); + } - // 为避免内存问题，创建深拷贝 - cv::Mat temp = depth.clone(); - depth = temp; + // 保存 latest_depth_ + { + std::lock_guard lock(frame_mtx_); + latest_depth_ = depth.clone(); } } void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { - intrinsics.cx = intrinsics_.ppx; intrinsics.cy = intrinsics_.ppy; intrinsics.fx = intrinsics_.fx; @@ -311,44 +325,39 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic color = frame.value().rgbImage.clone(); depth = frame.value().depthImage.clone(); } - else - { + else { rs2::frameset frames; try { - // 获取对齐的帧集合 frames = get_frameset(true); } catch (const std::exception& e) { throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what())); } - // 提取彩色帧 - rs2::frame color_frame = frames.get_color_frame(); - if (!color_frame) { - throw std::runtime_error("[RealsenseCamera] (getRGBDImages): No color frame available"); - } + // 对齐深度到彩色坐标系 + rs2::align align_to(RS2_STREAM_COLOR); + frames = align_to.process(frames); - // 提取深度帧 + rs2::video_frame color_frame = frames.get_color_frame(); + // rs2::frame color_frame = frames.get_color_frame(); rs2::depth_frame depth_frame = frames.get_depth_frame(); - if (!depth_frame) { - throw std::runtime_error("[RealsenseCamera] (getRGBDImages): No depth frame available"); + if (!color_frame || !depth_frame) { + throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Missing frames"); } - // 创建彩色图像（BGR格式） - color = cv::Mat(cv::Size(640, 480), CV_8UC3, const_cast(color_frame.get_data())); - - // 创建深度图像（16位无符号整数，单位：毫米） - depth = cv::Mat(cv::Size(640, 480), CV_16UC1, const_cast(depth_frame.get_data())); - - // 为避免内存问题，创建深拷贝 - cv::Mat color_temp = color.clone(); - cv::Mat depth_temp = depth.clone(); - color = color_temp; - depth = depth_temp; + color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), + CV_8UC3, const_cast(color_frame.get_data())).clone(); + depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()), + CV_16UC1, const_cast(depth_frame.get_data())).clone(); } - + // 保存 latest_depth_ + { + std::lock_guard lock(frame_mtx_); + latest_depth_ = depth.clone(); + } } + void RealsenseCamera::updateParams(const std::pair& param) { std::lock_guard lock(ctrl_mtx_); try { @@ -1024,3 +1033,22 @@ void RealsenseCamera::stopStreaming() state_.is_streaming = false; } } + + + +Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) { + std::lock_guard lock(frame_mtx_); + + if (latest_depth_.empty()) return {0,0,0}; + + // 获取深度值，单位米 + float depth_m = latest_depth_.at(v, u) * 0.001f; + if (depth_m <= 0) return {0,0,0}; + + float pixel[2] = { static_cast(u), static_cast(v) }; + float point[3]; + rs2_deproject_pixel_to_point(point, &intrinsics_, pixel, depth_m); + + return { point[0], point[1], point[2] }; +} + diff --git a/src/devices/camera/realsense_camera/realsense_camera.h b/src/devices/camera/realsense_camera/realsense_camera.h index 4a56fec4..39b20261 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.h +++ b/src/devices/camera/realsense_camera/realsense_camera.h @@ -39,6 +39,9 @@ namespace cmvr::device{ bool startStreaming() override; void stopStreaming() override; + + + Eigen::Vector3f get3DPointFromPixel(int u, int v) override; private: rs2::frameset get_frameset(bool align); void streaming_worker_(); @@ -100,6 +103,9 @@ namespace cmvr::device{ bool is_streaming_running = false; bool is_recording_running = false; int stream_count_ = 0; + + cv::Mat latest_depth_; + std::mutex depth_mtx_; }; } diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index ace42775..b579d94e 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -224,6 +224,30 @@ template HumanoidRobot::~HumanoidRobot() { // TODO: close can interfaces upd_timer_->stop(); + + std::vector cmd = { + {"L_SHOULDER_P", 0.0}, + {"L_SHOULDER_R", -1.31873}, + {"L_SHOULDER_Y", 0.0}, + {"L_ELBOW_R", -0.537621}, + {"L_WRIST_P", 0.0}, + {"L_WRIST_Y", 0.000183204}, + {"L_WRIST_R", 0.0225797}, + + {"R_SHOULDER_P", -0.0201069}, + {"R_SHOULDER_R", 1.46698}, + {"R_SHOULDER_Y", 1.45894}, + {"R_ELBOW_R", 0.159681}, + {"R_WRIST_P", 0.0808349}, + {"R_WRIST_Y", -0.138279}, + {"R_WRIST_R", -0.243169}, + + {"WAIST_Y", 0}, + {"WAIST_P", 0} + }; + + this->moveJ(cmd,0.8); + this->torqueOff(); } template @@ -659,22 +683,95 @@ void HumanoidRobot::servoJ(std::vector &joints, double dt) { template void HumanoidRobot::servoJ(std::vector &joints, double vel, double dt) { - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { - rsm_.store(ROBOT_RUNNING); - for (const auto &j: joints) { - auto motor = motor_manager_->getMotor(j.joint_name); - if (motor != nullptr) { - motor->setQd(vel); - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - motor->setQ(j.rad); + for (const auto &j: joints) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } + motor->setQd(vel); + motor->setQ(j.rad); } - rsm_.store(ROBOT_READY); } } +template +void HumanoidRobot::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, + double acc) { + try { + // update m_state_ + Eigen::Vector q_init; + auto q_map = getJointQ(); + q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], + q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], + 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_init: " << q_init; + m_state_->SetQ(q_init); + m_robot_->ComputeForwardKinematics(m_state_); + + + Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); + T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); + // 输入为弧度 + T_target(0, 3) = pose.position().x(); + T_target(1, 3) = pose.position().y(); + T_target(2, 3) = pose.position().z(); + + cmvr::ctrl::PoseTarget target; + target.T_target = T_target; + target.w_posrot = 0.5; + target.weight = 1.0; + target.link_name = ee_link; + + // slove ik + Eigen::Vector q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, + ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); + if (!ok) { + throw runtime_error("solve IK failed"); + } + std::vector joint_points{ + {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, + {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, + {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, + {"R_WRIST_R", q_cmd[13]} + }; + servoJ(joint_points, vel, 0.1); + } catch (exception &e) { + throw runtime_error(e.what()); + } +} + + + +template +void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) { + try { + // 1 : 计算当前位姿 + auto cur_pose = fk(base_link, ee_link); + + // 2 : 计算目标角度 target pos = cur_pose + delta_pose + cmvr::msgs::Pose3d target_pose; + target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x()); + target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y()); + target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z()); + + target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx()); + target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry()); + target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz()); + + //3 : + servoJ(base_link, ee_link, target_pose, vel, acc); + } catch (exception &e) { + throw runtime_error(e.what()); + } +} + + template void HumanoidRobot::servoL(std::string &base_link, std::vector &targets, double dt) { diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 64fb8248..02315e22 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -59,7 +59,7 @@ namespace cmvr::device{ /* robot basic command*/ void eStop() override; - void moveJ(std::vector &cmd, double vel, double acc) override; + void moveJ(std::vector &cmd, double vel = 0.5, double acc = 0.1) override; void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override; void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, double acc) override ; void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; @@ -72,6 +72,8 @@ namespace cmvr::device{ void servoJ(std::vector &joints, double dt) override; void servoJ(std::vector &joints, double vel, double dt) override; + void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) override; + void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; void servoL(std::string &base_link, std::vector &targets, double dt) override; protected: diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index fcf32beb..a278a90b 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -282,50 +282,50 @@ TEST(HumanoidRobotTest,FollowJointTrajectoryTest) { // {"R_WRIST_R", 0.280016} // }; - - std::vector cmd1{ - {"L_SHOULDER_P", -0.747573}, - {"L_SHOULDER_R", -1.26911}, - {"L_SHOULDER_Y", -1.20811}, - {"L_ELBOW_R", -1.51221}, - {"L_WRIST_P", 2.64099}, - {"L_WRIST_Y", 0.417608}, - {"L_WRIST_R", -0.518287}, - - {"R_SHOULDER_P", -0.956276}, - {"R_SHOULDER_R", 1.0244}, - {"R_SHOULDER_Y", 2.70621}, - {"R_ELBOW_R", 2.02276}, - {"R_WRIST_P", -1.99653}, - {"R_WRIST_Y", 0.68523}, - {"R_WRIST_R", 0.477066} - }; - - std::vector cmd2{ - {"L_SHOULDER_P", -0.747573}, - {"L_SHOULDER_R", -1.26911}, - {"L_SHOULDER_Y", -1.20811}, - {"L_ELBOW_R", -1.51221}, - {"L_WRIST_P", 2.64099}, - {"L_WRIST_Y", 0.417608}, - {"L_WRIST_R", -0.518287}, - - {"R_SHOULDER_P", 0.226871}, - {"R_SHOULDER_R", 0.624717}, - {"R_SHOULDER_Y", 1.15086}, - {"R_ELBOW_R", 1.30365}, - {"R_WRIST_P", -2.14683}, - {"R_WRIST_Y", 0.196003}, - {"R_WRIST_R", 0.106678} - }; - - - - while (true) { - robot->moveJ(cmd1,0.8); - robot->moveJ(cmd2,0.8); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } + // + // std::vector cmd1{ + // {"L_SHOULDER_P", -0.747573}, + // {"L_SHOULDER_R", -1.26911}, + // {"L_SHOULDER_Y", -1.20811}, + // {"L_ELBOW_R", -1.51221}, + // {"L_WRIST_P", 2.64099}, + // {"L_WRIST_Y", 0.417608}, + // {"L_WRIST_R", -0.518287}, + // + // {"R_SHOULDER_P", -0.956276}, + // {"R_SHOULDER_R", 1.0244}, + // {"R_SHOULDER_Y", 2.70621}, + // {"R_ELBOW_R", 2.02276}, + // {"R_WRIST_P", -1.99653}, + // {"R_WRIST_Y", 0.68523}, + // {"R_WRIST_R", 0.477066} + // }; + // + // std::vector cmd2{ + // {"L_SHOULDER_P", -0.747573}, + // {"L_SHOULDER_R", -1.26911}, + // {"L_SHOULDER_Y", -1.20811}, + // {"L_ELBOW_R", -1.51221}, + // {"L_WRIST_P", 2.64099}, + // {"L_WRIST_Y", 0.417608}, + // {"L_WRIST_R", -0.518287}, + // + // {"R_SHOULDER_P", 0.226871}, + // {"R_SHOULDER_R", 0.624717}, + // {"R_SHOULDER_Y", 1.15086}, + // {"R_ELBOW_R", 1.30365}, + // {"R_WRIST_P", -2.14683}, + // {"R_WRIST_Y", 0.196003}, + // {"R_WRIST_R", 0.106678} + // }; + // + // + // + // while (true) { + // robot->moveJ(cmd1,0.8); + // robot->moveJ(cmd2,0.8); + // std::this_thread::sleep_for(std::chrono::milliseconds(500)); + // } } @@ -345,16 +345,20 @@ TEST(HumanoidRobotTest,MoveDeltaTest) { cmvr::msgs::Pose3d pose; pose.mutable_position()->set_x( 0); pose.mutable_position()->set_y(0); - pose.mutable_position()->set_z(0.03); + pose.mutable_position()->set_z(0.01); pose.mutable_euler()->set_rx(0); pose.mutable_euler()->set_ry(0); pose.mutable_euler()->set_rz(0); - robot->moveDeltaJ("PELVIS_S","R_WRIST_R_S",pose); - - - + try { + robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1); + // robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01); + // robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose); + }catch (std::exception &e) { + robot->torqueOff(); + LOG(INFO) << e.what(); + } } TEST(HumanoidRobotTest,AngleToTest) { diff --git a/src/main.cpp b/src/main.cpp index 01297461..4aff85a2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,7 +17,7 @@ #include "utils/base/logger.h" #include "service/grpc_head_service.h" #include "service/grpc_humanoid_robot_service.h" -#include "service/grpc_touch_service.h" +#include "service/grpc_hlc_service.h" #include "http/httpclient.h" #include "json/json.h" @@ -49,7 +49,7 @@ void runServer(const XmlNode &cfg){ auto dexhand_service = gRPCDexHandServiceImpl(); auto biohand_service = gRPCMBioHeadServiceImpl(); auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl(); - auto touch_service = gRPCTouchServiceImpl(); + auto hlc_service = gRPCHlcServiceImpl(); grpc::ServerBuilder builder; builder.AddListeningPort(address, grpc::InsecureServerCredentials()); @@ -60,7 +60,7 @@ void runServer(const XmlNode &cfg){ builder.RegisterService(&dexhand_service); builder.RegisterService(&biohand_service); builder.RegisterService(&humanoid_robot_service); - builder.RegisterService(&touch_service); + builder.RegisterService(&hlc_service); // 🔥 关键！启用反射 //grpc::reflection::InitProtoReflectionServerBuilderPlugin(); diff --git a/src/service/CMakeLists.txt b/src/service/CMakeLists.txt index 28befede..f0569b9c 100644 --- a/src/service/CMakeLists.txt +++ b/src/service/CMakeLists.txt @@ -6,7 +6,7 @@ add_library(service grpc_head_service.cpp grpc_dexhand_service.cpp grpc_humanoid_robot_service.cpp - grpc_touch_service.cpp + grpc_hlc_service.cpp ) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/service/grpc_touch_client_test.cpp b/src/service/grpc_hlc_client_test.cpp similarity index 57% rename from src/service/grpc_touch_client_test.cpp rename to src/service/grpc_hlc_client_test.cpp index 6d47a0b2..4fb32bd6 100644 --- a/src/service/grpc_touch_client_test.cpp +++ b/src/service/grpc_hlc_client_test.cpp @@ -4,17 +4,17 @@ #include "gtest/gtest.h" #include #include -#include "service/grpc_touch_service.h" +#include "service/grpc_hlc_service.h" #include "google/protobuf/timestamp.pb.h" #include #include using namespace cmvr::api; -TEST(GrpcTouchClientTest, MyTest) { +TEST(GrpcHlcClientTest, MyTest) { // 连接服务端 auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::TouchService::NewStub(channel); + auto stub = cmvr::api::HlcService::NewStub(channel); grpc::ClientContext context; @@ -25,23 +25,11 @@ TEST(GrpcTouchClientTest, MyTest) { request.mutable_header()->set_device_id("hc01"); *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - // 目标位姿 - request.mutable_pose()->mutable_position()->set_x(0.15); - request.mutable_pose()->mutable_position()->set_y(-0.4); - request.mutable_pose()->mutable_position()->set_z(0); - request.mutable_pose()->mutable_euler()->set_rx(0); - request.mutable_pose()->mutable_euler()->set_ry(0); - request.mutable_pose()->mutable_euler()->set_rz(1.57); + request.set_u(12); + request.set_v(13); + request.set_max_force(1300); - // 偏移位姿 - request.mutable_offset()->mutable_position()->set_x(0); - request.mutable_offset()->mutable_position()->set_y(0); - request.mutable_offset()->mutable_position()->set_z(0); - - request.mutable_offset()->mutable_euler()->set_rx(0); - request.mutable_offset()->mutable_euler()->set_ry(0); - request.mutable_offset()->mutable_euler()->set_rz(0); // 调用 diff --git a/src/service/grpc_touch_service.cpp b/src/service/grpc_hlc_service.cpp similarity index 73% rename from src/service/grpc_touch_service.cpp rename to src/service/grpc_hlc_service.cpp index 7e55c657..95a6edaa 100644 --- a/src/service/grpc_touch_service.cpp +++ b/src/service/grpc_hlc_service.cpp @@ -3,7 +3,7 @@ // -#include "service/grpc_touch_service.h" +#include "service/grpc_hlc_service.h" #include #include "robot/humanoid_robot/humanoid_robot.h" #include "controller/touch_controller.h" @@ -14,16 +14,16 @@ using namespace cmvr::device; using namespace cmvr::api; using google::protobuf::util::TimeUtil; -gRPCTouchServiceImpl::gRPCTouchServiceImpl():dmgr_(DeviceManager::getInstance()){} +gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){} -grpc::Status gRPCTouchServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { +grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { grpc::Status ret = grpc::Status::OK; try { auto robot = dmgr_.getDevice(request->header().device_id()); ctrl::TouchController touch_controller; - touch_controller.touch(robot,request->pose(),request->offset()); + touch_controller.touch(request->u(),request->v(),request->max_force()); response->mutable_header()->set_success(true); response->mutable_header()->set_error_message(""); }catch (const std::exception& e) {