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

fix: resolve jerky movement during robotic arm initialization

Browse Source
This commit is contained in:
lgv 2025-12-18 11:43:48 +08:00
parent 0990b84cc8
commit e6cabe3f79
4 changed files with 86 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
config/cabin_robot.xml
View File

@ -20,9 +20,9 @@
</Camera>
<DexHand>
<!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
<RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">
<Freedom order="01" default_force="500" default_speed="500" />
</RH56DFTP>
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
@ -49,17 +49,17 @@
<Motor id="21" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="22" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" enable="false" toolFrame="R_FINGER_TIP">
<!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<!-- <Motor id="1" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
<Motor id="29" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="true">
<!-- <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<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>

4
src/devices/motor/ti5_motor/ti5_motor.h
View File

@ -31,6 +31,9 @@ namespace cmvr {
}
if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
// torqueOff(node_id_);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
// canopen_protocol->torqueOff(node_id_);
canopen_protocol->configPdo(node_id_);
canopen_protocol->configProfile(node_id_,4000,8000,8000);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
@ -38,6 +41,7 @@ namespace cmvr {
canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06);
canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F);
canopen_protocol->setLimitQd(node_id_,6.0);
// canopen_protocol->torqueOff(node_id_);
}
}
};

5
src/devices/robot/humanoid_robot/include/humanoid_robot.h
View File

@ -194,6 +194,11 @@ namespace cmvr::device{
std::shared_ptr<JointSpacePlanner> joint_space_planner_{nullptr};
// 每个电机组的锁和执行状态
std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_;
std::atomic<bool> is_left_arm_busy_{false}, is_right_arm_busy_{false};
std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
};

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

@ -220,11 +220,67 @@ template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
if (cmd.empty()) return;
for (auto c: cmd) {
LOG(INFO) << c.joint_name << c.rad << endl;
LOG(INFO) << vel << " "<< acc << endl;
LOG(INFO) << "vel : " << vel << "acc : " << acc << endl;
for (const auto &js: cmd) {
LOG(INFO) << js.joint_name << " , "<< js.rad << endl;
}
// 获取命令中的第一个电机，判断其所属电机组
auto &first_jp = cmd.at(0);
std::string group = "";
if (first_jp.joint_name.find("L_") != std::string::npos) {
group = "L_";
} else if (first_jp.joint_name.find("R_") != std::string::npos) {
group = "R_";
} else if (first_jp.joint_name.find("HEAD_") != std::string::npos) {
group = "HEAD_";
} else if (first_jp.joint_name.find("WAIST_") != std::string::npos) {
group = "WAIST_";
} else {
LOG(ERROR) << "Unknown joint group for " << first_jp.joint_name;
return;
}
// 检查所有电机是否属于同一组
for (auto &jp : cmd) {
if (jp.joint_name.find(group) == std::string::npos) {
LOG(ERROR) << "Mixed motor groups detected, skipping command.";
return; // 发现不属于同一组，直接返回
}
}
// 获取电机组的互斥锁和忙碌状态
std::mutex* group_mutex = nullptr;
std::atomic<bool>* is_busy_flag = nullptr;
// 根据电机组选择对应的mutex和忙碌标志
if (group == "L_") {
group_mutex = &left_arm_mutex_;
is_busy_flag = &is_left_arm_busy_;
} else if (group == "R_") {
group_mutex = &right_arm_mutex_;
is_busy_flag = &is_right_arm_busy_;
} else if (group == "HEAD_") {
group_mutex = &head_mutex_;
is_busy_flag = &is_head_busy_;
} else if (group == "WAIST_") {
group_mutex = &waist_mutex_;
is_busy_flag = &is_waist_busy_;
}
// 检查电机组是否已忙碌
bool is_busy = is_busy_flag->load();
if (is_busy) {
LOG(ERROR) << " Another thread is already controlling this motor group :" << group << ", skipping command.";
return;
}
// 设置为忙碌
is_busy_flag->store(true);
// 使用 mutex 锁住当前电机组，保证只有一个线程在控制
std::lock_guard<std::mutex> lock(*group_mutex);
// 1. 读当前关节角
std::unordered_map<std::string, double> cur_joints_angle;
@ -265,7 +321,6 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
return;
}
// 3. 采样
const double dt = 0.001;
auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
@ -274,9 +329,8 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
return;
}
// 4. 切模式、设速度
std::vector<std::shared_ptr<AbstractMotor> > motors;
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(cmd.size());
for (auto &jp: cmd) {
auto motor = motor_manager_->getMotor(jp.joint_name);
@ -291,13 +345,11 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
motors.push_back(motor);
}
// 5. 下发命令
const auto t0 = std::chrono::steady_clock::now();
size_t k = 1;
const size_t K = samples.size();
while (k < K) {
const auto &s = samples[k];
for (size_t i = 0; i < motors.size(); ++i) {
@ -321,9 +373,15 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
}
LOG(INFO) << jp.joint_name << " pos err = " << std::abs(it->second - jp.rad);
}
// 结束后恢复状态
is_busy_flag->store(false);
}
template<int DOF>
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
auto motor = motor_manager_->getMotor(joint_name);