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"
verbose="false">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0" 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"/>-->
<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"/>
</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="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"/>
@ -58,7 +58,12 @@
<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>
<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="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</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:
@ -48,19 +27,21 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# js = robot.getJointQ('right')
# 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.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# #
# time.sleep(200)
# robot.calibrateZeroQ("R_WRIST_Y")
# robot.calibrateZeroQ("R_WRIST_R")
time.sleep(10)
# # robot.torqueOff("R_WRIST_R")
# time.sleep(10)
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# time.sleep(20)
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
# # robot.torqueOff()
# robot.torqueOn()
# # time.sleep(500)
# robot.torqueOff("WAIST_P")
# time.sleep(30)

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

File diff suppressed because it is too large Load Diff

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> r_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> r_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>> r_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>> r_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>> r_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};

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

@ -55,7 +55,7 @@ TEST(HumanoidRobotTest,GetState) {
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);
if (!config.hasChild("DeviceManager")){
@ -71,17 +71,17 @@ TEST(HumanoidRobotTest,MyRobotTest) {
std::vector<std::vector<JointPoint>> traj;
// robot->calibrateZeroQ("R_WRIST_P");
robot->calibrateZeroQ("R_WRIST_Y");
// robot->calibrateZeroQ("R_WRIST_Y");
// robot->calibrateZeroQ("R_WRIST_R");
//
cmd = {
// {"L_SHOULDER_P", 0.0},
// {"L_SHOULDER_R", 0.0},
// {"L_SHOULDER_Y", 0.0},
// {"L_ELBOW_R", 0.0},
// {"L_WRIST_P", 0.0},
// {"L_WRIST_Y", 0.0},
// {"L_WRIST_R", 0.0},
{"L_SHOULDER_P", 0.0},
{"L_SHOULDER_R", 0.0},
{"L_SHOULDER_Y", 0.0},
{"L_ELBOW_R", 0.0},
{"L_WRIST_P", 0.0},
{"L_WRIST_Y", 0.0},
{"L_WRIST_R", 0.0},
//
// {"R_SHOULDER_P", 0.0}
// {"R_SHOULDER_R", 0.0},
@ -89,10 +89,15 @@ TEST(HumanoidRobotTest,MyRobotTest) {
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0.0},
// {"R_WRIST_R", 0.0},
// {"WAIST_P" ,0.0},
// {"WAIST_Y" ,0.0},
{"HEAD_P",0.0},
{"HEAD_Y",0.0},
{"HEAD_R",0.0},
};
robot->moveJ(cmd,0.8);