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

update moveJ limitQ

Browse Source
This commit is contained in:
linbo 2025-10-22 11:01:27 +08:00
parent 4da55aa445
commit 2c97319c9b
2 changed files with 40 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
config/cabin_robot.xml
View File

@ -41,31 +41,31 @@
verbose="false">
<CanManger id="" devId="">
<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="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="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="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="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="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="27" jointName="L_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" jointName="L_WRIST_Y" limitQLb="-1.02" limitQUb="1.02" limitQd="3.0"/>
<Motor id="1" jointName="L_WRIST_R" limitQLb="-0.293" limitQUb="0.293" limitQd="3.0"/>
</LeftArmCan>
<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="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="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"/>
</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="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"/>
<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="15" jointName="WAIST_P" 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"/>
</WaistCan>
</CanManger>

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

@ -3,6 +3,9 @@
//
#include "humanoid_robot.h"
#include <limits>
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h"
#include "utils/base/abstract_interpolation.h"
@ -315,7 +318,16 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
motor->setQ(j.rad);
auto cfg = motor->getConfig();
double limitQLb = cfg.getAttrDefault("limitQLb",(float)3.14);
double limitQUb = cfg.getAttrDefault("limitQUb",(float)3.14);
double clamped_rad = j.rad;
if (clamped_rad > limitQUb) {
clamped_rad = limitQUb; // 正向超限，限制到正向最大
} else if (clamped_rad < limitQLb) {
clamped_rad = limitQLb; // 反向超限，限制到反向最大
}
motor->setQ(clamped_rad);
}
}
@ -403,7 +415,17 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
motor->setQd(vel);
motor->setQ(j.rad);
auto cfg = motor->getConfig();
double limitQLb = cfg.getAttrDefault("limitQLb",(float)3.14);
double limitQUb = cfg.getAttrDefault("limitQUb",(float)3.14);
double clamped_rad = j.rad;
if (clamped_rad > limitQUb) {
clamped_rad = limitQUb; // 正向超限，限制到正向最大
} else if (clamped_rad < limitQLb) {
clamped_rad = limitQLb; // 反向超限，限制到反向最大
}
motor->setQ(clamped_rad);
}
}