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

feat:add head control API

Browse Source
This commit is contained in:
lgv 2025-10-11 14:51:47 +08:00
parent 3aa3f86027
commit 057984f440
5 changed files with 712 additions and 823 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
config/cabin_robot.xml
View File

@ -40,16 +40,16 @@
bufferSize="50" bufferSize="50"
verbose="false"> verbose="false">
<CanManger id="" devId=""> <CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0" toolFrame="L_FINGER_TIP"> <LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
<!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <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="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="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="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="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="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan> </LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" toolFrame="R_FINGER_TIP"> <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="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="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="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -58,7 +58,12 @@
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" 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"/> <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan> </RightArmCan>
<WaistCan id = " " devId = " " channelId ="2"> <HeadCan id = " " devId = " " channelId ="2" enable="true">
<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="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <Motor id="14" 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"/> <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</WaistCan> </WaistCan>

35
python/vision_servo/robot_warpper_test.py
View File

@ -16,28 +16,7 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# JOINT_POSITIONS = [
# [-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487],
# [-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434],
# [0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407],
# [0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825],
# [0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186],
# [0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301],
# [0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874],
# [0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988],
# [-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082],
# [-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444],
# [-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536],
# [0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621],
# [-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217],
# [-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659],
# [0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239],
# [0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999],
# [0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782],
# [0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079],
# [0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201],
# [-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615],
# ]
# #
# #
# while True: # while True:
@ -48,19 +27,21 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# js = robot.getJointQ('right') # js = robot.getJointQ('right')
# print(js) # print(js)
# 控制左臂关节 # 控制左臂关节
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # robot.calibrateZeroQ("R_WRIST_Y")
# robot.calibrateZeroQ("R_WRIST_R")
# time.sleep(3)
robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # 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(200) time.sleep(10)
# robot.calibrateZeroQ("R_WRIST_Y")
# robot.calibrateZeroQ("R_WRIST_R")
# # robot.torqueOff("R_WRIST_R") # # robot.torqueOff("R_WRIST_R")
# time.sleep(10) # time.sleep(10)
# 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(20) # time.sleep(20)
# # 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.torqueOn()
# # time.sleep(500) # # time.sleep(500)
# robot.torqueOff("WAIST_P") # robot.torqueOff("WAIST_P")
# time.sleep(30) # time.sleep(30)

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

