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

feat:add touch grpc service API

Browse Source
This commit is contained in:
lgv 2025-09-01 16:24:08 +08:00
parent f0b5dfc72d
commit 9bf547c2f7
20 changed files with 336 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
README.md
View File

@ -95,14 +95,15 @@ sudo make install
``` ```
### 5. librealsense2 install ### 5. librealsense2 2.55.1 install
```shell ```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 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 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-dkms=2.55.1-0~realsense.12473 -y
sudo apt-get install librealsense2-utils -y sudo apt-get install librealsense2-utils=2.55.1-0~realsense.12473 -y
sudo apt-get install librealsense2-dev -y sudo apt-get install librealsense2-dev=2.55.1-0~realsense.12473 -y
sudo apt-get install librealsense2-dbg -y sudo apt-get install librealsense2-dbg=2.55.1-0~realsense.12473 -y
``` ```
### 5. OSQPEigen install(build from source) ### 5. OSQPEigen install(build from source)

4
config/cabin_robot.xml
View File

@ -14,7 +14,7 @@
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>--> <!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>--> <!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>--> <!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/> <!-- <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>--> <!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>--> <!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera> </Camera>
@ -89,7 +89,7 @@
</Microphone> </Microphone>
<Speaker> <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="500000" volume="80"/>-->
</Speaker> </Speaker>
<Canbus> <Canbus>

11
include/controller/touch_controller.h
View File

@ -9,6 +9,8 @@
#include "devices/abstract_dexhand.h" #include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h" #include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/geometry.pb.h"
#include "librealsense2/rs.h"
#include "librealsense2/h/rs_frame.h"
namespace cmvr { namespace cmvr {
namespace ctrl { namespace ctrl {
@ -80,9 +82,14 @@ namespace cmvr {
TouchController(){}; TouchController(){};
TouchController( std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand) TouchController( std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand)
:robot_(std::move(robot)),hand_(std::move(hand)), :robot_(std::move(robot)),hand_(std::move(hand)),
pid_(std::make_shared<PID>(0.05, 0.001, 0.01, 50.0, 0.5, 1.0)){} pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
~TouchController()=default; ~TouchController()=default;
bool isArrive(double max_force);
void touch(int u,int v ,double max_force){}
void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset); void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
@ -98,6 +105,8 @@ namespace cmvr {
// 从压阻矩阵提取触控点与压力 // 从压阻矩阵提取触控点与压力
bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y); bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
}; };
} }
} }

2
include/devices/abstract_camera.h
View File

@ -4,6 +4,7 @@
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include "abstract_device.h" #include "abstract_device.h"
#include <Eigen/Core>
namespace cmvr::device { namespace cmvr::device {
struct Rs2Intrinsics struct Rs2Intrinsics
@ -58,6 +59,7 @@ namespace cmvr::device {
virtual bool startStreaming() {return true;} virtual bool startStreaming() {return true;}
virtual void stopStreaming() {} virtual void stopStreaming() {}
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
protected: protected:
CameraState state_{}; CameraState state_{};
void clear_error_() { void clear_error_() {

3
include/devices/abstract_robot.h
View File

@ -93,6 +93,9 @@ namespace cmvr::device{
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); } virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); } virtual void servoJ(std::vector<JointPoint> &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"); } virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }

8
include/service/grpc_touch_service.h → include/service/grpc_hlc_service.h
View File

@ -4,14 +4,14 @@
#pragma once #pragma once
#include "device_manager/device_manager.h" #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 cmvr {
namespace service { namespace service {
class gRPCTouchServiceImpl final : public api::TouchService::Service { class gRPCHlcServiceImpl final : public api::HlcService::Service {
public: public:
gRPCTouchServiceImpl(); gRPCHlcServiceImpl();
~gRPCTouchServiceImpl() = default; ~gRPCHlcServiceImpl() = default;
grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
private: private:

17
protos/cmvr/api/touch_service.proto → protos/cmvr/api/hlc_command.proto
View File

@ -1,15 +1,19 @@
syntax = "proto3"; 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 Touch{
message Request{ message Request{
CommandHeader.Request header = 1; CommandHeader.Request header = 1;
cmvr.msgs.Pose3d pose = 2; // 触点坐标，unit: ms
cmvr.msgs.Pose3d offset = 3; int32 u = 2; // 水平坐标
int32 v = 3; // 垂直坐标
// 触控时允许的最大作用力，unit: N
double max_force = 4;
} }
message Response{ message Response{
@ -18,6 +22,3 @@ message Touch{
} }
service TouchService{
rpc touch(Touch.Request) returns (Touch.Response);
}

9
protos/cmvr/api/hlc_service.proto Normal file
View File

@ -0,0 +1,9 @@
syntax = "proto3";
import "cmvr/api/hlc_command.proto";
package cmvr.api;
service HlcService{
rpc touch(Touch.Request) returns (Touch.Response);
}

6
python/vision_servo/robot_warpper_test.py
View File

@ -45,12 +45,12 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# robot.moveJ('right', j) # robot.moveJ('right', j)
robot.torqueOn() 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.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]) # # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # # #
time.sleep(10) # time.sleep(10)
robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
# robot.torqueOff() # robot.torqueOff()
# // {"R_SHOULDER_P", 0.00203898}, # // {"R_SHOULDER_P", 0.00203898},

20
src/controller/touch_controller.cpp
View File

@ -91,4 +91,24 @@ bool TouchController::extractTouch(const std::vector<std::vector<uint16_t> > &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;
}

24
src/controller/touch_controller_test.cpp
View File

@ -39,15 +39,35 @@ TEST(TouchControllerTest,MyTest) {
pose.mutable_euler()->set_rz(1.57); pose.mutable_euler()->set_rz(1.57);
robot->moveJ("PELVIS_S","R_WRIST_R_S",pose); 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; Pose3d offset;
TouchController controller(robot, hand); TouchController controller(robot, hand);
while (true) { 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)); std::this_thread::sleep_for(std::chrono::milliseconds(10));
} }
} }

96
src/devices/camera/realsense_camera/realsense_camera.cpp
View File

@ -244,13 +244,28 @@ void RealsenseCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
auto frame = stream_frame_buffer_->pop(getImageIndex_); auto frame = stream_frame_buffer_->pop(getImageIndex_);
if (frame.has_value()) { if (frame.has_value()) {
color = frame.value().rgbImage.clone(); color = frame.value().rgbImage.clone();
{
std::lock_guard<std::mutex> lock(frame_mtx_);
latest_depth_ = frame.value().depthImage.clone();
}
} }
else else
{ {
rs2::frameset frames = get_frameset(false); rs2::frameset frames = get_frameset(false);
auto color_frame = frames.get_color_frame(); auto color_frame = frames.get_color_frame();
color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), CV_8UC3, const_cast<void*>(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<void*>(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<void*>(depth_frame.get_data()),
cv::Mat::AUTO_STEP).clone();
std::lock_guard<std::mutex> lock(frame_mtx_);
latest_depth_ = depth;
}
} }
LOG(INFO) << "realsense get rgb frame successfully"; 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) { void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
intrinsics.cx = intrinsics_.ppx; intrinsics.cx = intrinsics_.ppx;
intrinsics.cy = intrinsics_.ppy; intrinsics.cy = intrinsics_.ppy;
intrinsics.fx = intrinsics_.fx; intrinsics.fx = intrinsics_.fx;
@ -272,8 +286,7 @@ void RealsenseCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
if (frame.has_value()) { if (frame.has_value()) {
depth = frame.value().depthImage.clone(); depth = frame.value().depthImage.clone();
} }
else else {
{
rs2::frameset frames; rs2::frameset frames;
try { 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())); throw std::runtime_error("[RealsenseCamera] (getDepthImage): Failed to get frames: " + std::string(e.what()));
} }
// 提取深度帧
rs2::depth_frame depth_frame = frames.get_depth_frame(); rs2::depth_frame depth_frame = frames.get_depth_frame();
if (!depth_frame) { if (!depth_frame) {
throw std::runtime_error("[RealsenseCamera] (getDepthImage): No depth frame available"); throw std::runtime_error("[RealsenseCamera] (getDepthImage): No depth frame available");
} }
// 创建深度图像（16位无符号整数，单位：毫米） depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()),
depth = cv::Mat(cv::Size(640, 480), CV_16UC1, const_cast<void*>(depth_frame.get_data()), cv::Mat::AUTO_STEP).clone(); CV_16UC1, const_cast<void*>(depth_frame.get_data()),
cv::Mat::AUTO_STEP).clone();
}
// 为避免内存问题，创建深拷贝 // 保存 latest_depth_
cv::Mat temp = depth.clone(); {
depth = temp; std::lock_guard<std::mutex> lock(frame_mtx_);
latest_depth_ = depth.clone();
} }
} }
void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) { void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {
intrinsics.cx = intrinsics_.ppx; intrinsics.cx = intrinsics_.ppx;
intrinsics.cy = intrinsics_.ppy; intrinsics.cy = intrinsics_.ppy;
intrinsics.fx = intrinsics_.fx; intrinsics.fx = intrinsics_.fx;
@ -311,44 +325,39 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic
color = frame.value().rgbImage.clone(); color = frame.value().rgbImage.clone();
depth = frame.value().depthImage.clone(); depth = frame.value().depthImage.clone();
} }
else else {
{
rs2::frameset frames; rs2::frameset frames;
try { try {
// 获取对齐的帧集合
frames = get_frameset(true); frames = get_frameset(true);
} catch (const std::exception& e) { } catch (const std::exception& e) {
throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what())); throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what()));
} }
// 提取彩色帧 // 对齐深度到彩色坐标系
rs2::frame color_frame = frames.get_color_frame(); rs2::align align_to(RS2_STREAM_COLOR);
if (!color_frame) { frames = align_to.process(frames);
throw std::runtime_error("[RealsenseCamera] (getRGBDImages): No color frame available");
}
// 提取深度帧 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(); rs2::depth_frame depth_frame = frames.get_depth_frame();
if (!depth_frame) { if (!color_frame || !depth_frame) {
throw std::runtime_error("[RealsenseCamera] (getRGBDImages): No depth frame available"); throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Missing frames");
} }
// 创建彩色图像（BGR格式） color = cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()),
color = cv::Mat(cv::Size(640, 480), CV_8UC3, const_cast<void*>(color_frame.get_data())); CV_8UC3, const_cast<void*>(color_frame.get_data())).clone();
depth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()),
// 创建深度图像（16位无符号整数，单位：毫米） CV_16UC1, const_cast<void*>(depth_frame.get_data())).clone();
depth = cv::Mat(cv::Size(640, 480), CV_16UC1, const_cast<void*>(depth_frame.get_data()));
// 为避免内存问题，创建深拷贝
cv::Mat color_temp = color.clone();
cv::Mat depth_temp = depth.clone();
color = color_temp;
depth = depth_temp;
} }
// 保存 latest_depth_
{
std::lock_guard<std::mutex> lock(frame_mtx_);
latest_depth_ = depth.clone();
}
} }
void RealsenseCamera::updateParams(const std::pair<std::string, std::string>& param) { void RealsenseCamera::updateParams(const std::pair<std::string, std::string>& param) {
std::lock_guard lock(ctrl_mtx_); std::lock_guard lock(ctrl_mtx_);
try { try {
@ -1024,3 +1033,22 @@ void RealsenseCamera::stopStreaming()
state_.is_streaming = false; state_.is_streaming = false;
} }
} }
Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) {
std::lock_guard<std::mutex> lock(frame_mtx_);
if (latest_depth_.empty()) return {0,0,0};
// 获取深度值，单位米
float depth_m = latest_depth_.at<uint16_t>(v, u) * 0.001f;
if (depth_m <= 0) return {0,0,0};
float pixel[2] = { static_cast<float>(u), static_cast<float>(v) };
float point[3];
rs2_deproject_pixel_to_point(point, &intrinsics_, pixel, depth_m);
return { point[0], point[1], point[2] };
}