@ -37,28 +37,56 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50)); CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
// 定义 lambda 函数，用于读取 XML 节点 enable 属性
auto readEnable = [](const XmlNode &node) -> bool {
std::string enable_str = node.getAttrString("enable"); // 默认 false
return (enable_str == "true" || enable_str == "1");
};
auto can_cfg = cfg.getChild("CanManger"); auto can_cfg = cfg.getChild("CanManger");
auto l_can_cfg = can_cfg.getChild("LeftArmCan"); auto l_can_cfg = can_cfg.getChild("LeftArmCan");
left_arm_enabled_ = readEnable(l_can_cfg);
if (left_arm_enabled_) {
l_motors_cfg_ = l_can_cfg.getChildren("Motor"); l_motors_cfg_ = l_can_cfg.getChildren("Motor");
l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg); l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
auto r_can_cfg = can_cfg.getChild("RightArmCan"); auto r_can_cfg = can_cfg.getChild("RightArmCan");
right_arm_enabled_ = readEnable(r_can_cfg);
if (right_arm_enabled_) {
r_motors_cfg_ = r_can_cfg.getChildren("Motor"); r_motors_cfg_ = r_can_cfg.getChildren("Motor");
r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg); r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
auto waist_can_cfg = can_cfg.getChild("WaistCan"); auto waist_can_cfg = can_cfg.getChild("WaistCan");
waist_enabled_ = readEnable(waist_can_cfg);
if (waist_enabled_) {
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor"); waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg); waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >(); waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >(); waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >(); waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
auto head_can_cfg = can_cfg.getChild("HeadCan");
head_enabled_ = readEnable(head_can_cfg);
if (head_enabled_) {
head_motors_cfg_ = head_can_cfg.getChildren("Motor");
head_can_client_ = std::make_shared<SocketCanClientRaw>(head_can_cfg);
head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
upd_timer_ = make_shared<FDTimer>(); upd_timer_ = make_shared<FDTimer>();
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
@ -72,151 +100,101 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::init() { void HumanoidRobot<DOF>::init() {
// 1 === 初始化公共组件 === struct Limb {
l_can_client_->init(); std::string name;
r_can_client_->init(); bool enabled;
waist_can_client_->init(); std::shared_ptr<AbstractCanbus> client;
auto ret = l_can_sender_->Init(l_can_client_.get(), false); std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
if (ret != ErrorCode::OK) { std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
LOG(ERROR) << "Failed to init can sender."; std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
} std::vector<XmlNode> motor_cfgs;
ret = r_can_sender_->Init(r_can_client_.get(), false); };
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to init can sender.";
}
ret = waist_can_sender_->Init(waist_can_client_.get(), false);
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to init can sender.";
}
std::vector<Limb> limbs{
ret = l_can_receiver_->Init(l_can_client_.get(), l_message_manager_.get(), false); {
if (ret != ErrorCode::OK) { "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_,
LOG(ERROR) << "Failed to init can receiver."; waist_motors_cfg_
} },
ret = r_can_receiver_->Init(r_can_client_.get(), r_message_manager_.get(), false); {
if (ret != ErrorCode::OK) { "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_,
LOG(ERROR) << "Failed to init can receiver."; l_motors_cfg_
} },
ret = waist_can_receiver_->Init(waist_can_client_.get(), waist_message_manager_.get(), false); {
if (ret != ErrorCode::OK) { "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_,
LOG(ERROR) << "Failed to init can receiver."; r_motors_cfg_
},
{
"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_,
head_motors_cfg_
} }
};
// 创建 MotorManager
// 2 === 启动通讯 ===
l_can_client_->start();
ret = l_can_sender_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can sender.";
}
r_can_client_->start();
ret = r_can_sender_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can sender.";
}
waist_can_client_->start();
ret = waist_can_sender_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can sender.";
}
ret = l_can_receiver_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can receiver.";
}
ret = r_can_receiver_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can receiver.";
}
ret = waist_can_receiver_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can receiver.";
}
// 3 == 创建协议 ===
auto l_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(l_can_sender_, l_message_manager_);
auto r_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(r_can_sender_, r_message_manager_);
auto waist_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(waist_can_sender_, waist_message_manager_);
// 4 === 创建 MotorManager ===
motor_manager_ = std::make_shared<MotorManager>(); motor_manager_ = std::make_shared<MotorManager>();
// for (const auto& cfg : r_motors_cfg_) { std::vector<std::future<void> > tasks;
// auto motor = std::make_shared<Ti5Motor>(cfg);
// motor->setProtocol(r_canopen_protocol);
// motor->init(); // 耗时操作
// motor_manager_->addMotor(motor);
// }
//
// for (const auto& cfg : l_motors_cfg_) {
// auto motor = std::make_shared<Ti5Motor>(cfg);
// motor->setProtocol(l_canopen_protocol);
// motor->init(); // 耗时操作
// motor_manager_->addMotor(motor);
// }
// 5 === 并行创建电机 === for (auto &limb: limbs) {
auto left_task = std::async(std::launch::async, [&] { if (!limb.enabled) continue;
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing LEFT motors...";
for (const auto &cfg: l_motors_cfg_) {
if (limb.client) limb.client->init();
// 2. 初始化 Sender / Receiver（如果有）
if (limb.sender && limb.receiver && limb.client) {
auto ret = limb.sender->Init(limb.client.get(), false);
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to init " << limb.name << " CAN sender.";
ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false);
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver.";
limb.client->start();
ret = limb.sender->Start();
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to start " << limb.name << " CAN sender.";
ret = limb.receiver->Start();
if (ret != ErrorCode::OK)
LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
}
// 3. 创建协议（如果有CAN）
std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
if (limb.sender && limb.message_manager) {
protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
}
// 4. 并行初始化电机
if (!limb.motor_cfgs.empty()) {
tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name <<
" motors...";
for (const auto &cfg: limb.motor_cfgs) {
auto motor = std::make_shared<Ti5Motor>(cfg); auto motor = std::make_shared<Ti5Motor>(cfg);
motor->setProtocol(l_canopen_protocol); if (protocol) motor->setProtocol(protocol);
motor->init(); motor->init();
motor_manager_->addMotor(motor); motor_manager_->addMotor(motor);
} }
}); }));
auto right_task = std::async(std::launch::async, [&] {
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing RIGHT motors...";
for (const auto &cfg: r_motors_cfg_) {
auto motor = std::make_shared<Ti5Motor>(cfg);
motor->setProtocol(r_canopen_protocol);
motor->init();
motor_manager_->addMotor(motor);
} }
});
auto waist_task = std::async(std::launch::async, [&] {
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing waist motors...";
for (const auto &cfg: waist_motors_cfg_) {
auto motor = std::make_shared<Ti5Motor>(cfg);
motor->setProtocol(waist_canopen_protocol);
motor->init();
motor_manager_->addMotor(motor);
} }
});
// 等待两个线程完成 // 等待所有任务完成
left_task.get(); for (auto &task: tasks) task.get();
right_task.get();
waist_task.get();
motor_manager_->getMotor("WAIST_Y")->setQ(0);
motor_manager_->getMotor("WAIST_P")->setQ(0);
rsm_.store(ROBOT_ESTOP); rsm_.store(ROBOT_ESTOP);
LOG(INFO) << "All motors initialized successfully."; LOG(INFO) << "All enabled motors initialized successfully.";
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::torqueOff() { void HumanoidRobot<DOF>::torqueOff() {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
throw runtime_error("robot is running");
}
if (rsm_.load() != ROBOT_TOROFF) {
for (const auto &pair: motor_manager_->motorsMap()) { for (const auto &pair: motor_manager_->motorsMap()) {
if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P") if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P")
pair.second->torqueOff(); pair.second->torqueOff();
} }
rsm_.store(ROBOT_TOROFF);
}
} catch (std::exception &e) { } catch (std::exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
@ -227,30 +205,7 @@ template<int DOF>
HumanoidRobot<DOF>::~HumanoidRobot() { HumanoidRobot<DOF>::~HumanoidRobot() {
// TODO: close can interfaces // TODO: close can interfaces
upd_timer_->stop(); upd_timer_->stop();
// this->torqueOff();
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>
@ -287,7 +242,6 @@ void HumanoidRobot<DOF>::getJointQ(std::unordered_map<std::string, double> &join
} }
template<int DOF> template<int DOF>
std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() { std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
return link_names_; return link_names_;
@ -317,7 +271,6 @@ void HumanoidRobot<DOF>::getState(RobotState &state) {
try { try {
lock_guard lock(exec_mtx_); lock_guard lock(exec_mtx_);
// TODO: copy m_state_ date into state // TODO: copy m_state_ date into state
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
@ -342,27 +295,17 @@ void HumanoidRobot<DOF>::torqueOff(const std::string &joint_name) {
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::eStop() { void HumanoidRobot<DOF>::eStop() {
if (rsm_.load() != ROBOT_ESTOP) {
CSP_buffer_->clear(); CSP_buffer_->clear();
CSV_buffer_->clear(); CSV_buffer_->clear();
CSC_buffer_->clear(); CSC_buffer_->clear();
for (const auto &pair: motor_manager_->motorsMap()) { for (const auto &pair: motor_manager_->motorsMap()) {
pair.second->brake(); pair.second->brake();
} }
rsm_.store(ROBOT_ESTOP);
}
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) { void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
for (const auto &j: cmd) { for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name); auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) { if (motor != nullptr) {
@ -396,11 +339,6 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
} }
std::this_thread::sleep_for(std::chrono::milliseconds(2)); std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion); } while (!completion);
rsm_.store(ROBOT_ESTOP);
} else {
throw runtime_error("rsm invalid");
}
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
@ -413,17 +351,9 @@ void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) { void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel,
double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
// update m_state_ // update m_state_
Eigen::Vector<double, DOF> q_init; Eigen::Vector<double, DOF> q_init;
auto q_map = getJointQ(); auto q_map = getJointQ();
@ -438,7 +368,8 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); 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.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());
// 输入为弧度
T_target(0, 3) = pose.position().x(); T_target(0, 3) = pose.position().x();
T_target(1, 3) = pose.position().y(); T_target(1, 3) = pose.position().y();
T_target(2, 3) = pose.position().z(); T_target(2, 3) = pose.position().z();
@ -451,7 +382,8 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position, bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw runtime_error("solve IK failed"); throw runtime_error("solve IK failed");
@ -495,10 +427,6 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
} }
std::this_thread::sleep_for(std::chrono::milliseconds(2)); std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion); } while (!completion);
rsm_.store(ROBOT_READY);
} else {
throw runtime_error("rsm invalid");
}
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
@ -506,17 +434,10 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets,
double vel,
double acc) { double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
// update m_state_ // update m_state_
Eigen::Vector<double, DOF> q_init; Eigen::Vector<double, DOF> q_init;
auto q_map = getJointQ(); auto q_map = getJointQ();
@ -574,26 +495,15 @@ void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vecto
// } // }
// std::this_thread::sleep_for(std::chrono::milliseconds(2)); // std::this_thread::sleep_for(std::chrono::milliseconds(2));
// } while (!completion); // } while (!completion);
rsm_.store(ROBOT_READY);
} else {
throw runtime_error("rsm invalid");
}
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) { void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
return;
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
// 获取当前关节状态 // 获取当前关节状态
Eigen::Vector<double, DOF> q_init; Eigen::Vector<double, DOF> q_init;
auto q_map = getJointQ(); auto q_map = getJointQ();
@ -703,9 +613,11 @@ void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::P
Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3); Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3);
// 打印 IK 解算出的 XYZ 位置 // 打印 IK 解算出的 XYZ 位置
if (i % 10 == 0) { // 每10个点打印一次，避免日志过多 if (i % 10 == 0) {
// 每10个点打印一次，避免日志过多
LOG(INFO) << "IK solution at point " << i << " : " LOG(INFO) << "IK solution at point " << i << " : "
<< "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos[2]; << "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos
[2];
} }
} }
@ -757,29 +669,15 @@ void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::P
} }
std::this_thread::sleep_for(std::chrono::milliseconds(2)); std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion); } while (!completion);
rsm_.store(ROBOT_READY);
} else {
throw std::runtime_error("rsm invalid");
}
} catch (std::exception &e) { } catch (std::exception &e) {
rsm_.store(ROBOT_ESTOP);
throw std::runtime_error(e.what()); throw std::runtime_error(e.what());
} }
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
rsm_.store(ROBOT_RUNNING);
// 获取电机控制对象 // 获取电机控制对象
// 这里的控制函数需要根据你的实际实现来进行填充 // 这里的控制函数需要根据你的实际实现来进行填充
// 获取目标关节的电机 // 获取目标关节的电机
@ -804,14 +702,8 @@ void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirectio
LOG(INFO) << "速度控制完成，电机已停止。"; LOG(INFO) << "速度控制完成，电机已停止。";
// 运动完成后不立即将 rsm_ 置为 READY，防止误操作 // 运动完成后不立即将 rsm_ 置为 READY，防止误操作
} else {
throw runtime_error("无效的机器人状态，无法进行速度控制");
}
} catch (exception &e) { } catch (exception &e) {
rsm_.store(ROBOT_ESTOP); // 出错时，设置为紧急停止状态
LOG(ERROR) << "speedJ 控制失败: " << e.what(); LOG(ERROR) << "speedJ 控制失败: " << e.what();
throw runtime_error("speedJ 控制失败: " + string(e.what())); throw runtime_error("speedJ 控制失败: " + string(e.what()));
} }
@ -944,8 +836,10 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
std::unique_lock<std::mutex> lock(queue_mutex); std::unique_lock<std::mutex> lock(queue_mutex);
// 等待队列中有数据或规划完成，使用load()读取原子变量 // 等待队列中有数据或规划完成，使用load()读取原子变量
if (queue_cv.wait_for(lock, std::chrono::milliseconds(500), if (queue_cv.wait_for(lock, std::chrono::milliseconds(500),
[&] { return !trajectory_queue.empty() || planning_completed.load() || execution_failed.load(); })) { [&] {
return !trajectory_queue.empty() || planning_completed.load() ||
execution_failed.load();
})) {
if (!trajectory_queue.empty()) { if (!trajectory_queue.empty()) {
point = trajectory_queue.front(); point = trajectory_queue.front();
trajectory_queue.pop(); trajectory_queue.pop();
@ -993,7 +887,8 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
} }
} }
if (execution_failed.load()) { // 使用load读取原子变量 if (execution_failed.load()) {
// 使用load读取原子变量
LOG(ERROR) << "控制执行线程异常退出"; LOG(ERROR) << "控制执行线程异常退出";
rsm_.store(ROBOT_ERROR); rsm_.store(ROBOT_ERROR);
} else { } else {
@ -1076,7 +971,8 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load(); return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load();
}); });
if (execution_failed.load()) { // 使用load读取原子变量 if (execution_failed.load()) {
// 使用load读取原子变量
break; break;
} }
@ -1110,14 +1006,14 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
control_thread.join(); control_thread.join();
} }
if (execution_failed.load()) { // 使用load读取原子变量 if (execution_failed.load()) {
// 使用load读取原子变量
LOG(ERROR) << "speedL执行失败"; LOG(ERROR) << "speedL执行失败";
rsm_.store(ROBOT_ERROR); rsm_.store(ROBOT_ERROR);
throw std::runtime_error("speedL execution failed"); throw std::runtime_error("speedL execution failed");
} else { } else {
LOG(INFO) << "speedL轨迹执行成功完成"; LOG(INFO) << "speedL轨迹执行成功完成";
} }
} catch (const std::exception &e) { } catch (const std::exception &e) {
LOG(ERROR) << "speedL失败: " << e.what(); LOG(ERROR) << "speedL失败: " << e.what();
rsm_.store(ROBOT_ERROR); rsm_.store(ROBOT_ERROR);
@ -1129,11 +1025,6 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) { void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
try { try {
// 1. 状态机检查：仅允许在 ESTOP/READY/TOROFF 状态启动
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY && rsm_.load() != ROBOT_TOROFF) {
throw runtime_error("followJointTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
}
// 2. 轨迹合法性检查 // 2. 轨迹合法性检查
if (!check_joint_traj_(traj, dt)) { if (!check_joint_traj_(traj, dt)) {
throw runtime_error("followJointTrajectory: invalid trajectory"); throw runtime_error("followJointTrajectory: invalid trajectory");
@ -1205,7 +1096,6 @@ void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoin
while (!traj_completed.load()) { while (!traj_completed.load()) {
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
} }
} catch (std::exception &e) { } catch (std::exception &e) {
// 异常处理：停止轨迹，重置状态机 // 异常处理：停止轨迹，重置状态机
rsm_.store(ROBOT_ESTOP); rsm_.store(ROBOT_ESTOP);
@ -1218,15 +1108,6 @@ template<int DOF>
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link, void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) { std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
try { try {
// 1. 基础校验：状态机与轨迹合法性
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop(); // 中断当前运动
throw runtime_error("followPoseTrajectory: robot is running, interrupted");
}
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY) {
throw runtime_error("followPoseTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
}
if (targets.empty()) { if (targets.empty()) {
throw runtime_error("followPoseTrajectory: pose trajectory is empty"); throw runtime_error("followPoseTrajectory: pose trajectory is empty");
} }
@ -1247,9 +1128,11 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
// 2.1 获取当前关节角度（初始化机器人状态） // 2.1 获取当前关节角度（初始化机器人状态）
Eigen::Vector<double, DOF> q_current; Eigen::Vector<double, DOF> q_current;
auto q_map_current = getJointQ(); auto q_map_current = getJointQ();
q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"],
q_map_current["L_ELBOW_R"],
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"],
q_map_current["R_ELBOW_R"],
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
m_state_->SetQ(q_current); m_state_->SetQ(q_current);
m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态 m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态
@ -1296,7 +1179,8 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
// 3.2.1 计算当前应到达的关节位置（匀速插值） // 3.2.1 计算当前应到达的关节位置（匀速插值）
auto now = std::chrono::high_resolution_clock::now(); auto now = std::chrono::high_resolution_clock::now();
double elapsed = std::chrono::duration<double>(now - transition_start_time).count(); double elapsed = std::chrono::duration<double>(now - transition_start_time).count();
Eigen::Vector<double, DOF> q_transition = q_current + (q_first - q_current) * std::min(elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0); Eigen::Vector<double, DOF> q_transition = q_current + (q_first - q_current) * std::min(
elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0);
// 3.2.2 发送过渡运动关节指令 // 3.2.2 发送过渡运动关节指令
for (size_t i = 0; i < DOF; ++i) { for (size_t i = 0; i < DOF; ++i) {
@ -1313,7 +1197,8 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
for (size_t i = 0; i < DOF; ++i) { for (size_t i = 0; i < DOF; ++i) {
const std::string &joint_name = joint_names_[i]; const std::string &joint_name = joint_names_[i];
auto motor = motor_manager_->getMotor(joint_name); auto motor = motor_manager_->getMotor(joint_name);
if (motor && !motor->reachedTargetQ()) { // 精度阈值：0.0001rad（≈0.0057°） if (motor && !motor->reachedTargetQ()) {
// 精度阈值：0.0001rad（≈0.0057°）
transition_completed = false; transition_completed = false;
break; break;
} }
@ -1416,7 +1301,6 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
while (!traj_completed.load()) { while (!traj_completed.load()) {
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
} }
} catch (std::exception &e) { } catch (std::exception &e) {
// 异常处理：重置状态机，确保机器人安全 // 异常处理：重置状态机，确保机器人安全
rsm_.store(ROBOT_ESTOP); rsm_.store(ROBOT_ESTOP);
@ -1446,7 +1330,6 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, dou
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) {
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);
} }
@ -1507,9 +1390,9 @@ void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string
} }
template<int DOF> 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) { void HumanoidRobot<DOF>::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,
double vel, double acc) {
try { try {
// 1 : 计算当前位姿 // 1 : 计算当前位姿
auto cur_pose = fk(base_link, ee_link); auto cur_pose = fk(base_link, ee_link);
@ -1532,7 +1415,6 @@ void HumanoidRobot<DOF>::servoDeltaJ(const std::string &base_link, const std::st
} }
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 {
@ -1644,8 +1526,10 @@ Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix
return Eigen::Vector3d(rx, ry, rz); return Eigen::Vector3d(rx, ry, rz);
} }
template<int DOF> template<int DOF>
std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) { std::vector<double> HumanoidRobot<
DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) {
// update m_state_ // update m_state_
try { try {
@ -1662,7 +1546,8 @@ std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const s
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); 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.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());
// 输入为弧度
T_target(0, 3) = pose.position().x(); T_target(0, 3) = pose.position().x();
T_target(1, 3) = pose.position().y(); T_target(1, 3) = pose.position().y();
T_target(2, 3) = pose.position().z(); T_target(2, 3) = pose.position().z();
@ -1675,7 +1560,8 @@ std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const s
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd{}; Eigen::Vector<double, DOF> q_cmd{};
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position, bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw std::runtime_error("IK solve failed"); throw std::runtime_error("IK solve failed");
@ -1685,8 +1571,6 @@ std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const s
} catch (std::exception &e) { } catch (std::exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
} }
@ -1725,7 +1609,6 @@ cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const st
pose.mutable_euler()->set_rx(euler(0)); pose.mutable_euler()->set_rx(euler(0));
pose.mutable_euler()->set_ry(euler(1)); pose.mutable_euler()->set_ry(euler(1));
pose.mutable_euler()->set_rz(euler(2)); pose.mutable_euler()->set_rz(euler(2));
} catch (const std::exception &e) { } catch (const std::exception &e) {
throw std::runtime_error(std::string("FK计算失败: ") + e.what()); throw std::runtime_error(std::string("FK计算失败: ") + e.what());
} }
@ -1739,12 +1622,12 @@ void printTrajectoryInfo(
const std::vector<double> &times, const std::vector<double> &times,
const std::vector<double> &velocities, const std::vector<double> &velocities,
double total_distance) { double total_distance) {
std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl; std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl;
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl; std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl;
std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl; std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
std::cout << std::setw(4) << "序号" << " | " std::cout << std::setw(4) << "序号" << " | "
<< std::setw(8) << "时间(s)" << " | " << std::setw(8) << "时间(s)" << " | "
<< std::setw(10) << "x(m)" << " | " << std::setw(10) << "x(m)" << " | "
@ -1752,7 +1635,8 @@ void printTrajectoryInfo(
<< std::setw(10) << "z(m)" << " | " << std::setw(10) << "z(m)" << " | "
<< std::setw(12) << "速度(m/s)" << " | " << std::setw(12) << "速度(m/s)" << " | "
<< std::setw(16) << "到起点距离(m)" << std::endl; << std::setw(16) << "到起点距离(m)" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
Eigen::Vector3d start_pos(trajectory[0](0, 3), trajectory[0](1, 3), trajectory[0](2, 3)); Eigen::Vector3d start_pos(trajectory[0](0, 3), trajectory[0](1, 3), trajectory[0](2, 3));
for (size_t idx = 0; idx < trajectory.size(); ++idx) { for (size_t idx = 0; idx < trajectory.size(); ++idx) {
@ -1768,7 +1652,8 @@ void printTrajectoryInfo(
<< std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | " << std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | "
<< std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl; << std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl;
} }
std::cout << "=========================================================================================\n" << std::endl; std::cout << "=========================================================================================\n" <<
std::endl;
} }
@ -1778,9 +1663,11 @@ void HumanoidRobot<DOF>::moveDeltaL(const std::string &base_link, const std::str
try { try {
Eigen::Vector<double, DOF> q_current_for_ik; Eigen::Vector<double, DOF> q_current_for_ik;
auto q_map_current = getJointQ(); auto q_map_current = getJointQ();
q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"],
q_map_current["L_ELBOW_R"],
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"],
q_map_current["R_ELBOW_R"],
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
LOG(INFO) << "q_current_for_ik: " << q_current_for_ik; LOG(INFO) << "q_current_for_ik: " << q_current_for_ik;
@ -1803,7 +1690,6 @@ void HumanoidRobot<DOF>::moveDeltaL(const std::string &base_link, const std::str
// 3. 调用moveL执行直线运动到目标位姿 // 3. 调用moveL执行直线运动到目标位姿
moveL(base_link, ee_link, target_pose, vel, acc); moveL(base_link, ee_link, target_pose, vel, acc);
} catch (const std::exception &e) { } catch (const std::exception &e) {
LOG(ERROR) << "moveDeltaL failed: " << e.what(); LOG(ERROR) << "moveDeltaL failed: " << e.what();
throw std::runtime_error(std::string("moveDeltaL error: ") + e.what()); throw std::runtime_error(std::string("moveDeltaL error: ") + e.what());
@ -1844,9 +1730,11 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
// 2. 获取当前关节配置并验证目标可达性 // 2. 获取当前关节配置并验证目标可达性
Eigen::Vector<double, DOF> q_current; Eigen::Vector<double, DOF> q_current;
auto q_map_current = getJointQ(); auto q_map_current = getJointQ();
q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"],
q_map_current["L_ELBOW_R"],
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"],
q_map_current["R_ELBOW_R"],
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
LOG(INFO) << "Current joint configuration: " << q_current; LOG(INFO) << "Current joint configuration: " << q_current;
@ -1948,16 +1836,20 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
} }
// 8. 打印轨迹信息 // 8. 打印轨迹信息
std::cout << "\n===================================== 轨迹规划信息 =====================================" << std::endl; std::cout << "\n===================================== 轨迹规划信息 =====================================" <<
std::endl;
std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl; std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl;
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl; std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl;
std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl; std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl;
std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl; std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl;
std::cout << "起点位置: (x=" << T_current(0,3) << ", y=" << T_current(1,3) << ", z=" << T_current(2,3) << ")" << std::endl; std::cout << "起点位置: (x=" << T_current(0, 3) << ", y=" << T_current(1, 3) << ", z=" << T_current(2, 3) << ")" <<
std::cout << "终点位置: (x=" << T_target(0,3) << ", y=" << T_target(1,3) << ", z=" << T_target(2,3) << ")" << std::endl; std::endl;
std::cout << "终点位置: (x=" << T_target(0, 3) << ", y=" << T_target(1, 3) << ", z=" << T_target(2, 3) << ")" <<
std::endl;
std::cout << "保持姿态不变" << std::endl; std::cout << "保持姿态不变" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl; std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
// 9. 执行轨迹 // 9. 执行轨迹
auto loop_start_time = std::chrono::high_resolution_clock::now(); auto loop_start_time = std::chrono::high_resolution_clock::now();
@ -1987,7 +1879,6 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
for (const auto &j: joint_command) { for (const auto &j: joint_command) {
auto motor = motor_manager_->getMotor(j.joint_name); auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) { if (motor != nullptr) {
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);
} }
@ -2012,7 +1903,8 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
std::this_thread::sleep_until(expected_time_point); std::this_thread::sleep_until(expected_time_point);
} else { } else {
LOG(WARNING) << "moveL: Behind schedule at point " << i LOG(WARNING) << "moveL: Behind schedule at point " << i
<< " by " << std::chrono::duration_cast<std::chrono::milliseconds>(now - expected_time_point).count() << "ms"; << " by " << std::chrono::duration_cast<std::chrono::milliseconds>(now - expected_time_point).
count() << "ms";
} }
} }
@ -2021,7 +1913,6 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
m_robot_->ComputeForwardKinematics(m_state_); m_robot_->ComputeForwardKinematics(m_state_);
rsm_.store(ROBOT_READY); rsm_.store(ROBOT_READY);
LOG(INFO) << "moveL: Trajectory completed successfully"; LOG(INFO) << "moveL: Trajectory completed successfully";
} catch (const std::exception &e) { } catch (const std::exception &e) {
LOG(ERROR) << "moveL failed: " << e.what(); LOG(ERROR) << "moveL failed: " << e.what();
rsm_.store(ROBOT_ERROR); rsm_.store(ROBOT_ERROR);
@ -2120,8 +2011,7 @@ void HumanoidRobot<DOF>::generateSTrapezoidalProfile(double total_distance, doub
} }
template<int DOF> template<int DOF>
cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std::string &target_link) cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std::string &target_link) {
{
// 获取gRPC生成的Pose3d消息 // 获取gRPC生成的Pose3d消息
auto grpc_pose = fk(base_link, target_link); auto grpc_pose = fk(base_link, target_link);
@ -2150,5 +2040,3 @@ cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std:
template class cmvr::device::HumanoidRobot<7>; template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>; template class cmvr::device::HumanoidRobot<14>;
template class cmvr::device::HumanoidRobot<20>; template class cmvr::device::HumanoidRobot<20>;

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

@ -190,23 +190,33 @@ namespace cmvr::device{
std::vector<XmlNode> l_motors_cfg_{}; std::vector<XmlNode> l_motors_cfg_{};
std::vector<XmlNode> r_motors_cfg_{}; std::vector<XmlNode> r_motors_cfg_{};
std::vector<XmlNode> waist_motors_cfg_{}; std::vector<XmlNode> waist_motors_cfg_{};
std::vector<XmlNode> head_motors_cfg_{};
std::shared_ptr<AbstractCanbus> l_can_client_{nullptr}; std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};
std::shared_ptr<AbstractCanbus> r_can_client_{nullptr}; std::shared_ptr<AbstractCanbus> r_can_client_{nullptr};
std::shared_ptr<AbstractCanbus> waist_can_client_{nullptr}; std::shared_ptr<AbstractCanbus> waist_can_client_{nullptr};
std::shared_ptr<AbstractCanbus> head_can_client_{nullptr};
std::shared_ptr<CanReceiver<msgs::RobotDetail>> l_can_receiver_{nullptr}; std::shared_ptr<CanReceiver<msgs::RobotDetail>> l_can_receiver_{nullptr};
std::shared_ptr<CanReceiver<msgs::RobotDetail>> r_can_receiver_{nullptr}; std::shared_ptr<CanReceiver<msgs::RobotDetail>> r_can_receiver_{nullptr};
std::shared_ptr<CanReceiver<msgs::RobotDetail>> waist_can_receiver_{nullptr}; std::shared_ptr<CanReceiver<msgs::RobotDetail>> waist_can_receiver_{nullptr};
std::shared_ptr<CanReceiver<msgs::RobotDetail>> head_can_receiver_{nullptr};
std::shared_ptr<CanSender<msgs::RobotDetail>> l_can_sender_{nullptr}; std::shared_ptr<CanSender<msgs::RobotDetail>> l_can_sender_{nullptr};
std::shared_ptr<CanSender<msgs::RobotDetail>> r_can_sender_{nullptr}; std::shared_ptr<CanSender<msgs::RobotDetail>> r_can_sender_{nullptr};
std::shared_ptr<CanSender<msgs::RobotDetail>> waist_can_sender_{nullptr}; std::shared_ptr<CanSender<msgs::RobotDetail>> waist_can_sender_{nullptr};
std::shared_ptr<CanSender<msgs::RobotDetail>> head_can_sender_{nullptr};
std::shared_ptr<MessageManager<msgs::RobotDetail>> l_message_manager_{nullptr}; std::shared_ptr<MessageManager<msgs::RobotDetail>> l_message_manager_{nullptr};
std::shared_ptr<MessageManager<msgs::RobotDetail>> r_message_manager_{nullptr}; std::shared_ptr<MessageManager<msgs::RobotDetail>> r_message_manager_{nullptr};
std::shared_ptr<MessageManager<msgs::RobotDetail>> waist_message_manager_{nullptr}; std::shared_ptr<MessageManager<msgs::RobotDetail>> waist_message_manager_{nullptr};
std::shared_ptr<MessageManager<msgs::RobotDetail>> head_message_manager_{nullptr};
bool waist_enabled_{false};
bool right_arm_enabled_{false};
bool left_arm_enabled_{false};
bool head_enabled_{false};
std::shared_ptr<MotorManager> motor_manager_{nullptr}; std::shared_ptr<MotorManager> motor_manager_{nullptr};

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

@ -55,7 +55,7 @@ TEST(HumanoidRobotTest,GetState) {
TEST(HumanoidRobotTest,MyRobotTest) { TEST(HumanoidRobotTest,MyRobotTest) {
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path); const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){ if (!config.hasChild("DeviceManager")){
@ -71,17 +71,17 @@ TEST(HumanoidRobotTest,MyRobotTest) {
std::vector<std::vector<JointPoint>> traj; std::vector<std::vector<JointPoint>> traj;
// robot->calibrateZeroQ("R_WRIST_P"); // robot->calibrateZeroQ("R_WRIST_P");
robot->calibrateZeroQ("R_WRIST_Y"); // robot->calibrateZeroQ("R_WRIST_Y");
// robot->calibrateZeroQ("R_WRIST_R"); // robot->calibrateZeroQ("R_WRIST_R");
// //
cmd = { cmd = {
// {"L_SHOULDER_P", 0.0}, {"L_SHOULDER_P", 0.0},
// {"L_SHOULDER_R", 0.0}, {"L_SHOULDER_R", 0.0},
// {"L_SHOULDER_Y", 0.0}, {"L_SHOULDER_Y", 0.0},
// {"L_ELBOW_R", 0.0}, {"L_ELBOW_R", 0.0},
// {"L_WRIST_P", 0.0}, {"L_WRIST_P", 0.0},
// {"L_WRIST_Y", 0.0}, {"L_WRIST_Y", 0.0},
// {"L_WRIST_R", 0.0}, {"L_WRIST_R", 0.0},
// //
// {"R_SHOULDER_P", 0.0} // {"R_SHOULDER_P", 0.0}
// {"R_SHOULDER_R", 0.0}, // {"R_SHOULDER_R", 0.0},
@ -89,10 +89,15 @@ TEST(HumanoidRobotTest,MyRobotTest) {
// {"R_ELBOW_R", 0.0}, // {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0}, // {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0}, // {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0.0}, // {"R_WRIST_R", 0.0},
// {"WAIST_P" ,0.0}, // {"WAIST_P" ,0.0},
// {"WAIST_Y" ,0.0}, // {"WAIST_Y" ,0.0},
{"HEAD_P",0.0},
{"HEAD_Y",0.0},
{"HEAD_R",0.0},
}; };
robot->moveJ(cmd,0.8); robot->moveJ(cmd,0.8);