6
src/devices/camera/realsense_camera/realsense_camera.h
View File

@ -39,6 +39,9 @@ namespace cmvr::device{
bool startStreaming() override; bool startStreaming() override;
void stopStreaming() override; void stopStreaming() override;
Eigen::Vector3f get3DPointFromPixel(int u, int v) override;
private: private:
rs2::frameset get_frameset(bool align); rs2::frameset get_frameset(bool align);
void streaming_worker_(); void streaming_worker_();
@ -100,6 +103,9 @@ namespace cmvr::device{
bool is_streaming_running = false; bool is_streaming_running = false;
bool is_recording_running = false; bool is_recording_running = false;
int stream_count_ = 0; int stream_count_ = 0;
cv::Mat latest_depth_;
std::mutex depth_mtx_;
}; };
} }

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

@ -224,6 +224,30 @@ template<int DOF>
HumanoidRobot<DOF>::~HumanoidRobot() { HumanoidRobot<DOF>::~HumanoidRobot() {
// TODO: close can interfaces // TODO: close can interfaces
upd_timer_->stop(); upd_timer_->stop();
std::vector<JointPoint> 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<int DOF> template<int DOF>
@ -659,23 +683,96 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, double dt) { void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, double dt) {
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
rsm_.store(ROBOT_RUNNING);
for (const auto &j: joints) { for (const auto &j: joints) {
auto motor = motor_manager_->getMotor(j.joint_name); auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) { if (motor != nullptr) {
motor->setQd(vel);
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
} }
motor->setQd(vel);
motor->setQ(j.rad); motor->setQ(j.rad);
} }
} }
rsm_.store(ROBOT_READY); }
template<int DOF>
void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel,
double acc) {
try {
// update m_state_
Eigen::Vector<double, DOF> 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<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw runtime_error("solve IK failed");
}
std::vector<JointPoint> 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<int DOF>
void HumanoidRobot<DOF>::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<int DOF> template<int DOF>
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
try { try {

4
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -59,7 +59,7 @@ namespace cmvr::device{
/* robot basic command*/ /* robot basic command*/
void eStop() override; void eStop() override;
void moveJ(std::vector<JointPoint> &cmd, double vel, double acc) override; void moveJ(std::vector<JointPoint> &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(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<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ; void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &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; 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<JointPoint> &joints, double dt) override; void servoJ(std::vector<JointPoint> &joints, double dt) override;
void servoJ(std::vector<JointPoint> &joints, double vel, double dt) override; void servoJ(std::vector<JointPoint> &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<cmvr::ctrl::PoseTarget> &targets, double dt) override; void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) override;
protected: protected:

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

@ -282,50 +282,50 @@ TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
// {"R_WRIST_R", 0.280016} // {"R_WRIST_R", 0.280016}
// }; // };
//
std::vector<JointPoint> cmd1{ // std::vector<JointPoint> cmd1{
{"L_SHOULDER_P", -0.747573}, // {"L_SHOULDER_P", -0.747573},
{"L_SHOULDER_R", -1.26911}, // {"L_SHOULDER_R", -1.26911},
{"L_SHOULDER_Y", -1.20811}, // {"L_SHOULDER_Y", -1.20811},
{"L_ELBOW_R", -1.51221}, // {"L_ELBOW_R", -1.51221},
{"L_WRIST_P", 2.64099}, // {"L_WRIST_P", 2.64099},
{"L_WRIST_Y", 0.417608}, // {"L_WRIST_Y", 0.417608},
{"L_WRIST_R", -0.518287}, // {"L_WRIST_R", -0.518287},
//
{"R_SHOULDER_P", -0.956276}, // {"R_SHOULDER_P", -0.956276},
{"R_SHOULDER_R", 1.0244}, // {"R_SHOULDER_R", 1.0244},
{"R_SHOULDER_Y", 2.70621}, // {"R_SHOULDER_Y", 2.70621},
{"R_ELBOW_R", 2.02276}, // {"R_ELBOW_R", 2.02276},
{"R_WRIST_P", -1.99653}, // {"R_WRIST_P", -1.99653},
{"R_WRIST_Y", 0.68523}, // {"R_WRIST_Y", 0.68523},
{"R_WRIST_R", 0.477066} // {"R_WRIST_R", 0.477066}
}; // };
//
std::vector<JointPoint> cmd2{ // std::vector<JointPoint> cmd2{
{"L_SHOULDER_P", -0.747573}, // {"L_SHOULDER_P", -0.747573},
{"L_SHOULDER_R", -1.26911}, // {"L_SHOULDER_R", -1.26911},
{"L_SHOULDER_Y", -1.20811}, // {"L_SHOULDER_Y", -1.20811},
{"L_ELBOW_R", -1.51221}, // {"L_ELBOW_R", -1.51221},
{"L_WRIST_P", 2.64099}, // {"L_WRIST_P", 2.64099},
{"L_WRIST_Y", 0.417608}, // {"L_WRIST_Y", 0.417608},
{"L_WRIST_R", -0.518287}, // {"L_WRIST_R", -0.518287},
//
{"R_SHOULDER_P", 0.226871}, // {"R_SHOULDER_P", 0.226871},
{"R_SHOULDER_R", 0.624717}, // {"R_SHOULDER_R", 0.624717},
{"R_SHOULDER_Y", 1.15086}, // {"R_SHOULDER_Y", 1.15086},
{"R_ELBOW_R", 1.30365}, // {"R_ELBOW_R", 1.30365},
{"R_WRIST_P", -2.14683}, // {"R_WRIST_P", -2.14683},
{"R_WRIST_Y", 0.196003}, // {"R_WRIST_Y", 0.196003},
{"R_WRIST_R", 0.106678} // {"R_WRIST_R", 0.106678}
}; // };
//
//
//
while (true) { // while (true) {
robot->moveJ(cmd1,0.8); // robot->moveJ(cmd1,0.8);
robot->moveJ(cmd2,0.8); // robot->moveJ(cmd2,0.8);
std::this_thread::sleep_for(std::chrono::milliseconds(500)); // std::this_thread::sleep_for(std::chrono::milliseconds(500));
} // }
} }
@ -345,16 +345,20 @@ TEST(HumanoidRobotTest,MoveDeltaTest) {
cmvr::msgs::Pose3d pose; cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 0); pose.mutable_position()->set_x( 0);
pose.mutable_position()->set_y(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_rx(0);
pose.mutable_euler()->set_ry(0); pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(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) { TEST(HumanoidRobotTest,AngleToTest) {

6
src/main.cpp
View File

@ -17,7 +17,7 @@
#include "utils/base/logger.h" #include "utils/base/logger.h"
#include "service/grpc_head_service.h" #include "service/grpc_head_service.h"
#include "service/grpc_humanoid_robot_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 "http/httpclient.h"
#include "json/json.h" #include "json/json.h"
@ -49,7 +49,7 @@ void runServer(const XmlNode &cfg){
auto dexhand_service = gRPCDexHandServiceImpl(); auto dexhand_service = gRPCDexHandServiceImpl();
auto biohand_service = gRPCMBioHeadServiceImpl(); auto biohand_service = gRPCMBioHeadServiceImpl();
auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl(); auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl();
auto touch_service = gRPCTouchServiceImpl(); auto hlc_service = gRPCHlcServiceImpl();
grpc::ServerBuilder builder; grpc::ServerBuilder builder;
builder.AddListeningPort(address, grpc::InsecureServerCredentials()); builder.AddListeningPort(address, grpc::InsecureServerCredentials());
@ -60,7 +60,7 @@ void runServer(const XmlNode &cfg){
builder.RegisterService(&dexhand_service); builder.RegisterService(&dexhand_service);
builder.RegisterService(&biohand_service); builder.RegisterService(&biohand_service);
builder.RegisterService(&humanoid_robot_service); builder.RegisterService(&humanoid_robot_service);
builder.RegisterService(&touch_service); builder.RegisterService(&hlc_service);
// 🔥 关键！启用反射 // 🔥 关键！启用反射
//grpc::reflection::InitProtoReflectionServerBuilderPlugin(); //grpc::reflection::InitProtoReflectionServerBuilderPlugin();

2
src/service/CMakeLists.txt
View File

@ -6,7 +6,7 @@ add_library(service
grpc_head_service.cpp grpc_head_service.cpp
grpc_dexhand_service.cpp grpc_dexhand_service.cpp
grpc_humanoid_robot_service.cpp grpc_humanoid_robot_service.cpp
grpc_touch_service.cpp grpc_hlc_service.cpp
) )
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

24
src/service/grpc_touch_client_test.cpp → src/service/grpc_hlc_client_test.cpp
View File

@ -4,17 +4,17 @@
#include "gtest/gtest.h" #include "gtest/gtest.h"
#include <glog/logging.h> #include <glog/logging.h>
#include <grpcpp/grpcpp.h> #include <grpcpp/grpcpp.h>
#include "service/grpc_touch_service.h" #include "service/grpc_hlc_service.h"
#include "google/protobuf/timestamp.pb.h" #include "google/protobuf/timestamp.pb.h"
#include <iostream> #include <iostream>
#include <google/protobuf/util/time_util.h> #include <google/protobuf/util/time_util.h>
using namespace cmvr::api; using namespace cmvr::api;
TEST(GrpcTouchClientTest, MyTest) { TEST(GrpcHlcClientTest, MyTest) {
// 连接服务端 // 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); 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; grpc::ClientContext context;
@ -25,23 +25,11 @@ TEST(GrpcTouchClientTest, MyTest) {
request.mutable_header()->set_device_id("hc01"); request.mutable_header()->set_device_id("hc01");
*request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); *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.set_u(12);
request.mutable_pose()->mutable_euler()->set_ry(0); request.set_v(13);
request.mutable_pose()->mutable_euler()->set_rz(1.57); 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);
// 调用 // 调用

8
src/service/grpc_touch_service.cpp → src/service/grpc_hlc_service.cpp
View File

@ -3,7 +3,7 @@
// //
#include "service/grpc_touch_service.h" #include "service/grpc_hlc_service.h"
#include <google/protobuf/util/time_util.h> #include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h" #include "robot/humanoid_robot/humanoid_robot.h"
#include "controller/touch_controller.h" #include "controller/touch_controller.h"
@ -14,16 +14,16 @@ using namespace cmvr::device;
using namespace cmvr::api; using namespace cmvr::api;
using google::protobuf::util::TimeUtil; 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; grpc::Status ret = grpc::Status::OK;
try { try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id()); auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
ctrl::TouchController touch_controller; 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_success(true);
response->mutable_header()->set_error_message(""); response->mutable_header()->set_error_message("");
}catch (const std::exception& e) { }catch (const std::exception& e) {