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

Compare commits

base: SmartBench:f9521a72e75d62f9d1b9122087a018f885138215
Branches Tags
SmartBench:main
SmartBench:xtkuang_dev
SmartBench:dev
SmartBench:linbo_dev
SmartBench:lgv_refator_code
SmartBench:backup-dev-before-rollback
SmartBench:dev_touch_screen
SmartBench:lgv_test_0227
SmartBench:lgv_11_12
SmartBench:backup/dev_before_replace
SmartBench:v1
SmartBench:refactor_dev
SmartBench:v1.1
SmartBench:v1.0
...
compare: SmartBench:95081c515588dbbcc9ffae2c5deaa61e45c499a8
Branches Tags
SmartBench:xtkuang_dev
SmartBench:dev
SmartBench:linbo_dev
SmartBench:lgv_refator_code
SmartBench:backup-dev-before-rollback
SmartBench:dev_touch_screen
SmartBench:lgv_test_0227
SmartBench:lgv_11_12
SmartBench:backup/dev_before_replace
SmartBench:v1
SmartBench:refactor_dev
SmartBench:main
SmartBench:v1.1
SmartBench:v1.0

10 Commits
f9521a72e7 ... 95081c5155

Author SHA1 Message Date
linbo
95081c5155 update AuboRobot 2025-11-17 13:48:22 +08:00
linbo
68756bd57a update controller 2025-11-07 15:29:07 +08:00
linbo
a76a92aa9b update controller 2025-11-07 11:28:18 +08:00
linbo
2065108bc2 update can_group 2025-11-06 17:27:37 +08:00
linbo
f5b765af3d update robot controller 2025-11-06 15:58:32 +08:00
linbo
82ab7cd887 update robot controller 2025-10-29 16:26:15 +08:00
linbo
505b373368 update robot controller 2025-10-24 17:06:17 +08:00
linbo
0720ec8171 update robot controller 2025-10-24 14:44:45 +08:00
linbo
80424c86f1 update robot controller 2025-10-24 14:37:17 +08:00
linbo
1a04ca2689 update HardWareManager 2025-10-24 10:51:49 +08:00
237 changed files with 84301 additions and 3731 deletions
Show Stats Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -49,7 +49,7 @@ protobuf_generate(
PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
)
target_include_directories(proto-objects PUBLIC ${PROTO_BINARY_DIR} ${PROTO_IMPORT_DIR})
target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++)
target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++ gRPC::grpc++_reflection)
############################################################
# include
@ -90,7 +90,7 @@ target_link_libraries(cmvr_es PRIVATE
cmvr_es::utils
cmvr_es::service
cmvr_es::monitor
cmvr_es::hardware
cmvr_es::hardware_manager
cmvr_es::httpclient
cmvr_es::device::canbus
cmvr_es::device::ti5motor

7
README.md
View File

@ -144,4 +144,9 @@ sudo dkpg -i Mech-Eye_API_2.5.1_amd64.deb
sudo systemctl stop brltty
sudo systemctl disable brltty
sudo apt remove brltty
```
```
### 开放端口
```shell
sudo iptables -A INPUT -p tcp --dport 50052 -j ACCEPT
```

74
config/cabin_robot.xml
View File

@ -32,44 +32,44 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />-->
<!-- <Neck/>-->
<!-- <Humanoid id="hc01" dof="14"-->
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"-->
<!-- baseLink="PELVIS_S"-->
<!-- jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"-->
<!-- linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"-->
<!-- bufferSize="50"-->
<!-- 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"/>-->
<!-- </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="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="false">-->
<!-- <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>-->
<!-- </CanManger>-->
<Humanoid id="hc01" dof="14"
urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
baseLink="PELVIS_S"
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
bufferSize="50"
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"/>
</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="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="false">
<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>
</CanManger>
<!-- </Humanoid>-->
</Humanoid>
</Robot>
<BioHead>

30
config/dev_config.xml
View File

@ -1,4 +1,4 @@
<CMVR-ES>
<CMVR-ES>
<Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
<Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
@ -53,24 +53,25 @@
<Devices>
<AGV>
</AGV>
<Battery>
</Battery>
<Camera>
<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"/>
<RealsenseCamera id="left_hand_cam" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>
<RealsenseCamera id="right_hand_cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" 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"/>-->
<!-- <RealsenseCamera id="left_hand_cam" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="right_hand_cam" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera>
<DexHand>
<RH56DFTP id="left_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>
<RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>
<!-- <RH56DFTP id="left_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
<!-- <RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
</DexHand>
<Robot>
<Robot id="" urdfPath="">
<Robot >
<HumanoidRobot id="hc01" urdfPath="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM">
<ControllerManager>
<ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
<JointPositionCtrl defaultSpeed="" defaultAcc=""/>
@ -83,7 +84,10 @@
<JointPositionCtrl/>
</ComponentGroup>
</ControllerManager>
</Robot>
</HumanoidRobot>
<AuboRobot id="hc02" ip="127.0.0.1" usrname="aubo" password="123456">
</AuboRobot>
</Robot>
<BioHead>
@ -92,11 +96,11 @@
<Microphone>
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
</Microphone>
<Speaker>
<ffmpegSpeaker id="spk1" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>
<!-- <ffmpegSpeaker id="spk1" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>-->
</Speaker>
</Devices>

140
config/dev_config_v2.xml Normal file
View File

@ -0,0 +1,140 @@
<CMVR-ES>
<Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
<Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
<HardwareManager>
<CanManager>
<CanGroup id="LeftArmCAN" channelId="0" enable="true">
<MotorProtocolManager>
<Ti5MotorProtocol/>
</MotorProtocolManager>
</CanGroup>
<CanGroup id="RightArmCAN" channelId="1" enable="true">
<MotorProtocolManager>
<Ti5MotorProtocol/>
</MotorProtocolManager>
</CanGroup>
<CanGroup id="NeckCAN" channelId ="2" enable="true">
<MotorProtocolManager>
<Ti5MotorProtocol/>
</MotorProtocolManager>
</CanGroup>
<CanGroup id = "WaistCAN" channelId ="3" enable="false">
<MotorProtocolManager>
<Ti5MotorProtocol/>
</MotorProtocolManager>
</CanGroup>
</CanManager>
<SerialManager>
<RS485Manager>
</RS485Manager>
<IOManager>
</IOManager>
</SerialManager>
</HardwareManager>
<DeviceManager>
<Devices>
<AGV>
</AGV>
<Battery>
</Battery>
<Camera>
<!-- <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"/>-->
<!-- <RealsenseCamera id="left_hand_cam" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="right_hand_cam" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera>
<DexHand>
<!-- <RH56DFTP id="left_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
<!-- <RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
</DexHand>
<Robot >
<HumanoidRobot id="hc01" urdfPath="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
baseLink="PELVIS_S">
<MotorManager>
<Motor id="23" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="24" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="25" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="26" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="27" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="1" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</MotorManager>
<ControllerManager>
<ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
<JointPositionCtrl defaultSpeed="" defaultAcc=""/>
<CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
</ComponentGroup>
<ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
<JointPositionCtrl/>
</ComponentGroup>
<ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
<JointPositionCtrl/>
</ComponentGroup>
</ControllerManager>
</HumanoidRobot>
<!-- <AuboRobot id="hc02" ip="127.0.0.1" usrname="aubo" password="123456">-->
<!-- </AuboRobot>-->
</Robot>
<BioHead>
</BioHead >
<Microphone>
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
</Microphone>
<Speaker>
<!-- <ffmpegSpeaker id="spk1" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>-->
</Speaker>
</Devices>
<HighLevelController>
<BioHeadExpre headId="bio_head" />
<CartesianWBC urdf="" />
<ScreenTouch robotID="" DexhandID="" />
</HighLevelController>
</DeviceManager>
<MonitorManager>
<DiskMonitor id="file_monitor" freq="1">
<!-- <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
<!-- <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
<!-- <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
<!-- <Folder fileDir="../log" maxVolume="1000"/>-->
</DiskMonitor>
<JointMonitor id="robot_joint_monitor" freq="200">
<RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
<RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
</JointMonitor>
</MonitorManager>
<Server>
<gRPCServer port="50053"/>
</Server>
</CMVR-ES>

114
include/controller/touch_controller.h
View File

@ -1,114 +0,0 @@
//
// Created by lgv on 2025/8/24.
//
#pragma once
#include <utility>
#include "devices/abstract_camera.h"
#include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h"
#include "librealsense2/rs.h"
#include "librealsense2/h/rs_frame.h"
namespace cmvr {
namespace ctrl {
// 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
class PID {
public:
PID(double kp, double ki, double kd,
double i_max,
double output_max_pos, double output_max_neg,
double delta_max = 0.0) // 输出变化最大值，0 表示不限制
: kp_(kp), ki_(ki), kd_(kd),
i_max_(i_max),
output_max_pos_(output_max_pos),
output_max_neg_(output_max_neg),
delta_max_(delta_max),
prev_error_(0), integral_(0), prev_output_(0) {}
double compute(double target, double current, double dt, double deadband = 0.0) {
double error = target - current;
// 死区处理
if (fabs(error) <= deadband) {
error = 0.0;
}
// 积分累加限幅
integral_ += error * dt;
if (integral_ > i_max_) integral_ = i_max_;
if (integral_ < -i_max_) integral_ = -i_max_;
// 微分
double derivative = (error - prev_error_) / dt;
prev_error_ = error;
// PID 输出
double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
// 输出限幅
if (output > output_max_pos_) output = output_max_pos_;
if (output < -output_max_neg_) output = -output_max_neg_;
// 输出斜率限制
if (delta_max_ > 0.0) {
double delta = output - prev_output_;
if (delta > delta_max_) output = prev_output_ + delta_max_;
else if (delta < -delta_max_) output = prev_output_ - delta_max_;
}
prev_output_ = output;
return output;
}
private:
double kp_, ki_, kd_;
double prev_error_;
double integral_;
double i_max_; // 积分限幅
double output_max_pos_; // 向下按的最大输出
double output_max_neg_; // 向上抬的最大输出
double delta_max_; // 输出斜率限制
double prev_output_;
};
class TouchController {
public:
TouchController() {};
TouchController(std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand,std::shared_ptr<device::AbstractCamera> cam)
:robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)),
pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
~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( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
private:
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
std::shared_ptr<device::AbstractCamera> cam_{nullptr};
std::shared_ptr<PID> pid_{nullptr};
const double touch_threshold_ = 5.0; // 触控判定阈值
// 从压阻矩阵提取触控点与压力
bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
};
}
}

8
include/devices/abstract_motor.h
View File

@ -9,7 +9,7 @@
#include "abstract_device.h"
#include "utils/dynamics/joint.h"
#include "motor/motor_protocol_interface.h"
#include "hardware/can/motor_protocol/abstractmotorprotocol.h"
#include <mutex>
namespace cmvr::device{
@ -27,6 +27,8 @@ namespace cmvr::device{
std::string joint_name;
float limitQ;
float limitQd;
std::string protocol_type;
std::string canGroupId;
};
typedef struct {
@ -155,7 +157,7 @@ namespace cmvr::device{
// 使用的通讯协议
virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
virtual void setProtocol(std::shared_ptr<hardware::AbstractMotorProtocol> protocol) {
std::scoped_lock lock(mtx_);
protocol_ = std::move(protocol);
protocol_->initNode(node_id_);
@ -172,7 +174,7 @@ namespace cmvr::device{
MotorInfo info_{};
uint8_t node_id_;
// 使用的通讯协议
std::shared_ptr<MotorProtocolInterface> protocol_;
std::shared_ptr<hardware::AbstractMotorProtocol> protocol_;
};
}
#endif //CMVR_ES_ABSTRACT_MOTOR_H

3
include/devices/abstract_robot.h
View File

@ -11,6 +11,7 @@
#include "utils/solver/qp_solver.h"
#include "cmvr/msgs/geometry.pb.h"
#include "cmvr/msgs/motor.pb.h"
#include "controller/controller_manager.h"
namespace cmvr::device{
@ -122,6 +123,8 @@ namespace cmvr::device{
int dof_{};
RobotState state_{};
std::string toolFrame_;
std::shared_ptr<ControllerManager> controller_manager_;
};
}

24
include/hardware/can/can_group.h
View File

@ -2,26 +2,34 @@
// Created by linbo on 2025/10/22.
//
#ifndef CMVR_ES_CAN_GROUP_H
#define CMVR_ES_CAN_GROUP_H
#pragma once
#include "motor_protocol/motorprotocolmanager.h"
namespace cmvr::device
namespace cmvr::hardware
{
class CanGroup
{
public:
explicit CanGroup(const XmlNode &cfg);
~CanGroup() = default;
private:
void init();
std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
uint8_t getNodeId(const std::string& joint_name);
const MotorProtocolManager::MotorInfo& getMotorInfo(const std::string& joint_name);
private:
XmlNode cfg_;
std::string id_;
unsigned int channelId_;
bool enable_;
std::shared_ptr<MotorProtocolManager> motor_protocol_manager;
std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
};
}
#endif //CMVR_ES_CAN_GROUP_H
}

2
include/hardware/can/can_manager.h
View File

@ -9,7 +9,7 @@
#include "rapidxml/rapidxml.hpp"
#include "can_group.h"
namespace cmvr::device
namespace cmvr::hardware
{
class CanManager
{

44
include/hardware/can/motor_protocol/abstractmotorprotocol.h
View File

@ -5,24 +5,54 @@
#ifndef CMVR_ES_ABSTRACTMOTORPROTOCOL_H
#define CMVR_ES_ABSTRACTMOTORPROTOCOL_H
#include "rapidxml/xml_parser.h"
namespace cmvr::device
#include "cmvr/msgs/canopen.pb.h"
#include "cmvr/msgs/motor.pb.h"
namespace cmvr::hardware
{
class AbstractMotorProtocol
{
public:
enum class CommProto : uint8_t {
CANOPEN = 1,
CUSTOM = 2
};
public:
explicit AbstractMotorProtocol(const XmlNode &cfg)
{
cfg_ = cfg;
id_ = cfg.getAttrString("id");
joint_name_ = cfg.getAttrString("joint_name");
limitQLb_ = cfg.getAttrDefault("limitQLb_",(float)3.14);
limitQUb_ = cfg.getAttrDefault("limitQUb_",(float)3.14);
limitQd = cfg.getAttrDefault("limitQd",(float)3.0);
limitQLb_ = cfg.getAttrDefault("limitQLb_",3.14f);
limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
limitQd = cfg.getAttrDefault("limitQd",3.0f);
}
~AbstractMotorProtocol() = default;
virtual ~AbstractMotorProtocol() = default;
private:
/**
* @brief 用于初始化与通讯协议相关的设置
* @param node_id 电机Id
* @return
*/
virtual bool initNode(uint8_t node_id) = 0;
virtual void setQ(uint8_t node_id, double angle_rad) = 0;
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 0;
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
virtual void setLimitQd(uint8_t node_id,double qd) = 0;
virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
virtual bool calibrateZeroQ(uint8_t node_id) = 0;
virtual bool reachedTargetQ(uint8_t node_id) = 0;
virtual void setQd(uint8_t node_id, double qd) = 0;
virtual void setQdd(uint8_t node_id,double qdd) = 0;
virtual void brake(uint8_t node_id) = 0;
virtual void torqueOff(uint8_t node_id) = 0;
virtual double getQ(uint8_t node_id) = 0;
virtual double getQd(uint8_t node_id) = 0;
CommProto comm_proto{CommProto::CANOPEN};
protected:
XmlNode cfg_;
std::string id_;
std::string joint_name_;

33
include/hardware/can/motor_protocol/motorprotocolmanager.h
View File

@ -8,18 +8,45 @@
#include <unordered_map>
#include <memory>
#include "abstractmotorprotocol.h"
#include "devices/abstract_canbus.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/robot_detail.pb.h"
namespace cmvr::device
namespace cmvr::hardware
{
class MotorProtocolManager
{
public:
explicit MotorProtocolManager(const XmlNode &cfg);
struct MotorInfo
{
uint8_t node_id; //电机ID
std::string jointName; //关节名称
double limitQLb; //逆时针限位
double limitQUb; //顺时针限位
double limitQd; //加速度限制
std::shared_ptr<AbstractMotorProtocol> motorProtocol;
};
public:
explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
~MotorProtocolManager() = default;
std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &joint_name);
uint8_t getNodeId(const std::string& joint_name);
//根据协议类型获取协议实例对象
std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
const MotorInfo& getMotorInfo(const std::string& joint_name);
private:
std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
std::shared_ptr<AbstractMotorProtocol> ti5Protocol_{nullptr};
//协议类型，协议实例对象
std::unordered_map<std::string, std::shared_ptr<AbstractMotorProtocol>> motor_protocols_;
//协议类型，电机参数
std::unordered_map<std::string, std::vector<MotorInfo>> motors_;
};
}

102
include/hardware/can/motor_protocol/ti5motorprotocol.h
View File

@ -2,19 +2,109 @@
// Created by linbo on 2025/10/22.
//
#ifndef CMVR_ES_TI5MOTOR_H
#define CMVR_ES_TI5MOTOR_H
#pragma once
#include "rapidxml/xml_parser.h"
#include "abstractmotorprotocol.h"
namespace cmvr::device{
#include "cmvr/msgs/motor.pb.h"
#include "motor/motor_protocol_interface.h"
#include "devices/abstract_canbus.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/error_code.pb.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "canbus/canopen/sdo_request_protocol.h"
#include "canbus/canopen/sync_protocol.h"
#include "canbus/canopen/nmt_request_protocol.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
#include <cmath>
namespace cmvr::hardware{
class Ti5MotorProtocol final : public AbstractMotorProtocol {
public:
explicit Ti5MotorProtocol(const XmlNode &cfg);
Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
~Ti5MotorProtocol() override;
bool initNode(uint8_t node_id) override;
void setMode(uint8_t node_id, msgs::RunMode mode) override;
void setQ(uint8_t node_id, double angle_rad) override;
void setLimitQ(uint8_t node_id, double ub, double lb) override;
void setLimitQd(uint8_t node_id, double qd) override;
void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
bool calibrateZeroQ(uint8_t node_id) override;
void brake(uint8_t node_id) override;
bool reachedTargetQ(uint8_t node_id) override;
double getQ(uint8_t node_id) override;
double getQd(uint8_t node_id) override;
void setQd(uint8_t node_id, double qd) override;
void setQdd(uint8_t node_id, double qdd) override;
void torqueOff(uint8_t node_id) override;
void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
void configPdo(uint8_t node_id);
std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
return data_ptr;
}
msgs::RunMode getMode(uint8_t node_id) override {
return cur_mode_[node_id];
}
private:
void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
void configTPDO1(uint8_t node_id);
void configTPDO2(uint8_t node_id);
// 目标位置 607A + 控制字 6040
void configRPDO1(uint8_t node_id, bool start);
bool waitUntil(std::function<bool()> condition, int timeout_ms) {
auto start = std::chrono::steady_clock::now();
while (!condition()) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms))
return false;
}
return true;
}
private:
static constexpr double GearRatio = 101.0; // 电机减速比
static constexpr double RADTODEG = 180.0 / M_PI;
std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
// key node_id
std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
std::unordered_map<uint8_t,uint32_t> last_Qd_{};
std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
// nmt
device::NmtRequestProtocol<msgs::RobotDetail> *nmt_command_{nullptr};
//sync
device::SyncProtocol<msgs::RobotDetail> *sync_command_{nullptr};
// sdo
std::map<uint8_t, device::SdoRequestProtocol<msgs::RobotDetail> *> sdo_commands_{};
// rpdo1
std::map<uint8_t, device::motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
};
}
#endif //CMVR_ES_TI5MOTOR_H
}

0
include/hardware/esp32_serial_port.h → include/hardware/serial/RS485/esp32_serial_port.h
View File

2
include/hardware/serial/serial_manager.h
View File

@ -5,7 +5,7 @@
#ifndef CMVR_ES_SERIAL_MANAGER_H
#define CMVR_ES_SERIAL_MANAGER_H
#include "rapidxml/xml_parser.h"
namespace cmvr::device
namespace cmvr::hardware
{
class SerialManager
{

2
include/hardware_manager/hardware_factory.h
View File

@ -7,7 +7,7 @@
#include "rapidxml/xml_parser.h"
#include "hardware/can/can_manager.h"
namespace cmvr::device {
namespace cmvr::hardware {
class HardwareFactory {

2
include/hardware_manager/hardware_manager.h
View File

@ -9,7 +9,7 @@
#include <mutex>
#include "hardware_factory.h"
#include "hardware/can/can_manager.h"
namespace cmvr::device
namespace cmvr::hardware
{
class HardWareManager
{

25
include/service/grpc_service/grpc_hlc_service.h
View File

@ -1,25 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#pragma once
#include "device_manager/device_manager.h"
#include "cmvr/api/hlc_service.grpc.pb.h"
namespace cmvr {
namespace service {
class gRPCHlcServiceImpl final : public api::HlcService::Service {
public:
gRPCHlcServiceImpl();
~gRPCHlcServiceImpl() = default;
grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
private:
device::DeviceManager& dmgr_;
};
}
}

10
include/utils/solver/qp_solver.h
View File

@ -40,7 +40,7 @@ namespace cmvr::ctrl{
// CartesianController declaration (implementation in .cpp)
// -----------------------------------------------------------------------------
template<int DOF>
class CartesianController {
class QPSolver {
public:
using RobotT = cmvr::dyn::Robot<DOF>;
using VecD = Eigen::Vector<double, DOF>;
@ -48,7 +48,7 @@ namespace cmvr::ctrl{
enum class Mode { Position, Velocity, Torque };
CartesianController(std::shared_ptr<RobotT> robot, double dsafe = 0.05, double lambda = 1e-2);
QPSolver(std::shared_ptr<RobotT> robot, double dsafe = 0.05, double lambda = 1e-2);
/* ------------------------- configuration ----------------------------- */
void setCollisionPairs(const std::vector<CollisionPair>& pairs);
@ -110,9 +110,9 @@ namespace cmvr::ctrl{
}
// ------------------------------ explicit instantiation -----------------------
extern template class cmvr::ctrl::CartesianController<7>;
extern template class cmvr::ctrl::CartesianController<14>;
extern template class cmvr::ctrl::CartesianController<20>;
extern template class cmvr::ctrl::QPSolver<7>;
extern template class cmvr::ctrl::QPSolver<14>;
extern template class cmvr::ctrl::QPSolver<20>;
#endif //CMVR_ES_CARTESIAN_CONTROLLER_H

58
src/controller/CMakeLists.txt
View File

@ -1,58 +0,0 @@
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
add_library(controller SHARED
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller.cpp
)
target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(controller PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
)
add_library(cmvr_es::ctrl::controller ALIAS controller)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)
link_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)
add_executable(touch_controller_test
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller_test.cpp
)
target_link_libraries(touch_controller_test
PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
cmvr_es::ctrl::controller
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

159
src/controller/touch_controller.cpp
View File

@ -1,159 +0,0 @@
//
// Created by lgv on 2025/8/24.
//
#include "controller/touch_controller.h"
using namespace cmvr::ctrl;
using namespace cmvr::msgs;
void TouchController::touch(std::shared_ptr<device::AbstractRobot> robot, const msgs::Pose3d pose, const msgs::Pose3d offset) {
msgs::Position target_position;
target_position.set_x(pose.position().x() - offset.position().x());
target_position.set_y(pose.position().y() - offset.position().y());
target_position.set_z(pose.position().z() - offset.position().z());
msgs::Euler target_euler;
target_euler.set_rx(pose.euler().rx() - offset.euler().rx());
target_euler.set_ry(pose.euler().ry() - offset.euler().ry());
target_euler.set_rz(pose.euler().rz() - offset.euler().rz());
msgs::Pose3d target_pose;
*target_pose.mutable_position() = target_position;
*target_pose.mutable_euler() = target_euler;
robot->moveJ("PELVIS_S","R_WRIST_R_S",target_pose);
}
void TouchController::touch(msgs::Pose3d pose, msgs::Pose3d offset, double max_force) {
auto hand_data = hand_->getSensorData();
double force = 0;
int x ,y;
extractTouch(hand_data.index.tip.data,force,x,y);
double dz = pid_->compute(max_force, force, 0.01,10);
std::cout << dz << std::endl;
// LOG(INFO) << "Force : " << force << " dz : " << dz;
// // 输出 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();
// }
}
bool TouchController::extractTouch(const std::vector<std::vector<uint16_t> > &matrix, double &force, int &x, int &y) {
int rows = matrix.size();
int cols = matrix[0].size();
double maxVal = 0;
int maxX = -1, maxY = -1;
double total = 0;
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
double val = matrix[i][j];
total += val;
if (val > maxVal) {
maxVal = val;
maxX = i;
maxY = j;
}
}
}
if (maxVal > touch_threshold_) {
force = total;
x = maxX;
y = maxY;
return true;
}
return false;
}
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;
}
void TouchController::touch(int u, int v, double max_force) {
LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force;
// 获取目标点和当前位姿
auto target_pose = cam_->get3DPointFromPixel(u, v);
auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP");
LOG(INFO) << "Target 3D Pose: " << target_pose;
// 定义关键点位
auto pre_touch_pose = cur_pose;
pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05);
pre_touch_pose.mutable_position()->set_y(target_pose[1]);
pre_touch_pose.mutable_position()->set_z(target_pose[2]);
auto touch_pose = pre_touch_pose;
touch_pose.mutable_position()->set_x(target_pose[0]);
const auto& retreat_pose = cur_pose;
// retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20);
// 1. 移动到预接触位置
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose);
// 2. 向前接触并开启压力监测
robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose);
// 3. 等待达到最大压力
constexpr int k_sleep_ms = 5;
constexpr int k_timeout_ms = 5000;
int elapsed_ms = 0;
while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) {
std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms));
elapsed_ms += k_sleep_ms;
}
if (elapsed_ms >= k_timeout_ms) {
LOG(WARNING) << "Timeout waiting for force feedback!";
} else {
LOG(INFO) << "Max force reached, retreating.";
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose);
}
}

71
src/controller/touch_controller_test.cpp
View File

@ -1,71 +0,0 @@
//
// Created by lgv on 2025/8/27.
//
#include "device_manager/device_manager.h"
#include "gtest/gtest.h"
#include "controller/touch_controller.h"
#include "cmvr/msgs/geometry.pb.h"
using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::ctrl;
TEST(TouchControllerTest,MyTest) {
//
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 1.49969573e-01);
pose.mutable_position()->set_y(-4.00100001e-01);
pose.mutable_position()->set_z(-1.00102800e-01);
pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(1.57);
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;
TouchController controller(robot, hand,cam);
while (true) {
// 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));
}
}

2
src/device_manager/CMakeLists.txt
View File

@ -15,7 +15,7 @@ target_link_libraries(device_manager PRIVATE
cmvr_es::device::rh56dftp_dexhand
cmvr::device::head_esp32
cmvr_es::device::humanoid_robot
# cmvr_es::device::ti5robot
cmvr_es::device::aubo_robot
)
add_library(cmvr_es::device_manager ALIAS device_manager)

7
src/device_manager/device_factory.cpp
View File

@ -11,7 +11,7 @@
#include "camera/realsense_camera/realsense_camera.h"
#include "dexhand/rh56dftp_dexhand/rh56dftp_dexhand.h"
#include "robot/humanoid_robot/humanoid_robot.h"
//#include "robot/ti5_robot/ti5_robot.h"
#include "robot/aubo_robot/aubo_robot.h"
using namespace std;
using namespace cmvr::device;
@ -119,9 +119,12 @@ std::shared_ptr<AbstractMicrophone> DeviceFactory::create_mic_(const XmlNode& cf
std::shared_ptr<AbstractRobot> DeviceFactory::create_robot_(const XmlNode& cfg) {
try {
if (cfg.getNodeName() == "Humanoid") {
if (cfg.getNodeName() == "HumanoidRobot") {
return std::make_shared<HumanoidRobot<14>>(cfg);
}
else if (cfg.getNodeName() == "AuboRobot") {
return std::make_shared<AuboRobot<6>>(cfg);
}
else {
LOG(ERROR) << "[DeviceFactory]: Unsupported device type " << cfg.getNodeName();
return nullptr;

2
src/devices/CMakeLists.txt
View File

@ -6,3 +6,5 @@ add_subdirectory(biohead)
add_subdirectory(robot)
add_subdirectory(canbus)
add_subdirectory(motor)
add_subdirectory(controller)
add_subdirectory(solver)

2
src/devices/biohead/biohead_esp32/CMakeLists.txt
View File

@ -11,7 +11,7 @@ find_package(ALSA REQUIRED)
add_library(cmvr::device::head_esp32 ALIAS head_esp32)
# 链接依赖库
target_link_libraries(head_esp32 PRIVATE ALSA::ALSA )
target_link_libraries(head_esp32 PRIVATE ALSA::ALSA cmvr_es::hardware::serial)

2
src/devices/biohead/biohead_esp32/biohead_esp32.h
View File

@ -2,7 +2,7 @@
#define CMVR_ES_BIOHEAD_ESP32_H
#include "devices/abstract_biohead.h"
#include "hardware/esp32_serial_port.h"
#include "../../../../include/hardware/serial/RS485/esp32_serial_port.h"
#include <vector>
#include <string>
#include <memory>

1
src/devices/canbus/canopen/nmt_request_protocol.h
View File

@ -7,6 +7,7 @@
* @file
* @brief 通过发送nmt 命令去控制 节点状态
*/
#pragma once
#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"

1
src/devices/canbus/canopen/sync_protocol.h
View File

@ -1,6 +1,7 @@
//
// Created by lgv on 2025/7/31.
//
#pragma once
#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"

10
src/devices/controller/CMakeLists.txt Normal file
View File

@ -0,0 +1,10 @@
add_library(controller_manager SHARED
jointpositioncontroller.cpp
cartesiancontroller.cpp
controller_manager.cpp
)
target_include_directories(controller_manager PUBLIC .)
target_link_libraries(controller_manager PRIVATE jsoncpp_lib)
add_library(cmvr_es::device::controller_manager ALIAS controller_manager)

44
src/devices/controller/abstractcontroller.h Normal file
View File

@ -0,0 +1,44 @@
//
// Created by linbo on 2025/10/23.
//
#pragma once
#include <curl/curl.h>
#include "rapidxml/xml_parser.h"
#include "jsoncpp/json/json.h"
namespace cmvr::device
{
enum ControllerState
{
ControllerState_Idle, //默认
ControllerState_Executing, //正在执行命令
ControllerState_Switching //命令被中断，正在切换
};
class AbstractController
{
public:
explicit AbstractController(const XmlNode& cfg){}
virtual ~AbstractController(){}
[[nodiscard]] ControllerState getState() const {return state_;}
//此处的Json中应该包含目标电机信息，电机id，canGroupId等其他必要参数，用以确定是调用哪个can实例发送消息
// 还要包含操作内容，比如要执行的是直接控制每个电机位置，还是指定末端关节位置
/*
"params":{“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]},
"operate":{}
**/
virtual void call(const Json::Value& json) = 0;
virtual void interrupt() = 0;
virtual void stop() = 0;
protected:
ControllerState state_;
double defaultSpeed_;
double defaultAcc_;
};
}

80
src/devices/controller/cartesiancontroller.cpp Normal file
View File

@ -0,0 +1,80 @@
//
// Created by linbo on 2025/10/24.
//
#include "cartesiancontroller.h"
using namespace std;
using namespace cmvr::device;
CartesianController::CartesianController(const XmlNode& cfg):AbstractController(cfg)
{
try
{
defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f);
defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f);
base_link_ = cfg.getAttrString("base_link");
ee_link_ = cfg.getAttrString("ee_link");
}
catch(const exception& e)
{
}
}
void CartesianController::call(const Json::Value& json)
{
try
{
if (state_ != ControllerState_Idle)
return;
//检测是否传入了base_link和ee_link，如果未指定，则使用成员变量中默认的值
std::string baseLink = base_link_;
std::string eeLink = ee_link_;
if (json.isMember("params"))
{
std::vector<std::string> canGroupIds;
const Json::Value& params = json["params"];
if (params.isMember("base_link"))
{
baseLink = params["base_link"].asString();
}
if (params.isMember("eeLink"))
{
eeLink = params["eeLink"].asString();
}
//通过canGroupId去获取当前需要控制的电机的实际位置
if (params.isMember("canGroupId"))
{
for (const auto & canGroupId : params["canGroupId"])
{
canGroupIds.emplace_back(canGroupId.asString());
}
}
}
state_ = ControllerState_Executing;
}
catch(const std::exception& e)
{
}
}
void CartesianController::interrupt()
{
state_ = ControllerState_Switching;
state_ = ControllerState_Idle;
}
void CartesianController::stop()
{
state_ = ControllerState_Idle;
}

22
src/devices/controller/cartesiancontroller.h Normal file
View File

@ -0,0 +1,22 @@
//
// Created by linbo on 2025/10/24.
//
#pragma once
#include "abstractcontroller.h"
#include "utils/solver/qp_solver.h"
namespace cmvr::device
{
class CartesianController final:public AbstractController
{
public:
explicit CartesianController(const XmlNode& cfg);
void call(const Json::Value& json) override;
void interrupt() override;
void stop() override;
private:
std::string base_link_;
std::string ee_link_;
};
}

107
src/devices/controller/controller_manager.cpp Normal file
View File

@ -0,0 +1,107 @@
//
// Created by linbo on 2025/10/23.
//
#include "controller_manager.h"
#include "jointpositioncontroller.h"
#include "cartesiancontroller.h"
using namespace std;
using namespace cmvr::device;
ControllerManager::ControllerManager(const XmlNode& cfg):state_(ControlManagerState_Idle)
{
create(cfg);
}
void ControllerManager::clearError()
{
state_ = ControlManagerState_Idle;
}
void ControllerManager::create(const XmlNode& cfg)
{
auto children = cfg.getChildren();
for (auto& child : children)
{
if (child.getNodeName() == "ComponentGroup")
{
ComponentGroup group;
group.id = child.getAttrString("id");
// CanGroupID可能记录了多个canGroupID
std::string canGroupIDs = child.getAttrString("CanGroupID");
std::istringstream iss(canGroupIDs);
std::string id;
while (std::getline(iss, id, ',')) {
group.CanGroupIDs.push_back(id);
}
auto controller_nodes = child.getChildren();
for (auto& controller_node : controller_nodes)
{
if (controller_node.getNodeName() == "JointPositionCtrl")
{
group.controllers_["JointPositionCtrl"] = std::make_shared<JointPositionController>(controller_node);
}
else if (controller_node.getNodeName() == "CartesianController")
{
group.controllers_["CartesianController"] = std::make_shared<CartesianController>(controller_node);
}
}
//
componentGroups_[group.id] = group;
}
}
}
void ControllerManager::switchMode(ControlManagerState state)
{
//无论当前切换成哪种模式都应该停止控制器操作？
if (activeController_)
{
activeController_->stop();
}
//切换当前状态
switch (state)
{
case ControlManagerState_Idle:
{
break;
}
case ControlManagerState_Command:
{
break;
}
case ControlManagerState_MinorFault:
{
break;
}
case ControlManagerState_MajorFault:
{
break;
}
case ControlManagerState_Teach:
{
break;
}
case ControlManagerState_EStop:
{
break;
}
default:
break;
}
state_ = state;
}
ComponentGroup& ControllerManager::getComponentGroup(const std::string& id)
{
if (componentGroups_.count(id))
return componentGroups_[id];
throw std::runtime_error("ComponentGroup not found");
}
std::shared_ptr<AbstractController> ControllerManager::getActiveController()
{
return activeController_;
}

54
src/devices/controller/controller_manager.h Normal file
View File

@ -0,0 +1,54 @@
//
// Created by linbo on 2025/10/23.
//
#pragma once
#include "abstractcontroller.h"
namespace cmvr::device
{
enum ControlManagerState
{
ControlManagerState_Idle, //默认
ControlManagerState_Command, //指令模式
ControlManagerState_MinorFault, //次要错误，可以继续运行
ControlManagerState_MajorFault, //主要错误，不能继续运行
ControlManagerState_Teach, //示教模式
ControlManagerState_EStop //急停模式
};
struct ComponentGroup
{
std::string id;
std::vector<std::string> CanGroupIDs;
std::unordered_map<std::string, std::shared_ptr<AbstractController>> controllers_;
};
class ControllerManager
{
public:
explicit ControllerManager(const XmlNode& cfg);
~ControllerManager() = default;
[[nodiscard]] ControlManagerState getSate() const {return state_;}
[[nodiscard]] ComponentGroup& getComponentGroup(const std::string& id);
// 清除错误信息
void clearError();
// 根据xml内容创建控制器组
void create(const XmlNode& cfg);
void switchMode(ControlManagerState state);
std::shared_ptr<AbstractController> getActiveController();
private:
ControlManagerState state_;
std::unordered_map<std::string,ComponentGroup> componentGroups_;
std::shared_ptr<AbstractController> activeController_{nullptr};//当前活动控制器
};
}

110
src/devices/controller/jointpositioncontroller.cpp Normal file
View File

@ -0,0 +1,110 @@
//
// Created by linbo on 2025/10/24.
//
#include "jointpositioncontroller.h"
#include "hardware/can/can_manager.h"
#include "hardware_manager/hardware_manager.h"
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg)
{
defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f);
defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f);
}
void JointPositionController::call(const Json::Value& json)
{
try
{
if (state_ != ControllerState_Idle)
return;
//解析json
/*
* 直接设置位置
"params":{
"canGroupId":["leftArm","rightArm"],
"motors":[{"joint_name":"","position":0.01,"velocity"：0.8}]
},
**/
std::string protocolType;
if (json.isMember("params"))
{
std::vector<std::string> canGroupIds;
const Json::Value& params = json["params"];
if (params.isMember("canGroupId"))
{
for (const auto & canGroupId : params["canGroupId"])
{
canGroupIds.emplace_back(canGroupId.asString());
}
}
//先把所有电机信息都找到
if (params.isMember("motors"))
{
for (int i = 0; i < params["motors"].size(); i++)
{
const Json::Value& motorJson = params["motors"][i];
std::string joint_name = motorJson["joint_name"].asString();
double pos = motorJson["position"].asDouble();
double vel = motorJson["velocity"].asDouble();
for (const auto& canGroupId : canGroupIds)
{
try
{
auto canGroup = HardWareManager::getInstance().getCanGroup(canGroupId);
auto motorInfo = canGroup->getMotorInfo(joint_name);
motorInfo.motorProtocol->setMode(motorInfo.node_id, msgs::RUN_MODE_PROFILE_POSITION);
motorInfo.motorProtocol->setQd(motorInfo.node_id, vel);
//考虑限位
if (pos > motorInfo.limitQUb)
{
pos = motorInfo.limitQUb;
LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name
<< "] target position exceeds upper limit! Current target: " << pos
<< ", upper limit: " << motorInfo.limitQUb;
}
else if (pos < motorInfo.limitQLb) // Assume lower limit field exists (limitQLb)
{
pos = motorInfo.limitQLb;
LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name
<< "] target position exceeds lower limit! Current target: " << pos
<< ", lower limit: " << motorInfo.limitQLb;
}
motorInfo.motorProtocol->setQ(motorInfo.node_id, pos);
break;
}
catch (const std::exception& e)
{
continue; // 捕获异常，继续尝试下一个 canGroup
}
}
}
}
}
state_ = ControllerState_Executing;
}
catch (const std::exception& e)
{
LOG(ERROR)<<"[JointPositionController](call):" <<e.what();
throw runtime_error(e.what());
}
}
void JointPositionController::interrupt()
{
state_ = ControllerState_Switching;
state_ = ControllerState_Idle;
}
void JointPositionController::stop()
{
state_ = ControllerState_Idle;
}

19
src/devices/controller/jointpositioncontroller.h Normal file
View File

@ -0,0 +1,19 @@
//
// Created by linbo on 2025/10/24.
//
#pragma once
#include "abstractcontroller.h"
namespace cmvr::device
{
class JointPositionController final:public AbstractController
{
public:
explicit JointPositionController(const XmlNode& cfg);
void call(const Json::Value& json) override;
void interrupt() override;
void stop() override;
private:
};
}

2
src/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt
View File

@ -4,4 +4,4 @@ target_include_directories(rh56dftp_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand)
target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware -lmodbus)
target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware_manager -lmodbus)

55
src/devices/dexhand/rh56dftp_dexhand/rh56dftp_dexhand.cpp
View File

@ -234,7 +234,32 @@ void RH56DFTPDexhand::init() {
LOG(INFO)<< "[RH56DFTPDexhand](init) sucess, id="<< id_;
}
std::vector<std::string> parse_error(unsigned char errorCode) {
std::vector<std::string> faults;
// 直接判断每一位是否为1
if (errorCode & (1 << 0)) { // Bit0: 堵转故障
faults.emplace_back("堵转故障");
}
if (errorCode & (1 << 1)) { // Bit1: 过温故障
faults.emplace_back("过温故障");
}
if (errorCode & (1 << 2)) { // Bit2: 过流故障
faults.emplace_back("过流故障");
}
if (errorCode & (1 << 3)) { // Bit3: 电机异常
faults.emplace_back("电机异常");
}
if (errorCode & (1 << 4)) { // Bit4: 通讯故障
faults.emplace_back("通讯故障");
}
return faults;
}
void RH56DFTPDexhand::updateState() {
//读取数据，初始化
std::vector<int> parsed_values;
@ -320,36 +345,6 @@ void RH56DFTPDexhand::updateState() {
}
}
}
void printPinkySensorData(const HandTactileSensors& sensors) {
// 打印小拇指指端数据
std::cout << "=== 小拇指指端数据 (" << sensors.pinky.tip.rows << "x" << sensors.pinky.tip.cols << ") ===\n";
for (const auto& row : sensors.pinky.tip.data) {
for (TactilePoint value : row) {
std::cout << std::setw(5) << value; // 每个值占5个字符宽度
}
std::cout << "\n";
}
std::cout << "\n";
// 打印小拇指指尖数据
std::cout << "=== 小拇指指尖数据 (" << sensors.pinky.finger.rows << "x" << sensors.pinky.finger.cols << ") ===\n";
for (const auto& row : sensors.pinky.finger.data) {
for (TactilePoint value : row) {
std::cout << std::setw(5) << value;
}
std::cout << "\n";
}
std::cout << "\n";
// 打印小拇指指腹数据
std::cout << "=== 小拇指指腹数据 (" << sensors.pinky.pad.rows << "x" << sensors.pinky.pad.cols << ") ===\n";
for (const auto& row : sensors.pinky.pad.data) {
for (TactilePoint value : row) {
std::cout << std::setw(5) << value;
}
std::cout << "\n";
}
}
void RH56DFTPDexhand::updateSensorData() {
//触觉传感器数据。

5
src/devices/dexhand/rh56dftp_dexhand/rh56dftp_dexhand.h
View File

@ -11,9 +11,8 @@
#include <condition_variable>
#include <modbus/modbus.h>
#include <boost/lockfree/spsc_queue.hpp>
#include "../../../../include/utils/base/os.h"
#include "utils/base/os.h"
#include "devices/abstract_dexhand.h"
#include "hardware/serial_interface.h"
namespace cmvr::device {
@ -67,8 +66,6 @@ namespace cmvr::device {
int default_force_;//上电的力控阈值
int default_speed_;//上电的自由角转动速度
std::shared_ptr<serial_interface> serial_;
std::string ip_address_;
int port_;
std::shared_ptr<ModbusController> controller_;

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

@ -1,11 +1,12 @@
//
// Created by lgv on 2025/8/1.
//
#pragma once
#include "devices/abstract_motor.h"
#include "motor/motor_protocol_interface.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "hardware/can/motor_protocol/motorprotocolmanager.h"
#include "hardware/can/motor_protocol/ti5motorprotocol.h"
#include "hardware_manager/hardware_manager.h"
/**
* class: Ti5Motor 完成电机硬件的相关工作，软件相关的在protocol 中完成
*/
@ -14,12 +15,14 @@ namespace cmvr {
class Ti5Motor : public AbstractMotor {
public:
Ti5Motor(const XmlNode &config,uint8_t node_id):AbstractMotor(config,node_id){}
Ti5Motor(const XmlNode &config):AbstractMotor(config) {
explicit Ti5Motor(const XmlNode &config):AbstractMotor(config) {
info_.id = config.getAttrDefault("id", -1);
info_.joint_name = config.getAttrString("jointName");
info_.limitQ = config.getAttrDefault("limitQ", 3.14f);
info_.limitQd = config.getAttrDefault("limitQd", 0.5f);
node_id_ = info_.id;
info_.protocol_type = config.getAttrString("protocolType");
info_.canGroupId = config.getAttrString("canGroupId");
}
/**
@ -27,10 +30,11 @@ namespace cmvr {
*/
void init() override {
if (!protocol_) {
throw std::runtime_error("Protocol not set for motor");
const auto canGroup = hardware::HardWareManager::getInstance().getCanGroup(info_.canGroupId);
protocol_ = canGroup->getMotorProtocol(info_.protocol_type);
}
if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
if (protocol_->comm_proto == hardware::AbstractMotorProtocol::CommProto::CANOPEN ) {
auto canopen_protocol = std::dynamic_pointer_cast<cmvr::hardware::Ti5MotorProtocol>(protocol_);
canopen_protocol->configPdo(node_id_);
canopen_protocol->configProfile(node_id_,2000,2000,2000);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);

4
src/devices/robot/CMakeLists.txt
View File

@ -1,4 +1,2 @@
#add_subdirectory(ti5_robot)
add_subdirectory(humanoid_robot)
#add_subdirectory(c701)
add_subdirectory(controller)
add_subdirectory(aubo_robot)

34
src/devices/robot/aubo_robot/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
# 第一步：先定义 Aubo SDK 的路径（必须在 add_library 之前！）
if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows")
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/include)
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/lib)
endif()
# 第二步：创建 aubo_robot 库（此时 SDK 路径已经定义好了）
add_library(aubo_robot SHARED aubo_robot.cpp)
# 第三步：给 aubo_robot 配置头文件路径（合并你原来的 ${CMAKE_CURRENT_SOURCE_DIR} 和 SDK 的 include 目录）
target_include_directories(aubo_robot
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR} # 你原来的路径（如果需要的话）
${AUBO_SDK_INCLUDE_DIR} # SDK 头文件路径（关键！）
)
# 第四步：给 aubo_robot 配置库文件路径（精准绑定到目标，比 link_directories 靠谱）
target_link_directories(aubo_robot
PRIVATE
${AUBO_SDK_LIB_DIR} # SDK 库文件路径（关键！）
)
# 第五步：链接 SDK 库（和你原来一致，但现在路径已经生效）
target_link_libraries(aubo_robot
PRIVATE
aubo_sdk
robot_proxy
)
# 别名（不变）
add_library(cmvr_es::device::aubo_robot ALIAS aubo_robot)

269
src/devices/robot/aubo_robot/aubo_robot.cpp Normal file
View File

@ -0,0 +1,269 @@
//
// Created by linbo on 2025/11/7.
//
#include "aubo_robot.h"
using namespace std;
using namespace cmvr::device;
template<int DOF>
AuboRobot<DOF>::AuboRobot(const XmlNode &cfg) : AbstractRobot(cfg)
{
try {
id_ = cfg.getAttrString("id");
ip_ = cfg.getAttrString("ip");
port_ = cfg.getAttrString("port");
} catch (std::exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF>
AuboRobot<DOF>::~AuboRobot()
{
if (rpc_cli_)
{
// 接口调用: 退出登录
rpc_cli_->logout();
// 接口调用: 断开连接
rpc_cli_->disconnect();
}
}
template<int DOF>
void AuboRobot<DOF>::init ()
{
//初始化AuboSDK
rpc_cli_ = std::make_shared<RpcClient>();
// 接口调用: 设置 RPC 超时
rpc_cli_->setRequestTimeout(1000);
// 接口调用: 连接到 RPC 服务
rpc_cli_->connect(ip_, 30004);
// 接口调用: 登录
rpc_cli_->login("aubo", "123456");
}
template<int DOF>
void AuboRobot<DOF>::waitForRobotMode(const RobotInterfacePtr& robot_interface,
const RobotModeType target_mode)
{
// 接口调用: 获取当前机械臂的模式
auto current_mode = robot_interface->getRobotState()->getRobotModeType();
while (current_mode != target_mode) {
std::cout << "机械臂当前模式:" << current_mode << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
current_mode = robot_interface->getRobotState()->getRobotModeType();
}
}
template<int DOF>
void AuboRobot<DOF>::torqueOn()
{
// 接口调用: 获取机器人的名字
auto robot_name = rpc_cli_->getRobotNames().front();
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
// 接口调用: 设置负载
double mass = 0.0;
std::vector<double> cog(3, 0.0);
std::vector<double> aom(3, 0.0);
std::vector<double> inertia(6, 0.0);
robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
// 接口调用: 获取机械臂当前模式
auto robot_mode = robot_interface->getRobotState()->getRobotModeType();
if (robot_mode == RobotModeType::Running) {
std::cout << "机械臂已松刹车，处于运行模式" << std::endl;
} else {
// 接口调用: 机械臂发起上电请求
robot_interface->getRobotManage()->poweron();
// 等待机械臂进入空闲模式
waitForRobotMode(robot_interface, RobotModeType::Idle);
std::cout << "机械臂上电成功，当前模式:"
<< robot_interface->getRobotState()->getRobotModeType()
<< std::endl;
// 接口调用: 机械臂发起松刹车请求
rpc_cli_->getRobotInterface(robot_name)->getRobotManage()->startup();
// 等待机械臂进入运行模式
waitForRobotMode(robot_interface, RobotModeType::Running);
std::cout << "机械臂松刹车成功，当前模式:"
<< robot_interface->getRobotState()->getRobotModeType()
<< std::endl;
}
}
template<int DOF>
void AuboRobot<DOF>::getJointsState(std::vector<JointState>& states)
{
try {
// 接口调用: 获取机器人的名字
auto robot_name = rpc_cli_->getRobotNames().front();
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
auto rebotState = robot_interface->getRobotState();
states.clear();
JointState state;
//获取关节位置和速度
for (int i = 0; i < 6; i++) {
state.velocity = rebotState->getJointSpeeds()[i];
state.position = rebotState->getJointPositions()[i];
states.push_back(state);
}
} catch (exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF>
void AuboRobot<DOF>::torqueOff()
{
// 接口调用: 获取机器人的名字
auto robot_name = rpc_cli_->getRobotNames().front();
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
// 接口调用: 机械臂断电
robot_interface->getRobotManage()->poweroff();
// 等待机械臂进入断电模式
waitForRobotMode(robot_interface, RobotModeType::PowerOff);
std::cout << "机械臂断电成功，当前模式:"
<< robot_interface->getRobotState()->getRobotModeType()
<< std::endl;
}
template<int DOF>
void AuboRobot<DOF>::moveJ(std::vector<JointPoint>& cmd, double vel, double acc)
{
try
{
//检查参数
if (cmd.size() != dof_)
{
LOG(ERROR) << "[AuboRobot](moveJ)Wrong number of cmd";
throw std::invalid_argument("[AuboRobot](moveJ)Wrong number of cmd");
}
//调用aubo sdk接口实现moveJ
// 接口调用: 获取机器人的名字
auto robot_name = rpc_cli_->getRobotNames().front();
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
// 接口调用: 设置机械臂的速度比率
robot_interface->getMotionControl()->setSpeedFraction(0.3);
/*
* 1、严格遵守公司安全规章制度，严格遵守相关的国家安全规章制度，发现隐患及时上报
* 2、工作前按规定佩戴安全保护工具
* 3、定期检查存在安全隐患的设备
* 4、积极参与公司组织的安全培训
* 5、在进行危险工作过程中相互监查，安全作业
*/
// 接口调用: 关节运动
// 关节角，单位: 弧度 (此处为6关节)
std::vector<double> joint_angle;
for (int i = 0; i < cmd.size(); i++)
{
joint_angle.emplace_back(cmd[i].rad);
}
robot_interface->getMotionControl()->moveJoint(
joint_angle, acc, vel, 0, 0);
}
catch (std::exception& e)
{
throw std::invalid_argument(e.what());
}
}
template<int DOF>
void AuboRobot<DOF>::calibrateZeroQ(const std::string& joint_name)
{
}
template<int DOF>
cmvr::msgs::Pose3d AuboRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link)
{
cmvr::msgs::Pose3d pose;
return pose;
}
template<int DOF>
std::vector<double> AuboRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose)
{
std::vector<double> ik;
return ik;
}
// 实现阻塞功能: 当机械臂运动到目标路点时，程序再往下执行
int waitArrival(RobotInterfacePtr impl) {
const int max_retry_count = 5;
int cnt = 0;
// 接口调用: 获取当前的运动指令 ID
int exec_id = impl->getMotionControl()->getExecId();
// 等待机械臂开始运动
while (exec_id == -1) {
if (cnt++ > max_retry_count) {
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
exec_id = impl->getMotionControl()->getExecId();
}
// 等待机械臂动作完成
while (impl->getMotionControl()->getExecId() != -1) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
return 0;
}
template <int DOF>
void AuboRobot<DOF>::moveL(math::Pose3d& pose, double vel, double acc)
{
// 接口调用: 获取机器人的名字
auto robot_name = rpc_cli_->getRobotNames().front();
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
// 接口调用: 设置机械臂的速度比率
robot_interface->getMotionControl()->setSpeedFraction(0.75);
// 接口调用: 设置工具中心点（TCP相对于法兰盘中心的偏移）
std::vector<double> tcp_offset(6, 0.0);
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
// 接口调用: 直线运动到位置
std::vector<double> pose1;
pose1.emplace_back(pose.position.x);
pose1.emplace_back(pose.position.y);
pose1.emplace_back(pose.position.z);
pose1.emplace_back(pose.euler.rx);
pose1.emplace_back(pose.euler.ry);
pose1.emplace_back(pose.euler.rz);
robot_interface->getMotionControl()->moveLine(pose1, acc, vel, 0.025, 0);
// 阻塞
auto ret = waitArrival(robot_interface);
if (ret == 0) {
LOG(INFO)<<"直线运动到位置成功！";
} else {
LOG(INFO)<<"直线运动到位置失败！";
}
}
template class cmvr::device::AuboRobot<6>;

46
src/devices/robot/aubo_robot/aubo_robot.h Normal file
View File

@ -0,0 +1,46 @@
//
// Created by linbo on 2025/11/7.
//
#ifndef CMVR_ES_AUBO_ROBOT_H
#define CMVR_ES_AUBO_ROBOT_H
#include "devices/abstract_robot.h"
#include "aubo_sdk/rpc.h"
#include "cmvr/msgs/robot_detail.pb.h"
using namespace arcs::common_interface;
using namespace arcs::aubo_sdk;
namespace cmvr::device
{
template<int DOF>
class AuboRobot: public AbstractRobot {
public:
explicit AuboRobot(const XmlNode& cfg);
~AuboRobot();
void init () override;
void torqueOn() override;
void torqueOff() override;
void getJointsState(std::vector<JointState>& states) override;
void moveJ(std::vector<JointPoint>& cmd, double vel, double acc) override;
void moveL(math::Pose3d& pose, double vel, double acc) override;
void calibrateZeroQ(const std::string& joint_name) override;
msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override;
std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override;
private:
static void waitForRobotMode(const RobotInterfacePtr& robot_interface,
const RobotModeType target_mode);
private:
std::string ip_;
std::string port_;
std::shared_ptr<RpcClient> rpc_cli_;
};
}
#endif //CMVR_ES_AUBO_ROBOT_H

54
src/devices/robot/c701/CMakeLists.txt
View File

@ -1,54 +0,0 @@
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
add_library(c701 SHARED
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller.cpp
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_sdo_response.cpp
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo1.cpp
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo2.cpp
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_rpdo1.cpp)
target_include_directories(c701 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::robot::c701 ALIAS c701)
target_link_libraries(c701
PRIVATE
cmvr_es::device::canbus
protobuf::libprotobuf
glog::glog
)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
include_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)
link_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)
add_executable(motor_controller_test
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller_test.cpp
)
target_link_libraries(motor_controller_test
PRIVATE
cmvr_es::device::canbus
cmvr_es::robot::c701
c701
gtest
gtest_main
pthread
glog::glog
proto-objects
)

400
src/devices/robot/c701/motor/motor_controller.cpp
View File

@ -1,400 +0,0 @@
//
// Created by lgv on 2025/7/17.
//
#include "robot/c701/motor/motor_controller.h"
#include "robot/c701/motor/protocol/ti5_motor_sdo_response.h"
#include "canbus/canopen/register.h"
#include "robot/c701/motor/protocol/ti5_motor_tpdo1.h"
#include "robot/c701/motor/protocol/ti5_motor_tpdo2.h"
using namespace cmvr::robot::c701;
using namespace cmvr::robot::motor;
using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::msgs;
ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool enable_log) {
can_client->init();
can_client->start();
auto ret = ErrorCode::OK;
// 初始化 message_manager_
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
// NMT
message_manager_->AddSendProtocolData<NmtRequestProtocol<RobotDetail>, false>();
//sync
message_manager_->AddSendProtocolData<SyncProtocol<RobotDetail>, false>();
for (const auto node_id: node_ids_) {
//nmt
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
//sdo
message_manager_->AddSendProtocolData<SdoRequestProtocol<RobotDetail>, false>(node_id);
message_manager_->AddRecvProtocolData<Ti5MotorSdoResponse, false>(node_id);
//TPDO
message_manager_->AddRecvProtocolData<Ti5MotorTPDO1,false>(node_id);
message_manager_->AddRecvProtocolData<Ti5MotorTPDO2,false>(node_id);
//RPDO
message_manager_->AddSendProtocolData<Ti5MotorRPDO1,false>(node_id);
}
// 初始化 sender
can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
ret = can_sender_->Init(can_client, enable_log);
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to init can sender.";
return ret;
}
// 初始化 receiver
can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
ret = can_receiver_->Init(can_client, message_manager_.get(), enable_log);
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to init can receiver.";
return ret;
}
// nmt
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
if (nmt_command_ == nullptr) {
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
// sync
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
if (sync_command_ == nullptr) {
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
// can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
// sdo
for (auto node_id: node_ids_) {
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
if (sdo_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
}
// pdo1
for (auto node_id: node_ids_) {
rpdo1_commands_[node_id] = dynamic_cast<Ti5MotorRPDO1 *>(
message_manager_->GetMutableProtocolDataById(Ti5MotorRPDO1::ID(node_id)));
if (rpdo1_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
}
// need sleep to ensure all messages received
LOG(INFO) << "Motor Controller is initialized.";
return ErrorCode::OK;
}
ErrorCode MotorController::Start() {
auto ret = ErrorCode::OK;
ret = can_sender_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can sender.";
return ret;
}
ret = can_receiver_->Start();
if (ret != ErrorCode::OK) {
LOG(ERROR) << "Failed to start can receiver.";
return ret;
}
return ret;
}
void MotorController::SetMode(uint8_t node_id, msgs::RunMode mode) {
cur_mode_ = mode;
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,OPERATION_MODE_6060,SUB_INDEX_0,data);
// 2 : 状态机步进 —— Shutdown（0x06）
controlword_t cw = {};
cw.quick_stop = 1;
cw.enable_voltage = 1;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
cw.switch_on = 1;
cw.enable_operation = 1;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
switch (mode) {
case RUN_MODE_PROFILE_POSITION: {
// 4 : 设置目标位置（为当前位置）
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
// 5 : 触发位置运动（new_set_point 翻转）
cw.new_set_point = 1;
cw.change_set_immediately = 1;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
cw.new_set_point = 0;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_CYCLIC_SYNC_POSITION: {
// 设置目标位置为当前位置
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
//3 : 使能 15
cw.enable_operation = 1;
cw.switch_on = 1;
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CONTROL_WORD_6040,SUB_INDEX_0,cw.value);
break;
}
default:
// TODO: Handle unspecified or unknown mode
break;
}
}
void MotorController::ConfigTPDO1(uint8_t node_id) {
//TDPO1 配置 状态字 和 控制字
// 1: 失能 pdo
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (1U << 31));
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,0);
// 2: 配置为异步
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_COMM_1800,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
// 3：配置约束时间 unit:0.1ms
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_3,1);
// 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_5,1000);
// 5 ：映射控制字
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_1,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
//6 : 映射状态字
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_2,STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
//7 : 映射模式
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_3,MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
//8 映射错误码
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_4,ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
//9 写入该PDO映射对象总个数
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,4);
//10 使能
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (0U << 31));
}
void MotorController::ConfigTPDO2(uint8_t node_id) {
// 1: 失能 pdo
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (1U << 31));
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,0);
// 2: 配置为异步
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_COMM_1801,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
// 3：配置约束时间 unit:0.1ms
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_3,10000);
// 4 : 配置周期发送时间 unit : ms
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_5,1000);
// 5 ：映射当前位置
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_1,ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射当前速度
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_2,ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
//9 写入该PDO映射对象总个数
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,2);
//10 使能
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (0U << 31));
}
void MotorController::ConfigRPDO1(uint8_t node_id,bool start) {
// 1: 失能 pdo
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (1U << 31));
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,0);
// 2: 配置为
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_COMM_1400,SUB_INDEX_2,SYNC_EVENT_DRIVEN);
// // 3：配置约束时间 unit:0.1ms
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
//
// // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
// 5 ：映射位置
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_1,TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射控制字
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_2,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
if (start) {
//7 写入该PDO映射对象总个数
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,2);
//8 使能
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (0U << 31));
}
}
void MotorController::SetPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
controlword_t cw = {};
cw.switch_on = 1;
cw.enable_voltage = 1;
cw.enable_operation = 1;
cw.quick_stop = 1;
cw.change_set_immediately = 1;
// 1. 设置目标位置
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
// 2. 设置触发位（bit4 = 1）
cw.new_set_point = 1;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 3. 清除触发位（bit4 = 0），准备下一次触发
cw.new_set_point = 0;
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
}
void MotorController::SetPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
// 触发目标位置运动
controlword_t cw;
cw.value = 0x0F;
cw.new_set_point = 1;
cw.change_set_immediately = 1;
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
cw.new_set_point = 0;
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void MotorController::SetTargetPosition(uint8_t node_id, double angle_rad) {
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (cur_mode_) {
case RUN_MODE_CYCLIC_SYNC_POSITION:
SetCSPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
break;
case RUN_MODE_PROFILE_POSITION:
SetPPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
// SetPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
break;
}
}
void MotorController::ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
}
void MotorController::SetCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void MotorController::SeedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
nmt_command_->RequestService(node_id,command);
can_sender_->Update(nmt_command_->ID);
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void MotorController::SeedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms) {
sdo_commands_[node_id]->SetFrameData(cs,index,sub_index,data);
can_sender_->Update(sdo_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}

101
src/devices/robot/c701/motor/motor_controller.h
View File

@ -1,101 +0,0 @@
//
// Created by lgv on 2025/7/17.
//
#pragma once
#include <utility>
#include "canbus/can_client/socket/socket_can_client_raw.h"
#include "can_comm/can_sender.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/error_code.pb.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "canbus/canopen/sdo_request_protocol.h"
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
#include "canbus/canopen/sync_protocol.h"
#include "canbus/canopen/nmt_response_protocol.h"
#include "canopen/nmt_request_protocol.h"
namespace cmvr {
namespace robot {
namespace c701 {
class MotorController {
public:
MotorController(std::vector<uint8_t> node_ids):node_ids_(node_ids) {}
MotorController(const MotorController &) = delete;
MotorController &operator=(const MotorController &) = delete;
/**
* @brief Destructor.
*/
virtual ~MotorController() = default;
msgs::ErrorCode Init(cmvr::device::AbstractCanbus *can_client,bool enable_log);
msgs::ErrorCode Start();
std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
return data_ptr;
}
void SeedNmtRequest(uint8_t node_id,msgs::NmtCommand command,uint32_t delay_ms = 10);
void SeedSdoRequest(uint8_t node_id,msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data,uint32_t delay_ms = 10);
// 设置软件限位（上限和下限）
void SetPositionLimits(uint8_t node_id, int32_t lower_limit, int32_t upper_limit);
void ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
void ConfigTPDO1(uint8_t node_id);
void ConfigTPDO2(uint8_t node_id);
// 目标位置 607A + 控制字 6040
void ConfigRPDO1(uint8_t node_id,bool start);
void SetTargetPosition(uint8_t node_id,double angle_rad);
void SetMode(uint8_t node_id,msgs::RunMode mode);
private:
static constexpr double GearRatio = 101.0; // 电机减速比
static constexpr double RADTODEG = 180.0 / 3.1415926;
std::vector<uint8_t> node_ids_{}; // 要控制的电机 id
// std::map<uint8_t,motor::Ti5MotorSdoRequestProtocol*> control_commands_{};
// nmt
device::NmtRequestProtocol<msgs::RobotDetail>* nmt_command_{nullptr};
//sync
device::SyncProtocol<msgs::RobotDetail>* sync_command_{nullptr};
// sdo
std::map<uint8_t,device::SdoRequestProtocol<msgs::RobotDetail>*> sdo_commands_{};
// rpdo1
std::map<uint8_t,robot::motor::Ti5MotorRPDO1*> rpdo1_commands_{};
std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
msgs::RunMode cur_mode_{msgs::RunMode::RUN_MODE_UNSPECIFIED};
void SetPPTargetPosBySdo(uint8_t node_id,int32_t pos);
void SetPPTargetPosByPdo(uint8_t node_id,int32_t pos);
void SetCSPTargetPosByPdo(uint8_t node_id,int32_t pos);
};
}
}
}

86
src/devices/robot/c701/motor/motor_controller_test.cpp
View File

@ -1,86 +0,0 @@
//
// Created by lgv on 2025/7/18.
//
#include <gtest/gtest.h>
#include "robot/c701/motor/motor_controller.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "canbus/canopen/sdo_request_protocol.h"
#include "canbus/can_client/socket/socket_can_client_raw.h"
// #include "cmvr/msgs/ti5_motor.pb.h"
using cmvr::msgs::RobotDetail;
using cmvr::msgs::ErrorCode;
using cmvr::device::CanSender;
using cmvr::device::MessageManager;
using cmvr::robot::c701::MotorController;
using cmvr::device::SdoRequestProtocol;
using cmvr::device::SocketCanClientRaw;
using namespace cmvr::msgs;
TEST(MotorControllerTest, MotorControllerCmdTest) {
std::vector<uint8_t> node_ids = {0x03};
MotorController controller(node_ids);
XmlNode xml_node;
SocketCanClientRaw can_client(xml_node);
can_client.start();
auto result = controller.Init(&can_client, false);
controller.Start();
for (uint8_t node_id: node_ids) {
controller.ConfigTPDO1(node_id);
controller.ConfigTPDO2(node_id);
controller.ConfigRPDO1(node_id,true);
controller.ConfigProfile(node_id,1000,1000,1000);
controller.SeedNmtRequest(node_id,NMT_ENTER_PRE_OPERATIONAL);
controller.SeedNmtRequest(node_id,NMT_START_REMOTE_NODE);
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
}
for (uint8_t node_id: node_ids) {
controller.SetMode(node_id,RUN_MODE_CYCLIC_SYNC_POSITION);
controller.SetTargetPosition(node_id,3.14);
// controller.SyncStart();
// controller.SetTarget(node_id,3.14,1000);
}
while (true) {
// controller.SetNmtRequest(test_id,NMT_RESET_NODE);
// controller.SeedSdoRequest(test_id,CS_READ_REQUEST,ACTUAL_POSITION_6064,SUB_INDEX_0,0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
EXPECT_EQ(result, ErrorCode::CANBUS_ERROR);
}

27
src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp
View File

@ -1,27 +0,0 @@
//
// Created by lgv on 2025/7/28.
//
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
#include "glog/logging.h"
using namespace cmvr::msgs;
using namespace cmvr::device;
using namespace cmvr::robot::motor;
void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
std::lock_guard<std::mutex> lock(mutex_);
data[0] = target_pos_ & 0xFF;
data[1] = target_pos_ >> 8 & 0xFF;
data[2] = target_pos_ >> 16 & 0xFF;
data[3] = target_pos_ >> 24 & 0xFF;
data[4] = ctrl_word_ & 0xFF;
data[5] = ctrl_word_ >> 8 & 0xFF;
// data[4] = target_vel_ & 0xFF;
// data[5] = target_vel_ >> 8 & 0xFF;
// data[6] = target_vel_ >> 16 & 0xFF;
// data[7] = target_vel_ >> 24 & 0xFF;
}

60
src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h
View File

@ -1,60 +0,0 @@
//
// Created by lgv on 2025/7/28.
//
#pragma once
#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include <mutex>
namespace cmvr {
namespace robot {
namespace motor {
class Ti5MotorRPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
public:
static constexpr uint32_t BASE_ID = msgs::RPDO1_BASE_ID_200;
static uint32_t ID(uint8_t node_id) {
return BASE_ID + node_id;
}
uint32_t ID() const{
return BASE_ID + node_id_;
}
explicit Ti5MotorRPDO1(uint8_t node_id) : node_id_(node_id) {}
void UpdateData(uint8_t *data) override;
int32_t GetLength() const override {
return 0x06;
}
uint32_t GetPeriod() const override {
return 1000 * 1; // 5 ms
}
void SetTargetPos(int32_t position) {
std::lock_guard<std::mutex> lock(mutex_);
target_pos_ = position;
}
void SetTargetVel(int32_t velocity) {
std::lock_guard<std::mutex> lock(mutex_);
target_vel_ = velocity;
}
void SetCtrlWord(uint16_t ctrl_word) {
std::lock_guard<std::mutex> lock(mutex_);
ctrl_word_ = ctrl_word;
}
private:
mutable std::mutex mutex_;
uint8_t node_id_{0};
int32_t target_pos_{0};
int32_t target_vel_{0};
uint16_t ctrl_word_{0};
};
}
}
}

39
src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp
View File

@ -1,39 +0,0 @@
//
// Created by lgv on 2025/7/24.
//
#include "ti5_motor_sdo_response.h"
using namespace cmvr::robot::motor;
using namespace cmvr::msgs;
void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
cmvr::msgs::RobotDetail *sensor_data) const {
// 通过 node_id 获取对应的电机状态（Ti5_MotorStatus）
auto &motors_map = *sensor_data->mutable_motors();
auto *motor_status = &motors_map[this->node_id_];
// 先把基础的 sdo_response 拷贝进去
motor_status->mutable_sdo_response()->CopyFrom(sdo_response);
switch (sdo_response.index()) {
case msgs::CONTROL_WORD_6040:
motor_status->set_ctrl_word(sdo_response.data());
break;
case msgs::STATUS_WORD_6041:
motor_status->set_status_word(sdo_response.data());
break;
case msgs::ACTUAL_POSITION_6064:
motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
LOG(INFO) << "pos = " << motor_status->position();
break;
}
LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
<< ": command=" << int(sdo_response.cs())
<< ", index=" << sdo_response.index()
<< ", subindex=" << int(sdo_response.sub_index())
<< ", data=" << sdo_response.data();
}

26
src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h
View File

@ -1,26 +0,0 @@
//
// Created by lgv on 2025/7/24.
//
#pragma once
#include "canbus/canopen/sdo_response_protocol.h"
#include "cmvr/msgs/robot_detail.pb.h"
namespace cmvr {
namespace robot {
namespace motor {
class Ti5MotorSdoResponse : public device::SdoResponseProtocol<cmvr::msgs::RobotDetail> {
public:
explicit Ti5MotorSdoResponse(uint8_t node_id)
: SdoResponseProtocol<cmvr::msgs::RobotDetail>(node_id) {
}
protected:
void ParseSdoData(const msgs::SdoFrame &sdo_response,
cmvr::msgs::RobotDetail *sensor_data) const override;
};
}
}
}

36
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp
View File

@ -1,36 +0,0 @@
//
// Created by lgv on 2025/7/25.
//
#include "ti5_motor_tpdo1.h"
#include "glog/logging.h"
#include "canbus/canopen/register.h"
using namespace cmvr::msgs;
using namespace cmvr::device;
using namespace cmvr::robot::motor;
void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
if (length < 7) {
LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
return;
}
auto &motors_map = *sensor_data->mutable_motors();
auto *motor_status = &motors_map[this->node_id_];
motor_status->set_ctrl_word(bytes[1] << 8 | bytes[0]);
motor_status->set_status_word(bytes[3] << 8 | bytes[2]);
motor_status->set_run_mode(static_cast<RunMode>(bytes[4]));
motor_status->set_error_state(bytes[6] << 8 | bytes[5]);
statusword_t st{};
st.value = motor_status->status_word();
if (st.op_mode_specific > 0) {
LOG(INFO) << st.op_mode_specific ;
}
}

32
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h
View File

@ -1,32 +0,0 @@
//
// Created by lgv on 2025/7/25.
//
#pragma once
#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"
namespace cmvr {
namespace robot {
namespace motor {
class Ti5MotorTPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
public:
static constexpr uint32_t BASE_ID = msgs::TPDO1_BASE_ID_180;
static uint32_t ID(uint8_t node_id) {
return BASE_ID + node_id;
}
uint32_t ID() const{
return BASE_ID + node_id_;
}
explicit Ti5MotorTPDO1(uint8_t node_id) : node_id_(node_id) {}
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
private:
uint8_t node_id_{0};
};
}
}
}

26
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp
View File

@ -1,26 +0,0 @@
//
// Created by lgv on 2025/7/25.
//
#include "ti5_motor_tpdo2.h"
#include "glog/logging.h"
using namespace cmvr::msgs;
using namespace cmvr::device;
using namespace cmvr::robot::motor;
void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
if (length < 8) {
LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
return;
}
auto &motors_map = *sensor_data->mutable_motors();
auto *motor_status = &motors_map[this->node_id_];
motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed();
}

32
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h
View File

@ -1,32 +0,0 @@
//
// Created by lgv on 2025/7/25.
//
#pragma once
#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"
namespace cmvr {
namespace robot {
namespace motor {
class Ti5MotorTPDO2 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
public:
static constexpr uint32_t BASE_ID = msgs::TPDO2_BASE_ID_280;
static uint32_t ID(uint8_t node_id) {
return BASE_ID + node_id;
}
uint32_t ID() const{
return BASE_ID + node_id_;
}
explicit Ti5MotorTPDO2(uint8_t node_id) : node_id_(node_id) {}
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
private:
uint8_t node_id_{0};
};
}
}
}

6
src/devices/robot/controller/CMakeLists.txt
View File

@ -1,6 +0,0 @@
add_library(controller SHARED controller_manager.cpp)
target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::controller ALIAS controller)

21
src/devices/robot/controller/abstractcontroller.h
View File

@ -1,21 +0,0 @@
//
// Created by linbo on 2025/10/23.
//
#ifndef CMVR_ES_ABSTRACTCONTROLLER_H
#define CMVR_ES_ABSTRACTCONTROLLER_H
#include <curl/curl.h>
#include "rapidxml/xml_parser.h"
namespace cmvr::device
{
class AbstractController
{
public:
explicit AbstractController();
};
}
#endif //CMVR_ES_ABSTRACTCONTROLLER_H

8
src/devices/robot/controller/controller_manager.cpp
View File

@ -1,8 +0,0 @@
//
// Created by linbo on 2025/10/23.
//
#include "controller_manager.h"
using namespace std;
using namespace cmvr::device;

25
src/devices/robot/controller/controller_manager.h
View File

@ -1,25 +0,0 @@
//
// Created by linbo on 2025/10/23.
//
#ifndef CMVR_ES_CONTROLLER_MANAGER_H
#define CMVR_ES_CONTROLLER_MANAGER_H
#include "abstractcontroller.h"
namespace cmvr::device
{
class ControllerManager
{
public:
explicit ControllerManager();
~ControllerManager() = default;
private:
};
}
#endif //CMVR_ES_CONTROLLER_MANAGER_H

1
src/devices/robot/humanoid_robot/CMakeLists.txt
View File

@ -11,6 +11,7 @@ target_link_libraries(humanoid_robot PRIVATE
cmvr_es::device::canbus
cmvr_es::device::ti5motor
protobuf::libprotobuf
cmvr_es::device::controller_manager
)

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

@ -17,11 +17,12 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
id_ = cfg.getAttrString("id");
dof_ = DOF;
if (!pathExists(cfg.getAttrString("urdf"))) {
std::string urdf = cfg.getAttrString("urdfPath");
if (!pathExists(urdf)) {
throw runtime_error("urdf file does not exist");
}
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
cfg.getAttrString("urdf"), cfg.getAttrString("baseLink"));
urdf, cfg.getAttrString("baseLink"));
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
joint_names_ = splitString(cfg.getAttrString("jointNames"), ",");
link_names_ = splitString(cfg.getAttrString("linkNames"), ",");
@ -30,68 +31,21 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
}
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
m_cctrl_ = make_shared<ctrl::QPSolver<DOF> >(m_robot_);
upd_freq_ = cfg.getAttrDefault("updFreq", 500);
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50));
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(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 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_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
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_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
}
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_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
waist_can_receiver_ = std::make_shared<CanReceiver<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_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
[this] { update_state_(); });
rsm_.store(ROBOT_READY);
auto controllerManagerCfg = cfg.getChild("ControllerManager");
controller_manager_ = std::make_shared<ControllerManager>(controllerManagerCfg);
} catch (exception &e) {
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
throw runtime_error(e.what());
@ -100,90 +54,19 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
template<int DOF>
void HumanoidRobot<DOF>::init() {
struct Limb {
std::string name;
bool enabled;
std::shared_ptr<AbstractCanbus> client;
std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
std::vector<XmlNode> motor_cfgs;
};
std::vector<Limb> limbs{
{
"WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_,
waist_motors_cfg_
},
{
"LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_,
l_motors_cfg_
},
{
"RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_,
r_motors_cfg_
},
{
"HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_,
head_motors_cfg_
}
};
auto motorManagerCfg = cfg_.getChild("MotorManager");
// 创建 MotorManager
motor_manager_ = std::make_shared<MotorManager>();
std::vector<std::future<void> > tasks;
for (auto &limb: limbs) {
if (!limb.enabled) continue;
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);
if (protocol) motor->setProtocol(protocol);
motor->init();
motor_manager_->addMotor(motor);
}
}));
}
// 3. 创建协议（如果有CAN）
std::shared_ptr<hardware::AbstractMotorProtocol> protocol = nullptr;
for (const auto& motorCfg: motorManagerCfg.getChildren())
{
auto canGroupID = motorCfg.getAttrString("canGroupID");
auto protocolType = motorCfg.getAttrString("protocolType");
auto motor = std::make_shared<Ti5Motor>(motorCfg);
//motor->init();
motor_manager_->addMotor(motor);
}
// 等待所有任务完成
for (auto &task: tasks) task.get();
rsm_.store(ROBOT_ESTOP);
LOG(INFO) << "[HumanoidRobot](init):All enabled motors initialized successfully.";
}
@ -306,39 +189,48 @@ void HumanoidRobot<DOF>::eStop() {
template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
try {
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
// PPM 模式下 这个实际速度会超30% 左右
motor->setQd(vel);
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
motor->setQ(j.rad);
}
auto state = controller_manager_->getSate();
if (state == ControlManagerState_Teach
|| state == ControlManagerState_EStop
|| state == ControlManagerState_MajorFault)
{
throw runtime_error("Controller state error, current state:" + std::to_string(state));
}
else if (state == ControlManagerState_Command || state == ControlManagerState_Idle)
{
//先获取当前正在执行的控制器
auto activeController = controller_manager_->getActiveController();
if (activeController)
{
activeController->stop();
}
//ComponentGroup的id要作为参数传入才能保证获取到对应的实例对象
auto group = controller_manager_->getComponentGroup("leftArm");
//控住器实例由接口控制，比如当前MoveJ调用JointPositionCtrl去实现功能
if (!group.controllers_.count("JointPositionCtrl"))
throw runtime_error("Controller not found");
auto controller = group.controllers_["JointPositionCtrl"];
//3. wait for completion
bool completion = true;
do {
completion = true;
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
if (!motor->reachedTargetQ()) {
completion = false;
break;
}
}
Json::Value callJson;
Json::Value paramsJson;
//传入多个canGroupId
for (int i = 0; i < group.CanGroupIDs.size(); i++)
{
paramsJson["canGroupId"][i] = group.CanGroupIDs[i];
}
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
if (flash_cmd_.load()) {
flash_cmd_.store(false);
return;
for (int i = 0; i < cmd.size(); i++)
{
paramsJson["motors"][i]["joint_name"] = cmd[i].joint_name;
paramsJson["motors"][i]["position"] = cmd[i].rad;
paramsJson["motors"][i]["velocity"] = cmd[i].vel;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion);
//拆分cmd，构建json内容
callJson["params"] = paramsJson;
controller->call(callJson);
}
} catch (exception &e) {
throw runtime_error(e.what());
}
@ -383,7 +275,7 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
// 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,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw runtime_error("solve IK failed");
@ -452,7 +344,7 @@ void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vecto
// slove ik
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw runtime_error("solve IK failed");
@ -589,7 +481,7 @@ void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::P
// 求解逆运动学
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
@ -945,7 +837,7 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
ctrl::QPSolver<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {
@ -1146,7 +1038,7 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
base_link, // 基座链接
first_pose_targets, // 第一个点的位姿目标
dt, // 控制周期（用于速度限制）
ctrl::CartesianController<DOF>::Mode::Position, // 位置控制模式
ctrl::QPSolver<DOF>::Mode::Position, // 位置控制模式
q_first, // 输出：第一个点的关节配置
10000, // IK最大迭代次数（确保精度）
1e-6 // IK位置精度（1mm/0.001°）
@ -1264,7 +1156,7 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
base_link, // 基座链接
current_pose_targets, // 当前点的位姿目标
dt, // 控制周期
ctrl::CartesianController<DOF>::Mode::Position,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, // 输出：当前点的关节配置
5000, // 减少迭代次数（平衡精度与速度）
5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽）
@ -1372,7 +1264,7 @@ void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string
// 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,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw runtime_error("solve IK failed");
@ -1419,7 +1311,7 @@ template<int DOF>
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
try {
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 60, 1e-4);
if (!ok) {
LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_;
@ -1561,7 +1453,7 @@ std::vector<double> HumanoidRobot<
// 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,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw std::runtime_error("IK solve failed");
@ -1751,7 +1643,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
Eigen::Vector<double, DOF> q_cmd_check;
bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
ctrl::QPSolver<DOF>::Mode::Position,
q_cmd_check, 10000, 1e-6);
if (!ik_solvable) {
throw std::runtime_error("moveL: Target pose is unreachable with constant orientation");
@ -1813,7 +1705,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
// 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
ctrl::QPSolver<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {

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

@ -31,6 +31,7 @@
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "controller/controller_manager.h"
namespace cmvr::device{
@ -150,7 +151,7 @@ namespace cmvr::device{
std::shared_ptr<cmvr::dyn::State<DOF>> m_state_;
std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_;
std::shared_ptr<cmvr::ctrl::CartesianController<DOF>> m_cctrl_;
std::shared_ptr<cmvr::ctrl::QPSolver<DOF>> m_cctrl_;
std::vector<std::string> joint_names_;
std::vector<std::string> link_names_;
@ -221,8 +222,8 @@ namespace cmvr::device{
std::shared_ptr<MotorManager> motor_manager_{nullptr};
private:
std::shared_ptr<ControllerManager> controller_manager_{nullptr};
};

5
src/devices/robot/ti5_robot/CMakeLists.txt
View File

@ -1,5 +0,0 @@
#add_library(ti5robot SHARED ti5_robot.cpp)
#
#target_include_directories(ti5robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
#
#add_library(cmvr_es::device::ti5robot ALIAS ti5robot)

343
src/devices/robot/ti5_robot/ti5_robot.cpp
View File

@ -1,343 +0,0 @@
//#include "ti5_robot.h"
//#include <string>
//#include <vector>
//#include <memory>
//#include <chrono>
//#include <thread>
//#include <sstream>
//#include <cstring>
//#include <sys/socket.h>
//#include <arpa/inet.h>
//#include <unistd.h>
//#include <csignal>
//#include <mutex>
//#include <shared_mutex>
//#include <termios.h>
//#include <fcntl.h>
//#include <algorithm>
//#include <sys/socket.h>
//#include <nlohmann/json.hpp>
//
//using json = nlohmann::json;
//using namespace std;
//using namespace cmvr::device;
//// 服务端地址和端口（需与服务端保持一致）
//#define SERVER_IP "10.148.108.142" // 服务端IP，本地测试用127.0.0.1
//#define SERVER_PORT 8888 // 服务端端口，与服务端PORT一致
//// 设置socket超时（单位：毫秒）
//void set_socket_timeout(int sock, long timeout_ms) {
// struct timeval tv;
// tv.tv_sec = timeout_ms / 1000;
// tv.tv_usec = (timeout_ms % 1000) * 1000;
// setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
//}
//
//// 发送请求并接收响应的函数 正常返回应该是Json字符串
//bool send_request(const string& request_str,string& result) {
// int sock = 0;
// struct sockaddr_in serv_addr;
// char buffer[1024] = {0};
//
// // 创建socket
// if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
// LOG(ERROR) << "[Ti5Robot] (send_request): socket create failed";
// return false;
// }
//
// // 设置服务端地址信息
// serv_addr.sin_family = AF_INET;
// serv_addr.sin_port = htons(SERVER_PORT);
// if (inet_pton(AF_INET, SERVER_IP, &serv_addr.sin_addr) <= 0) {
// LOG(ERROR) << "[Ti5Robot] (send_request): invaid ip address";
// close(sock);
// return false;
// }
//
// // 连接服务端
// if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
// LOG(ERROR) << "[Ti5Robot] (send_request): connect server failed";
// close(sock);
// return false;
// }
//
// // 发送JSON请求
// send(sock, request_str.c_str(), request_str.length(), 0);
// LOG(INFO) << "[Ti5Robot] (send_request): request info:" << request_str;
// // 设置接收超时为3秒
// set_socket_timeout(sock, 2000);
// // 接收服务端响应
// ssize_t valread = read(sock, buffer, 1024);
//
// close(sock); // 关闭连接
// if (valread == -1 && errno == EAGAIN) {
// LOG(ERROR) << "[Ti5Robot] (send_request): timeout";
// return false;
// }
// else if (valread <= 0) {
// LOG(ERROR) << "[Ti5Robot] (send_request): read error:" << valread;
// return false;
// }
// else {
// result = buffer;
// }
// return true;
//}
//
//
//// Ti5Robot 类的实现
//Ti5Robot::Ti5Robot(const XmlNode& cfg) : AbstractRobot(cfg) {
// // 从配置中初始化机器人参数
// try {
// id_ = cfg_.getAttrString("id");
// if (id_ == "left_arm") {
// arm_ = Left_Arm;
// } else if (id_ == "right_arm") {
// arm_ = Right_Arm;
// } else {
// //报错
// }
// canInd_ = cfg_.getAttrDefault("canInd", 1);
// deviceInd_ = cfg_.getAttrDefault("deviceInd", 1);
// }
// catch (const exception& e) {
// LOG(ERROR) << "[Ti5Robot] ([Ti5Robot]): Failed to parse XML: " << e.what();
// }
//}
//
//Ti5Robot::~Ti5Robot() {
// LOG(INFO) << "Ti5Robot 析构函数被调用";
//}
//
//void Ti5Robot::init() {
// // 连接socket服务
// // 检查是否可以连接到服务端。
// // 角度范围应该在[-π,π]之间？
// std::vector<double> goal_j(7,0.5);
// moveJ(goal_j,0.5,0.1);
//}
//
//void Ti5Robot::start() {
// LOG(INFO) << "Ti5Robot 启动";
// // 启动机器人运行
// // 可以添加启动CAN通道等操作
//}
//
//void Ti5Robot::stop() {
// LOG(INFO) << "Ti5Robot 停止";
// // 停止机器人运行
// // 可以添加停止CAN通道等操作
//}
//
//void Ti5Robot::getState(RobotState &state) {
// // 获取机器人当前状态
// LOG(INFO) << "[Ti5Robot](getState)";
//
// // 获取左右臂的关节角度
// float currentAngles[7];
// get_current_angle(arm_,currentAngles,canInd_,deviceInd_);
// for (float currentAngle : currentAngles)
// {
// state.joint_positions.push_back(currentAngle);
// }
//
// // 获取末端执行器位姿
//// float pos[6];
//// get_current_pose(Left_Arm, pos, 0, 0);
//// state.ee_pose.position.x = pos[0];
//// state.ee_pose.position.y = pos[1];
//// state.ee_pose.position.z = pos[2];
//// state.ee_pose.euler.rx = pos[3];
//// state.ee_pose.euler.ry = pos[4];
//// state.ee_pose.euler.rz = pos[5];
//}
//
//void Ti5Robot::eStop() {
// LOG(WARNING) << "[Ti5Robot::eStop]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::eStop]: The current interface is not yet implemented.");
// LOG(INFO) << "Ti5Robot 紧急停止";
// // 实现紧急停止逻辑
// // 例如刹车操作
// // brake(LEFT_ARM, 0, 0); // 假设使用左臂，设备索引0，通道索引0
// // brake(RIGHT_ARM, 0, 0);
//}
//
//void Ti5Robot::moveJ(std::vector<double> &joints, double vel, double acc) {
// //LOG(WARNING) << "[Ti5Robot::moveJ]: The current interface is not yet implemented.";
// //throw std::runtime_error("[Ti5Robot::moveJ]: The current interface is not yet implemented.");
// LOG(INFO) << "Ti5Robot 关节空间运动: 关节角度=";
// // 实现关节空间运动控制逻辑
// float goal_j[7];
// for (int i = 0; i < 7; ++i) {
// goal_j[i] = static_cast<float>(joints[i]);
// }
// joint_to_move(arm_,goal_j,deviceInd_,canInd_);
//}
//
//void Ti5Robot::moveJ_IK(math::Pose3d &pose, double vel, double acc) {
// float pos[6] = {static_cast<float>(pose.position.x), static_cast<float>(pose.position.y), static_cast<float>(pose.position.z),
// static_cast<float>(pose.euler.rx), static_cast<float>(pose.euler.ry), static_cast<float>(pose.euler.rz)};
// pos_to_move(arm_, pos, 0, 0);
//}
//
//void Ti5Robot::moveL(math::Pose3d &pose, double vel, double acc) {
// LOG(WARNING) << "[Ti5Robot::moveL]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::moveL]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::teachJ(RobotJointIndex joint, RobotJointIndexDirection dir, double vel, double acc) {
// LOG(WARNING) << "[Ti5Robot::teachJ]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::teachJ]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::teachL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
// LOG(WARNING) << "[Ti5Robot::teachL]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::teachL]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) {
// LOG(WARNING) << "[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) {
// LOG(WARNING) << "[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::servoJ(std::vector<double> &joints, double dt) {
// LOG(WARNING) << "[Ti5Robot::servoJ]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::servoJ]: The current interface is not yet implemented.");
//}
//
//void Ti5Robot::servoL(math::Pose3d &pose, double dt) {
// LOG(WARNING) << "[Ti5Robot::servoL]: The current interface is not yet implemented.";
// throw std::runtime_error("[Ti5Robot::servoL]: The current interface is not yet implemented.");
//}
//
//bool Ti5Robot::get_current_angle(ArmType arm, float goal_j[7], int canInd, int deviceInd)
//{
// // 获取关节位置
// // 获取当前关节角度
// bool success = false;
// json req;
// req["interface"] = "get_current_angle"; // 接口名
// if (arm == Left_Arm) {
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
// }
// else if (arm == Right_Arm) {
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
// }
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
// req["parameters"]["canInd"] = canInd; // CAN通道号
//
// string resp_str;
// auto ret = send_request(req.dump(),resp_str);
// if (ret && !resp_str.empty()) {
// json resp = json::parse(resp_str);
// if (resp.contains("error")) {
// cerr << "接口调用失败: " << resp["error"] << endl;
// } else {
// std::vector<float> angeles = resp["result"];
// for (int i = 0; i < 7; ++i) {
// goal_j[i] = angeles[i];
// success = true;
// }
// }
// }
//
// return success;
//}
//
//
//bool Ti5Robot::get_current_pose(ArmType arm, float pos[6], int canInd, int deviceInd)
//{
// // 获取关节位置
// // 获取当前关节角度
// bool success = false;
// json req;
// req["interface"] = "get_current_pose"; // 接口名
// if (arm == Left_Arm) {
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
// }
// else if (arm == Right_Arm) {
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
// }
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
// req["parameters"]["canInd"] = canInd; // CAN通道号
//
// string resp_str;
// auto ret = send_request(req.dump(),resp_str);
// if (ret && !resp_str.empty()) {
// json resp = json::parse(resp_str);
// if (resp.contains("error")) {
// cerr << "接口调用失败: " << resp["error"] << endl;
// } else {
// std::vector<float> result = resp["result"];
// for (int i = 0; i < 6; ++i) {
// pos[i] = result[i];
// success = true;
// }
// }
// }
//
// return success;
//}
//
//
//bool Ti5Robot::joint_to_move(ArmType side, float *goal_j, int deviceInd, int canInd)
//{
// bool success = false;
//
// json req;
// req["interface"] = "joint_to_move"; // 接口名
// if (arm_ == Left_Arm) {
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
// }
// else if (arm_ == Right_Arm) {
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
// }
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
// req["parameters"]["canInd"] = canInd; // CAN通道号
// // 目标角度数组（7个关节角度，单位：弧度）
// req["parameters"]["goal_j"] = *goal_j;
//
// string resp_str;
// auto ret = send_request(req.dump(),resp_str);
// if (ret && !resp_str.empty()) {
// json resp = json::parse(resp_str);
// if (resp.contains("error")) {
// success = false;
// } else {
// success = true;
// }
// }
// return success;
//}
//
//bool Ti5Robot::pos_to_move(ArmType side, float *pos, int deviceInd, int canInd)
//{
// bool success = false;
// json req;
// req["interface"] = "pos_to_move"; // 接口名
// if (arm_ == Left_Arm) {
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
// }
// else if (arm_ == Right_Arm) {
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
// }
// req["parameters"]["deviceInd"] = deviceInd_; // CAN设备号
// req["parameters"]["canInd"] = canInd_; // CAN通道号
// req["parameters"]["pos"] = *pos;
//
// string resp_str;
// auto ret = send_request(req.dump(),resp_str);
// if (ret && !resp_str.empty()) {
// json resp = json::parse(resp_str);
// if (resp.contains("error")) {
// success = false;
// } else {
// success = true;
// }
// }
// return success;
//}

49
src/devices/robot/ti5_robot/ti5_robot.h
View File

@ -1,49 +0,0 @@
//
// Created by linbo on 2025/7/1.
//
#ifndef TI5_ROBOT_H
#define TI5_ROBOT_H
#include "devices/abstract_robot.h"
namespace cmvr::device {
class Ti5Robot final : public AbstractRobot
{
public:
explicit Ti5Robot(const XmlNode& cfg);
~Ti5Robot() override;
void init() override;
void start() override;
void stop() override;
void getState(RobotState &state) override;
void eStop() override;
void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) override;
void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) override;
void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) override;
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) override;
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) override;
void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) override;
void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) override;
void servoJ(std::vector<double> &joints, double dt) override;
void servoL(math::Pose3d &pose, double dt) override;
// 以下为socket调用ti5robot sdk的接口
// bool get_current_angle(ArmType arm, float goal_j[7], int canInd,int deviceInd);
// bool get_current_pose(ArmType arm, float pos[6], int canInd, int deviceInd);
// bool joint_to_move(ArmType side, float *goal_j, int deviceInd, int canInd);
// bool pos_to_move(ArmType side, float *pos, int deviceInd, int canInd);
private:
// ArmType arm_;
// int canInd_;
// int deviceInd_;
};
}
#endif //TI5_ROBOT_H

252
src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h
View File

@ -1,252 +0,0 @@
#ifndef Ti5BASIC_H
#define Ti5BASIC_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <ifaddrs.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <iostream>
#include <fstream>
#include <string>
#include <unordered_map>
#include <sstream>
#include <map>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <vector>
#include <regex>
#include <iomanip>
#include <cstring>
#include "Ti5LOGIC.h"
#include "Ti5MOVE.h"
// #include "can/tcontrolcanfactor.h"
#define MAX_IP_ADDR_LEN 256 // 存储IP地址的最大长度
// extern float arr_s[6];
extern bool flag;
extern string log_path; // log文件
extern char LogInfo[100]; // 存储写入log文件的信息
extern char Info_Str[100]; // 定义一个字符数组用于存储 flag 的字符串表示
class ArmController; // 声明ArmController类
extern uint8_t l_id[IDNUM], r_id[IDNUM]; // 声明左臂和右臂的canID数组
extern ArmController l_controller; // 声明TH_L对象
extern ArmController r_controller; // 声明TH_R对象
extern class humanoidLeftArm l_solver; // 数学上的解算器
extern class humanoidRightArm r_solver; // 解算器
enum ArmSide
{
LEFT_ARM, // 左臂
RIGHT_ARM // 右臂
};
extern "C"
{ // 添加extern "C"
// 写入调试信息到文件
void writeDebugInfoToFile(const char *func_name, const char *info);
// 输出数组的调试信息
void printArrayDebugInfo(float arr[], int size, const char *arr_name);
/* 获取本机IP地址*/
std::string ip_address();
/*std::string query_can();
查询can设备号
*/
std::vector<std::string> query_can();
void receive_fully(int sock, void *buffer, size_t size);
/*socket通信
参数：
deviceInd：can设备号
canInd：can通道
port：端口号
发送端发送数据格式：{标识位，数据}
标识位
0x01 左臂pos
0x02 右臂pos
0x03 左臂joint
0x04 右臂joint
*/
int ti5_socket_server(int deviceInd, int canInd,int port);
/*获取电机错误状态
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
deviceInd：can设备号
canInd：can通道
dataList:接收数据的数组，错误状态
返回值：为电机错误
0：无错误
1：软件错误
2：过压
4：欠压
16：启动错误
*/
// int get_mechanicalarm_Maschinen_status(ArmSide side, int deviceInd, int canInd);
void get_mechanicalarm_status(ArmSide side, int deviceInd, int canInd, int32_t *dataList);
/*清除电机错误
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
deviceInd：can设备号
canInd：can通道
*/
void clear_elc_error(ArmSide side, int deviceInd, int canInd);
/*机械臂刹车
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
canInd：can通道
deviceInd：can设备号
返回值：
true：成功
false：失败
*/
bool brake(ArmSide side, int deviceInd, int canInd);
/*将数据记录下来写入文件
参数：
pj_flag：角度或者位姿标识,1为角度 ，0为坐标
filename：存储文件名
array[6]：被保存的值
*/
void write_value(int pj_flag, string filename, float array[7]);
/*机械臂回到原点
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
canInd：can通道
deviceInd：can设备号
返回值：无
*/
void mechanical_arm_origin(ArmSide side, int deviceInd, int canInd);
/*机械臂关节运动
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
goal_j：目标关节角
canInd：can通道
DeviceInd：can设备号
返回值：无*/
void joint_to_move(ArmSide side, float *goal_j, int deviceInd, int canInd);
void new_joint_to_move(ArmSide side, float *goal_j, int deviceInd, int canInd);
/*机械臂关节运动，同时获取当前位置
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
goal_j：目标关节角
CUrrentJointPosition：存储当前位置
canInd：can通道
deviceInd：can设备号
返回值：无
*/
void GetP_joint_to_move(ArmSide side, float *goal_j, float *CUrrentJointPosition, int deviceInd, int canInd);
/*pos运动
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
pos：目标位置
value：dim的值
dim：-1~2 代表x,y,z， -1的时候是没有臂角约束，只会接收末端位姿，其他参数忽略
absolute：true的时候是绝对位置（以胸部原点位置），false的时候是相对位置（以当前点胳膊轴位置）
canInd：can通道
deviceInd：can设备号
返回值：无
*/
// void pos_to_move(ArmSide side, float *pos, float value, int dim, bool absolute, int deviceInd, int canInd);
bool pos_to_move(ArmSide side, float *pos, int deviceInd, int canInd);
/*获取当前角度
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
goal_j：存储角度的数组
canInd：can通道
deviceInd：can设备号
*/
void get_current_angle(ArmSide side, float goal_j[7], int deviceInd, int canInd);
/*获取当前位姿
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
posz：存储位姿的数组
canInd：can通道
deviceInd：can设备号
*/
void get_current_pose(ArmSide side, float posz[7], int deviceInd, int canInd);
/*设置为电流模式，并设置目标电流
参数：
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
current: 目标电流
canInd：can通道
deviceInd：can设备号
*/
void set_current_mode(ArmSide side, uint32_t current[7], int deviceInd, int canInd);
/*设置键盘控制模式
参数：
canInd：can通道
deviceInd：can设备号
*/
void LLL_keyboard_controller_J(int deviceInd, int canInd);
void LLL_keyboard_controller_P(int deviceInd, int canInd);
void RRR_keyboard_controller_J(int deviceInd, int canInd);
void RRR_keyboard_controller_P(int deviceInd, int canInd);
// void L_keyboard_controller(int deviceInd, int canInd);
// void R_keyboard_controller(int deviceInd, int canInd);
/*获取电机当前位置
参数：
canInd：can通道
deviceInd：can设备号
MotorsTotal：电机数量
MotorsIDlist：电机ID列表
data：存储电机位置数据的数组
*/
void get_motor_position(int deviceInd, int canInd, int MotorsTotal,uint8_t *MotorsIDlist,int32_t *MotorPosition);
/*设置电机当前位置
参数：
canInd：can通道
deviceInd：can设备号
MotorsTotal：电机数量
MotorsIDlist：电机ID列表
data：存储电机位置数据的数组
*/
void set_motor_position(int deviceInd, int canInd, int MotorsTotal,uint8_t *MotorsIDlist,int32_t *MotorPosition);
// 设定终端为非阻塞模式
void setNonBlocking(bool enable) ;
// 设定文件描述符为非阻塞
void setNonBlockingInput() ;
// 清空键盘缓冲区
void clearStdinBuffer();
// 读取键盘输入
char ssscanKeyboard();
} // 添加extern "C"
#endif

171
src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h
View File

@ -1,171 +0,0 @@
#ifndef TI5CAN_DRIVER_H
#define TI5CAN_DRIVER_H
#include <iostream>
#include <string>
#include <unistd.h>
#include <vector>
#include <map>
#include <algorithm>
#include <unordered_map>
#include "controlcan.h"
#include "Ti5BASIC.h"
#define GET_MOTOR_PERSIONS 8 //获取电机当前位置指令（1字节），转化为减速机角度公式：(返回值/65536/减速比)*360
#define GET_MOTOR_ERROR 10 //获取电机错误指令（1字节）
#define CLEAR_ERROR 11 //清除电机错误指令（1字节）
#define SET_MOTOR_CURRENTS 28 //设置电机为电流模式，并设置目标电流指令（5字节）
#define SET_MOTOR_SPEED 29 //设置电机为速度模式，并设置目标速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
#define SET_MOTOR_POSITION 30 //设置电机为位置模式，并设置目标位置指令（5字节），下发参数为：(减速机目标角度/360)*减速比*65536
#define SET_MOTOR_MAX_SPEED 36 //设置电机最大正向允许速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
#define SET_MOTOR_MIN_SPEED 37 //设置电机最小负向允许速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
extern int CanNum;
// 查询 CAN 设备并绑定序列号与设备索引
std::map<std::string, int> query_can_with_index();
/*登录并初始化can设备
参数：无
返回值
true：成功
false：失败
*/
bool Start();
// void Start();
bool compare_serial_numbers(const std::string &a, const std::string &b);
std::unordered_map<std::string, int> get_device_map();
void operate_multiple_can_devices();
/*登出can设备
参数：无
返回值
true：成功
false：失败
*/
bool Exit();
int32_t convertHexArrayToDecimal(const uint8_t hexArray[4]);
void toIntArray(int number, int *res, int size);
/*发送1字节指令
参数：
DeviceInd：can设备索引 （一个为0,2个为1）
CANInd：can通道索引 （通道1：0,通道2：1）
numOfActuator：发送指令的电机数量
canIdList：电机canId列表
commandList：指令列表
dataList：接收数据列表
*/
void sendSimpleCanCommand(int DeviceInd, int CANInd,uint8_t numOfActuator, uint8_t *canIdList, uint8_t command,int32_t *dataList);
void sendCanCommand(int DeviceInd,int CANInd,uint8_t numOfActuator, uint8_t *canIdList, uint8_t command, uint32_t *parameterList);
#endif // TEST_CAN_H
// // 比较函数，用于按字典序排序设备序列号
// bool compare_serial_numbers(const std::string &a, const std::string &b)
// {
// return a < b; // 字典序比较
// }
// std::unordered_map<std::string, int> get_device_map()
// {
// VCI_BOARD_INFO pinfo[50];
// std::unordered_map<std::string, int> deviceMap;
// int num = VCI_FindUsbDevice2(pinfo); // 查询所有CAN设备
// // 提取所有设备的序列号
// std::vector<std::string> serialNumbers;
// for (int i = 0; i < num; i++)
// {
// std::string serialNumber = "";
// for (int j = 0; j < 20; j++) // 提取序列号
// {
// if (pinfo[i].str_Serial_Num[j] == '\0') // 结尾判断
// break;
// serialNumber += pinfo[i].str_Serial_Num[j];
// }
// serialNumbers.push_back(serialNumber);
// }
// // 按照字典序对序列号进行排序
// std::sort(serialNumbers.begin(), serialNumbers.end(), compare_serial_numbers);
// // 将排序后的序列号和索引进行绑定
// for (int i = 0; i < serialNumbers.size(); i++)
// {
// deviceMap[serialNumbers[i]] = i;
// }
// return deviceMap; // 返回序列号与设备索引的映射关系
// }
// void operate_multiple_can_devices()
// {
// auto deviceMap = get_device_map(); // 获取设备序列号与设备索引的映射
// // 遍历所有设备
// for (const auto &pair : deviceMap)
// {
// const std::string &serialNumber = pair.first; // 设备序列号
// int nDeviceInd = pair.second; // 设备索引
// int nDeviceType = 4; // 根据实际设备类型设置
// DWORD dwRel;
// VCI_INIT_CONFIG vic;
// std::cout << "正在操作设备: " << serialNumber << " (索引: " << nDeviceInd << ")" << std::endl;
// // 打开设备
// dwRel = VCI_OpenDevice(nDeviceType, nDeviceInd, 0);
// if (dwRel != 1)
// {
// std::cout << "无法打开设备 " << serialNumber << std::endl;
// continue;
// }
// // 初始化 CAN 通道
// vic.AccCode = 0x80000008;
// vic.AccMask = 0xFFFFFFFF;
// vic.Filter = 1;
// vic.Timing0 = 0x00;
// vic.Timing1 = 0x14;
// vic.Mode = 0;
// if (VCI_InitCAN(nDeviceType, nDeviceInd, 0, &vic) != 1)
// {
// std::cout << "初始化设备 " << serialNumber << " 的 CAN 通道 0 失败" << std::endl;
// VCI_CloseDevice(nDeviceType, nDeviceInd);
// continue;
// }
// if (VCI_InitCAN(nDeviceType, nDeviceInd, 1, &vic) != 1)
// {
// std::cout << "初始化设备 " << serialNumber << " 的 CAN 通道 1 失败" << std::endl;
// VCI_CloseDevice(nDeviceType, nDeviceInd);
// continue;
// }
// // 启动 CAN 通道
// if (VCI_StartCAN(nDeviceType, nDeviceInd, 0) != 1)
// {
// std::cout << "启动设备 " << serialNumber << " 的 CAN 通道 0 失败" << std::endl;
// VCI_CloseDevice(nDeviceType, nDeviceInd);
// continue;
// }
// if (VCI_StartCAN(nDeviceType, nDeviceInd, 1) != 1)
// {
// std::cout << "启动设备 " << serialNumber << " 的 CAN 通道 1 失败" << std::endl;
// VCI_CloseDevice(nDeviceType, nDeviceInd);
// continue;
// }
// std::cout << "设备 " << serialNumber << " 的所有 CAN 通道启动成功!" << std::endl;
// }
// }

160
src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h
View File

@ -1,160 +0,0 @@
#ifndef _TI5LOGIC_H_
#define _TI5LOGIC_H_
#include "mathfunc.h"
#include <iostream>
#include <time.h>
#include <cmath>
#include "tool.h"
using namespace std;
extern "C"
{ // 添加extern "C"
class pos_trans
{
protected:
float j[7] = {0, 0, 0, 0, 0, 0, 0};
float orij[7];
float prcj = 0.017;
float scd = sin(prcj) * sin(prcj);
float prc1 = float(int(10000 * sin(prcj) + 1)) / 10000;
float prc2 = prc1 * (prc1 + 2);
float prc3 = prc1 * (3 + prc1 + prc1 * prc1) + prc2;
float P_0[19][3] = {{0, 0, 0}}; // 机械臂按线性顺序的点集
struct ROD
{
int i1, i2; // 连杆端点为P_0[i1]和P_0[i2]
float r; // 连杆半径
float l; // 连杆长度
};
ROD rod[18]; // 机械臂按线性顺序的连杆集
float len[18]; // 数学模型的长度参数（与rod.l独立）
int rodnum; // 连杆总数
// 检测两连杆是否碰撞，P1、P2构成一根连杆，半径为r1；P3、P4构成另一根连杆，半径为r2
bool LLcolsp(float P1[3], float P2[3], float P3[3], float P4[3], float r1, float r2);
void init_rodindex();
// 若点距和连杆长度一致则认为点坐标值正确
bool check_Points();
// 根据变换矩阵计算位姿
void MatrixT2ypr(float T[4][4]);
// 根据位姿计算变换矩阵
void Pos2MatrixT(float T[4][4]);
// 修正角度并检测关节范围是否合法（相邻连杆间是否碰撞）
bool mendjoints();
bool checkcalj();
// 检测所有连杆间的碰撞
bool check_colsp();
public:
float jr1[7] = {-pi, -pi, -pi, -pi, -pi, -pi, -pi}; // 关节静态角度范围
float jr2[7] = {pi, pi, pi, pi, pi, pi, pi};
float gap0, gap;
float ypr[6] = {0, 0, 0};
// float j[7] = {0, 0, 0, 0, 0, 0, 0};
void showpointsinfo();
};
class humanoidLeftArm : public pos_trans
{
protected:
// 初始化固有的机械结构
void init_arm_structure()
{
rodnum = 3;
rod[0].r = 10, rod[1].r = 10, rod[2].r = 10; // 连杆半径 170A
rod[0].l = 171, rod[1].l = 250, rod[2].l = 250; // 连杆长度 170A
// rod[0].r = 45, rod[1].r = 42, rod[2].r = 37.5; // 连杆半径 T230
// rod[0].l = 234.5, rod[1].l = 339, rod[2].l = 301.5; // 连杆长度 T230
init_rodindex();
}
// 初始化数学模型参数
void init_model_structure()
{
float M = 1.57;
jr1[6] = -pi/6;
// jr1[0] = -M, jr1[1] = -M, jr1[2] = -0.5, jr1[3] = -M, jr1[4] = -M, jr1[5] = -M, jr1[6] = -pi/6;
// jr2[0] = M, jr2[1] = M, jr2[2] = 0.5, jr2[3] = 0, jr2[4] = M, jr2[5] = M, jr2[6] = M;
gap = abs(jr1[0]);
for (int i = 1; i < 7; i++)
if (abs(jr1[i]) > gap)
gap = abs(jr1[i]);
for (int i = 0; i < 7; i++)
if (abs(jr2[i]) > gap)
gap = abs(jr2[i]);
gap0 = gap;
len[0] = rod[0].l, len[1] = rod[1].l, len[2] = rod[2].l;
P_0[1][0] = 0, P_0[1][1] = len[0], P_0[1][2] = 0;
float goal_j[7] = {0, 0, 0, 0, 0, 0, 0};
l_forward_move(goal_j); // show_value("pos",pos,6);
ypr[0]=ypr[2]=0;
}
// 点或向量在不同坐标系转换（p=true表示点，p=false表示向量）
void fromS0toS1(float P0[3], float P[3], bool p);
void fromS1toS2(float P0[3], float P[3], bool p);
void fromS2toS3(float P0[3], float P[3], bool p);
void fromS3toS4(float P0[3], float P[3], bool p);
void fromS4toS5(float P0[3], float P[3], bool p);
void fromS5toS6(float P0[3], float P[3], bool p);
void fromS6toS7(float P0[3], float P[3], bool p);
void fromS1toS0(float P0[3], float P[3], bool p);
void fromS2toS1(float P0[3], float P[3], bool p);
void fromS3toS2(float P0[3], float P[3], bool p);
void fromS4toS3(float P0[3], float P[3], bool p);
void fromS5toS4(float P0[3], float P[3], bool p);
void fromS6toS5(float P0[3], float P[3], bool p);
void fromS7toS6(float P0[3], float P[3], bool p);
bool solve_in_S6();
bool solve_in_S4();
bool solve_in_S2();
// 逆运动解算器
bool Points2J();
// 根据角度计算点变换矩阵
void J2MatrixT(float T[4][4]);
public:
humanoidLeftArm();
// 检测逆运动解出的角是否满足原始位姿
bool checkacc();
// 正运动
bool l_forward_move(float goal_j[7]);
// 获取当前角度
void get_crt_j(float joints[7]);
// 获取当前位置
void get_crt_pos(float pos[6]);
/*
参数：
pos:目标位置
value：dim的值
dim：-1~2 代表x,y,z，-1的时候是没有臂角约束，只会接收末端位姿，其他参数忽略
absolute：true的时候是绝对位置（以胸部原点位置），false的时候是相对位置（以当前点胳膊轴位置）
*/
// bool l_backward_move(float pos[6], float value, int dim, bool absolute);
bool l_backward_move(float pos[6]);
// 测试fromSi2Sj函数是否正确（是否抄错）
void testj2p();
void show_crt_pos();
void show_crt_j();
};
class humanoidRightArm : public humanoidLeftArm
{
private:
void get_r_ypr(float r_ypr[3]);
public:
humanoidRightArm();
bool r_forward_move(float goal_j[7]);
void get_crt_j(float joints[7]);
void get_crt_pos(float postrue[6]);
// bool r_backward_move(float postrue[6], float value, int dim, bool absolute);
bool r_backward_move(float postrue[6]);
void show_crt_pos();
void show_crt_j();
void showpointsinfo();
};
} // 添加extern "C"
#endif

46
src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h
View File

@ -1,46 +0,0 @@
#ifndef TI5MOVE_H
#define TI5MOVE_H
#include <unistd.h>
#include <cstdlib>
#include <chrono>
#include <thread>
// #include "can/SingleCaninterface.h"
// #include "can/motortypehelper.h"
#include <vector>
#include "tool.h"
#include "Ti5LOGIC.h"
#include <time.h>
#include <mutex>
#include "Ti5CAN_Driver.h"
#define USLEEPTIME 3000
extern "C"{ //添加：extern C
class ArmController{
private:
float AG = 0.005; //启停时变速的采样间距（秒
float scale = 101; //电机内圈与外圈的速度比
float n2p = 655.36; //内圈转速到步速的转化系数
// float n2p = 655.36/101; //双编码器的 内圈转速到步速的转化系数
float mvtime = 0;
// float j2p = scale * 65536 / 2 / pi; //电机外圈角度到内圈步数的转化
float min_time = 0;
bool jstp = false;
uint8_t canidList[IDNUM];
float nplL[4][4]; //add 用于linear_move函数机械臂直线运动
void setn(int npL[IDNUM],int deviceInd,int canInd);
void ACTmove(float *a,float *b,float T0,int deviceInd,int canInd);// 实际运动
public:
ArmController(uint8_t canid[IDNUM]);
// void plan_move(float crtj[IDNUM],int canInd,int deviceInd);
void plan_move(int deviceInd, int canInd,float crtj[IDNUM]);
void new_plan_move(int deviceInd,int canInd,float crtj[IDNUM]);//mfs add 2024-12-3
void GETP_plan_move(float crtj[IDNUM],float *CUrrentJointPosition,int deviceInd,int canInd);
float NMAX = 3000; //所有电机内核最大转速值（(NMAX/100)圈/秒）
float j2p = scale * 65536 / 2 / pi; //电机外圈角度到内圈步数的转化
// float j2p = 131072/pi; //双编码器 电机外圈角度到内圈步数的转化
};
}//添加：extern C
#endif

104
src/devices/robot/ti5_robot/ti5include/controlcan.h
View File

@ -1,104 +0,0 @@
#ifndef CONTROLCAN_H
#define CONTROLCAN_H
////文件版本：v2.02 20190609
//接口卡类型定义
#define VCI_USBCAN1 3
#define VCI_USBCAN2 4
#define VCI_USBCAN2A 4
#define VCI_USBCAN_E_U 20
#define VCI_USBCAN_2E_U 21
//函数调用返回状态值
#define STATUS_OK 1
#define STATUS_ERR 0
#define USHORT unsigned short int
#define BYTE unsigned char
#define CHAR char
#define UCHAR unsigned char
#define UINT unsigned int
#define DWORD unsigned int
#define PVOID void*
#define ULONG unsigned int
#define INT int
#define UINT32 UINT
#define LPVOID void*
#define BOOL BYTE
#define TRUE 1
#define FALSE 0
//1.ZLGCAN系列接口卡信息的数据类型。
typedef struct _VCI_BOARD_INFO{
USHORT hw_Version;
USHORT fw_Version;
USHORT dr_Version;
USHORT in_Version;
USHORT irq_Num;
BYTE can_Num;
CHAR str_Serial_Num[20];
CHAR str_hw_Type[40];
USHORT Reserved[4];
} VCI_BOARD_INFO,*PVCI_BOARD_INFO;
//2.定义CAN信息帧的数据类型。
typedef struct _VCI_CAN_OBJ{
UINT ID;
UINT TimeStamp;
BYTE TimeFlag;
BYTE SendType;
BYTE RemoteFlag;//是否是远程帧
BYTE ExternFlag;//是否是扩展帧
BYTE DataLen;
BYTE Data[8];
BYTE Reserved[3];
}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
//3.定义初始化CAN的数据类型
typedef struct _INIT_CONFIG{
DWORD AccCode;
DWORD AccMask;
DWORD Reserved;
UCHAR Filter;
UCHAR Timing0;
UCHAR Timing1;
UCHAR Mode;
}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
///////// new add struct for filter /////////
typedef struct _VCI_FILTER_RECORD{
DWORD ExtFrame; //是否为扩展帧
DWORD Start;
DWORD End;
}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
#ifdef __cplusplus
#define EXTERN_C extern "C"
#else
#define EXTERN_C
#endif
EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,ULONG Len);
EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,ULONG Len,INT WaitTime);
EXTERN_C DWORD VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
EXTERN_C DWORD VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
#endif

28
src/devices/robot/ti5_robot/ti5include/mathfunc.h
View File

@ -1,28 +0,0 @@
#ifndef MATHFUNC_H
#define MATHFUNC_H
#include<math.h>
#define pi M_PI
extern "C"{//添加extern "C"
float vec_dot(float v1[3],float v2[3]);
float vec_length(float v[3]);
void vec_rescale(float v[3],float l,float nv[3]);
int fx2solv(float a,float b,float c,float *p1,float *p2);
float pointsdistance(float P1[3],float P2[3]);
void vec_subtraction(float v1[3],float v2[3],float v[3]);
void vec_addition(float v1[3],float v2[3],float v[3]);
float vec_angle(float v1[3],float v2[3],float f);
float solve_trangle(float a,float b,float c);
void vec_cross(float v1[3],float v2[3],float v[3]);
float calcu_angle(float sa,float ca,float cp2);
void MatrixmultPoint(float T[4][4],float P0[3],float P[3]);
void Matrixmult(float T1[4][4],float T2[4][4],float T[4][4]);
void MatrixT2MatrixI(float T[4][4],float I[4][4]);
}
#endif

35
src/devices/robot/ti5_robot/ti5include/tool.h
View File

@ -1,35 +0,0 @@
#ifndef TOOL_H
#define TOOL_H
#include <iostream>
#include <string.h>
#include <termio.h>
#include <unistd.h>
using namespace std;
#define IDNUM 7
// extern "C"
// { // 添加extern "C"
void Mcopy(float (*C)[4], float (*P)[4]);
void show_value(string name, float (*T)[4]);
void copy_value(float *copy, float *paste, int n);
void show_value(string name, float *value);
void show_value(string name,float* value,int n);
void show_value(string name, uint32_t *value);
void show_point(string name, float *value);
void show_value(string name, uint8_t *value);
void get_cmdlist(uint8_t *L, uint8_t c);
void get_paralist(uint32_t *L, uint32_t c);
int getch();
char scanKeyboard();
// void test_demo();
// void test();
// } // 添加extern "C"
#endif

12
src/devices/solver/CMakeLists.txt Normal file
View File

@ -0,0 +1,12 @@
add_library(solver SHARED
solver_interface.cpp
)
target_include_directories(solver PUBLIC
${PROJECT_SOURCE_DIR}/include
)
target_link_libraries(solver PRIVATE
cmvr_es::hardware::can
cmvr_es::hardware::serial
)
add_library(cmvr_es::solver ALIAS solver)

57
src/devices/solver/solver_interface.cpp Normal file
View File

@ -0,0 +1,57 @@
//
// Created by linbo on 2025/11/11.
//
#include "solver_interface.h"
#include <glog/logging.h>
using namespace std;
using namespace cmvr::device;
template<int DOF>
std::shared_ptr<SolverInterface<DOF>> SolverInterface<DOF>::instance_ = nullptr;
template<int DOF>
std::once_flag SolverInterface<DOF>::init_flag_;
template<int DOF>
SolverInterface<DOF>::SolverInterface(const XmlNode& cfg) {
try {
cfg_ = cfg;
if (!pathExists(cfg.getAttrString("urdf"))) {
throw runtime_error("urdf file does not exist");
}
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
cfg.getAttrString("urdf"), cfg.getAttrString("baseLink"));
joint_names_ = splitString(cfg.getAttrString("jointNames"), ",");
link_names_ = splitString(cfg.getAttrString("linkNames"), ",");
if (joint_names_.size() != DOF) {
throw runtime_error("joint names size mismatched with dof");
}
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
m_cctrl_ = make_shared<ctrl::QPSolver<DOF> >(m_robot_);
}
catch (const exception &e) {
LOG(FATAL) << "[SolverInterface] (SolverInterface): Device manager initialization failed: " << e.what();
throw std::runtime_error("[SolverInterface] (SolverInterface): Device manager create failed" + string(e.what()));
}
}
template<int DOF>
SolverInterface<DOF>& SolverInterface<DOF>::getInstance(const XmlNode& cfg) {
std::call_once(init_flag_, [&cfg] {
instance_.reset(new SolverInterface(cfg));
});
return *instance_;
}
template<int DOF>
SolverInterface<DOF>& SolverInterface<DOF>::getInstance() {
if (!instance_) {
throw std::runtime_error("[SolverInterface] (getInstance): SolverInterface not initialized. Call getInstance(const XmlNode&) first.");
}
return *instance_;
}
template<int DOF>
void SolverInterface<DOF>::destroyInstance() {
instance_.reset();
}

42
src/devices/solver/solver_interface.h Normal file
View File

@ -0,0 +1,42 @@
//
// Created by linbo on 2025/11/11.
//
#ifndef CMVR_ES_SOVLER_INTERFACE_H
#define CMVR_ES_SOVLER_INTERFACE_H
#include <mutex>
#include "rapidxml/xml_parser.h"
#include "utils/solver/qp_solver.h"
#include "utils/base/os.h"
#include "utils/base/timer.h"
#include "utils/base/ring_buffer.h"
namespace cmvr::device
{
template<int DOF>
class SolverInterface
{
public:
SolverInterface(const SolverInterface&) = delete;
SolverInterface& operator=(const SolverInterface&) = delete;
static SolverInterface& getInstance(const XmlNode& cfg);
static SolverInterface& getInstance();
static void destroyInstance();
private:
explicit SolverInterface(const XmlNode &cfg);
private:
XmlNode cfg_;
static std::once_flag init_flag_;
static std::shared_ptr<SolverInterface> instance_;
std::vector<std::string> joint_names_;
std::vector<std::string> link_names_;
std::shared_ptr<cmvr::dyn::State<DOF>> m_state_;
std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_;
std::shared_ptr<cmvr::ctrl::QPSolver<DOF>> m_cctrl_;
};
}
#endif //CMVR_ES_SOVLER_INTERFACE_H

15
src/hardware/CMakeLists.txt
View File

@ -1,18 +1,3 @@
add_subdirectory(can)
add_subdirectory(serial)
add_library(hardware SHARED
esp32_serial_port.cpp
serial_interface.cpp
)
target_include_directories(hardware PUBLIC
${PROJECT_SOURCE_DIR}/include
)
target_link_libraries(hardware PRIVATE
cmvr_es::can
cmvr_es::serial
)
add_library(cmvr_es::hardware ALIAS hardware)

2
src/hardware/can/CMakeLists.txt
View File

@ -11,4 +11,4 @@ target_link_libraries(can PRIVATE
cmvr_es::motor_protocol
)
add_library(cmvr_es::can ALIAS can)
add_library(cmvr_es::hardware::can ALIAS can)

50
src/hardware/can/can_group.cpp
View File

@ -5,21 +5,63 @@
#include "hardware/can/can_group.h"
#include <glog/logging.h>
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
CanGroup::CanGroup(const XmlNode &cfg)
CanGroup::CanGroup(const XmlNode &cfg):cfg_(cfg)
{
try {
id_ = cfg.getAttrString("id");
channelId_ = cfg.getAttrDefault("channelId",0);
enable_ = cfg.getAttrDefault("enable", false);
motor_protocol_manager = std::make_shared<MotorProtocolManager>(cfg);
}
catch (const exception &e) {
LOG(FATAL) << "[CanGroup] (CanGroup): CanGroup initialization failed: " << e.what();
throw std::runtime_error("[CanGroup] (CanGroup): CanGroup create failed" + string(e.what()));
}
}
void CanGroup::init()
{
can_client_ = std::make_shared<device::SocketCanClientRaw>(cfg_);
can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
can_receiver_ = std::make_shared<device::CanReceiver<msgs::RobotDetail> >();
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
//初始化protocol协议实例
can_client_->init();
auto ret = can_sender_->Init(can_client_.get(), false);
if (ret != device::ErrorCode::OK)
LOG(ERROR) << "Failed to init " << id_ << " CAN sender.";
ret = can_receiver_->Init(can_client_.get(), message_manager_.get(), false);
if (ret != device::ErrorCode::OK)
LOG(ERROR) << "Failed to init " << id_ << " CAN receiver.";
can_client_->start();
ret = can_sender_->Start();
if (ret != device::ErrorCode::OK)
LOG(ERROR) << "Failed to start " << id_ << " CAN sender.";
ret = can_receiver_->Start();
if (ret != device::ErrorCode::OK)
LOG(ERROR) << "Failed to start " << id_ << " CAN receiver.";
motor_protocol_manager = std::make_shared<MotorProtocolManager>(cfg_,can_sender_,message_manager_);
}
std::shared_ptr<AbstractMotorProtocol> CanGroup::getMotorProtocol(const std::string &protocolType)
{
return motor_protocol_manager->getMotorProtocol(protocolType);
}
uint8_t CanGroup::getNodeId(const std::string& joint_name)
{
return motor_protocol_manager->getNodeId(joint_name);
}
const MotorProtocolManager::MotorInfo& CanGroup::getMotorInfo(const std::string& joint_name)
{
return motor_protocol_manager->getMotorInfo(joint_name);
}

7
src/hardware/can/can_manager.cpp
View File

@ -6,7 +6,7 @@
#include <glog/logging.h>
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
CanManager::CanManager(const XmlNode& cfg)
{
@ -18,7 +18,10 @@ CanManager::CanManager(const XmlNode& cfg)
string id = node.getAttrString("id");
if (canGroups_.count(id))
throw runtime_error("[CanManager](CanManager): duplicate CanGroup id:" + id);
canGroups_[id] = std::make_shared<CanGroup>(cfg);
auto canGroup = std::make_shared<CanGroup>(cfg);
canGroup->init();
canGroups_[id] = canGroup;
}
}
}

89
src/hardware/can/motor_protocol/motorprotocolmanager.cpp
View File

@ -7,24 +7,46 @@
#include <glog/logging.h>
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg)
MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager)
{
can_sender_ = sender;
message_manager_ = manager;
try {
for (auto &node: cfg.getChildren()){
if (node.getNodeName() == "Ti5MotorProtocol")
{
std::string id = node.getAttrString("id");
std::string joint_name = node.getAttrString("joint_name");
if (motor_protocols_.count(joint_name))
throw runtime_error("[MotorProtocolManager] (MotorProtocolManager): duplicate motorProtocol joint name:" + joint_name);
if (cfg.hasChild("Ti5MotorProtocol"))
{
auto protocol = std::make_shared<Ti5MotorProtocol>(cfg,can_sender_,message_manager_);
motor_protocols_["Ti5MotorProtocol"] = protocol;
std::vector<MotorInfo> motors;
for (auto &node: cfg.getChildren()){
if (node.getNodeName() == "Ti5MotorProtocol")
{
MotorInfo motor;
motor.node_id = node.getAttrDefault("id",0);
if (motor.node_id == 0)
{
throw std::runtime_error("MotorProtocolManager: Can't find motor id");
}
motor.jointName = node.getAttrString("joint_name");
motor.limitQd = node.getAttrDefault("limitQd",3.0f);
motor.limitQLb = node.getAttrDefault("limitQLb",3.14f);
motor.limitQUb = node.getAttrDefault("limitQUb",3.14f);
motor_protocols_[joint_name] = std::make_shared<Ti5MotorProtocol>(cfg);
//初始化参数
protocol->configPdo(motor.node_id);
protocol->configProfile(motor.node_id,2000,2000,2000);
protocol->seedNmtRequest(motor.node_id,msgs::NMT_ENTER_PRE_OPERATIONAL);
protocol->seedNmtRequest(motor.node_id,msgs::NMT_START_REMOTE_NODE);
protocol->seedSdoRequest(motor.node_id, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06);
protocol->seedSdoRequest(motor.node_id, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F);
motors.emplace_back(motor);
}
}
motors_["Ti5MotorProtocol"] = motors;
}
}
catch (const exception &e) {
LOG(FATAL) << "[MotorProtocolManager] (MotorProtocolManager): MotorProtocolManager initialization failed: " << e.what();
@ -33,9 +55,44 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg)
}
std::shared_ptr<AbstractMotorProtocol> MotorProtocolManager::getMotorProtocol(const std::string &joint_name)
std::shared_ptr<AbstractMotorProtocol> MotorProtocolManager::getMotorProtocol(const std::string &protocolType)
{
if (motor_protocols_.count(joint_name))
return motor_protocols_[joint_name];
return nullptr;
if (motor_protocols_.count(protocolType))
return motor_protocols_[protocolType];
throw runtime_error("Can't find motor protocol: " + protocolType);
}
uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name)
{
uint8_t node_id = 0;
if (motors_.count("Ti5MotorProtocol"))
{
const auto motors = motors_["Ti5MotorProtocol"];
for (auto &motor: motors)
{
if (motor.jointName == joint_name)
{
node_id = motor.node_id;
break;
}
}
}
throw runtime_error("Can't find motor nodeId: " + joint_name);
}
const MotorProtocolManager::MotorInfo& MotorProtocolManager::getMotorInfo(const std::string& joint_name)
{
if (motors_.count("Ti5MotorProtocol"))
{
const auto motors = motors_["Ti5MotorProtocol"];
for (auto &motor: motors)
{
if (motor.jointName == joint_name)
{
return motor;
}
}
}
throw runtime_error("Can't find motor info: " + joint_name);
}

487
src/hardware/can/motor_protocol/ti5motorprotocol.cpp
View File

@ -3,9 +3,494 @@
//
#include "hardware/can/motor_protocol/ti5motorprotocol.h"
#include "canbus/canopen/register.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
#include "canbus/canopen/nmt_response_protocol.h"
using namespace std;
using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::hardware;
Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg):AbstractMotorProtocol(cfg)
{
}
}
Ti5MotorProtocol::Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<CanSender<msgs::RobotDetail> > sender,
std::shared_ptr<MessageManager<msgs::RobotDetail> > manager)
: AbstractMotorProtocol(cfg),can_sender_(sender), message_manager_(manager){
comm_proto = CommProto::CANOPEN;
// 添加 message
// NMT
message_manager_->AddSendProtocolData<NmtRequestProtocol<RobotDetail>, false>();
//sync
message_manager_->AddSendProtocolData<SyncProtocol<RobotDetail>, false>();
// nmt
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
if (nmt_command_ == nullptr) {
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
}
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
// sync
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
if (sync_command_ == nullptr) {
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
}
can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
}
Ti5MotorProtocol::~Ti5MotorProtocol()
{
}
bool Ti5MotorProtocol::initNode(uint8_t node_id) {
//nmt
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
//sdo
message_manager_->AddSendProtocolData<SdoRequestProtocol<RobotDetail>, false>(node_id);
message_manager_->AddRecvProtocolData<motor::Ti5MotorSdoResponse, false>(node_id);
//TPDO
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO1,false>(node_id);
message_manager_->AddRecvProtocolData<motor::Ti5MotorTPDO2,false>(node_id);
//RPDO
message_manager_->AddSendProtocolData<motor::Ti5MotorRPDO1,false>(node_id);
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
if (sdo_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
// pdo1
rpdo1_commands_[node_id] = dynamic_cast<motor::Ti5MotorRPDO1 *>(
message_manager_->GetMutableProtocolDataById(motor::Ti5MotorRPDO1::ID(node_id)));
if (rpdo1_commands_[node_id] == nullptr) {
LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
}
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
return ErrorCode::OK;
}
void Ti5MotorProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
uint32_t data, uint32_t delay_ms) {
sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
can_sender_->Update(sdo_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void Ti5MotorProtocol::setQ(uint8_t node_id, double angle_rad) {
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (cur_mode_[node_id]) {
case RUN_MODE_CYCLIC_SYNC_POSITION:
setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
break;
case RUN_MODE_PROFILE_POSITION:
setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
break;
default:
break;
}
}
void Ti5MotorProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
controlword_t cw = {};
cw.switch_on = 1;
cw.enable_voltage = 1;
cw.enable_operation = 1;
cw.quick_stop = 1;
cw.change_set_immediately = 1;
// 1. 设置目标位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
// 2. 设置触发位（bit4 = 1）
cw.new_set_point = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 3. 清除触发位（bit4 = 0），准备下一次触发
cw.new_set_point = 0;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
}
void Ti5MotorProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
// 触发目标位置运动
controlword_t cw;
cw.value = 0x0F;
cw.new_set_point = 1;
cw.change_set_immediately = 1;
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
cw.new_set_point = 0;
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
cur_mode_[node_id] = mode;
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
// 2 : 状态机步进 —— Shutdown（0x06）
controlword_t cw = {};
cw.quick_stop = 1;
cw.enable_voltage = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
cw.switch_on = 1;
cw.enable_operation = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
switch (mode) {
case RUN_MODE_PROFILE_POSITION: {
// 4 : 设置目标位置（为当前位置）
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
// 5 : 触发位置运动（new_set_point 翻转）
cw.new_set_point = 1;
cw.change_set_immediately = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
cw.new_set_point = 0;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_CYCLIC_SYNC_POSITION: {
// 设置目标位置为当前位置
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
//3 : 使能 15
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_PROFILE_VELOCITY: {
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
default:
break;
}
}
void Ti5MotorProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
nmt_command_->RequestService(node_id, command);
can_sender_->Update(nmt_command_->ID);
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void Ti5MotorProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
}
void Ti5MotorProtocol::configTPDO1(uint8_t node_id) {
//TDPO1 配置 状态字 和 控制字
// 1: 失能 pdo
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0);
// 2: 配置为异步
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// 3：配置约束时间 unit:0.1ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10);
// 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0);
// 5 ：映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1,
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
//6 : 映射状态字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2,
STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
//7 : 映射模式
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3,
MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
//8 映射错误码
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4,
ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
//9 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4);
//10 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorProtocol::configTPDO2(uint8_t node_id) {
// 1: 失能 pdo
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0);
// 2: 配置为异步
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// 3：配置约束时间 unit:0.1ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100);
// 4 : 配置周期发送时间 unit : ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0);
// 5 ：映射当前位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1,
ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射当前速度
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2,
ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
//9 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2);
//10 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorProtocol::configRPDO1(uint8_t node_id, bool start) {
// 1: 失能 pdo
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0);
// 2: 配置为
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// // 3：配置约束时间 unit:0.1ms
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
//
// // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
// 5 ：映射位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1,
TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
if (start) {
//7 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
//8 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
}
}
void Ti5MotorProtocol::configPdo(uint8_t node_id) {
configTPDO1(node_id);
configTPDO2(node_id);
configRPDO1(node_id, true);
}
void Ti5MotorProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
}
void Ti5MotorProtocol::setLimitQd(uint8_t node_id,double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
}
void Ti5MotorProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
}
bool Ti5MotorProtocol::calibrateZeroQ(uint8_t node_id) {
// 0: 设置控制字为 0x06，确保停机状态
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06,1000);
// 1: 清除偏置值 0x2008 ← 0
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
// 2: 等待确认清除成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
}, 1000)) {
LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
return false;
}
// 3: 读取当前位置 0x6064
seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
// 4: 将当前位置写入偏置寄存器
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
// 5: 保存参数到永久区（0x2000 ← 1）
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1,100);
// 6: 确认写入成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
}, 500)) {
return false;
LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
}
return true;
}
void Ti5MotorProtocol::brake(uint8_t node_id) {
// // 开机未使能电机时调用
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 6 抱闸 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,100);
// 必须要发送 0xf 才能按照6085中设定的减速度减速
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
}
bool Ti5MotorProtocol::reachedTargetQ(uint8_t node_id) {
statusword_t st{};
st.value = GetRobotDetail()->motors().at(node_id).status_word();
return st.target_reached == 1 ;
}
void Ti5MotorProtocol::setQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (cur_mode_[node_id]) {
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION: {
auto it = last_Qd_.find(node_id);
if (it == last_Qd_.end() || it->second != speed) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),0);
last_Qd_[node_id] = speed;
}
break;
}
case msgs::RUN_MODE_PROFILE_VELOCITY: {
// 在速度模式下，直接设置目标速度
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
break;
}
default:
break;
}
}
void Ti5MotorProtocol::setQdd(uint8_t node_id, double qdd) {
uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
auto it = last_Qdd_.find(node_id);
if (it == last_Qdd_.end() || it->second != accel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
last_Qdd_[node_id] = accel;
}
}
void Ti5MotorProtocol::torqueOff(uint8_t node_id) {
// 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02,20);
// 必须要发送 0xf 才能按照6085中设定的减速度减速
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 停机之后，要重新使能？
// cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED;
}
double Ti5MotorProtocol::getQ(uint8_t node_id) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).position();
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
}
double Ti5MotorProtocol::getQd(uint8_t node_id) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).speed();
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
}

8
src/hardware/serial/CMakeLists.txt
View File

@ -1,3 +1,7 @@
#add_subdirectory(IO)
add_subdirectory(RS485)
add_library(serial
serial_manager.cpp
)
@ -5,7 +9,7 @@ add_library(serial
target_include_directories(serial PUBLIC ${PROJECT_SOURCE_DIR}/include)
target_link_libraries(serial PRIVATE
protobuf::libprotobuf
cmvr_es::hardware::rs485
)
add_library(cmvr_es::serial ALIAS serial)
add_library(cmvr_es::hardware::serial ALIAS serial)

9
src/hardware/serial/RS485/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
add_library(rs485 SHARED
esp32_serial_port.cpp
)
target_include_directories(rs485 PUBLIC
${PROJECT_SOURCE_DIR}/include
)
add_library(cmvr_es::hardware::rs485 ALIAS rs485)

2
src/hardware/esp32_serial_port.cpp → src/hardware/serial/RS485/esp32_serial_port.cpp
View File

@ -1,5 +1,5 @@
// esp32_serial_port.cpp
#include "hardware/esp32_serial_port.h"
#include "hardware/serial/RS485/esp32_serial_port.h"
#include <iostream>
#include <fcntl.h>
#include <unistd.h>

2
src/hardware/serial/serial_manager.cpp
View File

@ -5,7 +5,7 @@
#include "hardware/serial/serial_manager.h"
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
SerialManager::SerialManager(const XmlNode &cfg)
{

512
src/hardware/serial_interface.cpp
View File

@ -1,512 +0,0 @@
//
// Created by xtkuang on 2025/5/6.
//
#include "hardware/serial_interface.h"
#include <iostream>
#include <bitset>
#include <vector>
#include <string>
#include <iomanip>
#include <sstream>
#include <random>
#include <chrono>
#include <glog/logging.h>
#include <boost/asio.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::asio;
// 地址转换和二进制转十六进制函数
std::string convertAddressToBinary_read(int address_dec, const std::string& id_value) {
std::string address_bin = std::bitset<32>(address_dec).to_string();
size_t first_one = address_bin.find('1');
if (first_one != std::string::npos) {
address_bin = address_bin.substr(first_one);
} else {
address_bin = "0";
}
std::string formatted_output = "0000000" + address_bin + "000000000000" + id_value;
return formatted_output;
}
std::string convertAddressToBinary_write(int address_dec, const std::string& id_value) {
std::string address_bin = std::bitset<32>(address_dec).to_string();
size_t first_one = address_bin.find('1');
if (first_one != std::string::npos) {
address_bin = address_bin.substr(first_one);
} else {
address_bin = "0";
}
std::string formatted_output = "0000010" + address_bin + "000000000000" + id_value;
return formatted_output;
}
std::string binaryToHex(const std::string& binary_string) {
unsigned long decimal_value = std::stoul(binary_string, nullptr, 2);
std::stringstream ss;
ss << std::hex << std::uppercase << decimal_value;
return ss.str();
}
// 处理十六进制数据的函数
std::vector<unsigned char> processHexData(const std::string& hex_data) {
std::string padded_hex = hex_data;
if (padded_hex.length() < 8) {
padded_hex = std::string(8 - padded_hex.length(), '0') + padded_hex;
}
std::vector<unsigned char> processed_data;
for (size_t i = padded_hex.length(); i > 0; i -= 2) {
std::string byte_str = padded_hex.substr(i - 2, 2);
unsigned char byte_value = static_cast<unsigned char>(std::stoi(byte_str, nullptr, 16));
processed_data.push_back(byte_value);
}
return processed_data;
}
namespace cmvr {
// 构造函数，初始化串口参数
serial_interface::serial_interface(const std::string& port, unsigned int baudrate, CommunicationMode mode) : io(), comm_mode(mode),timer(io), timeout(false) {
try {
serial = std::make_shared<serial_port>(io, port);
serial->set_option(serial_port_base::baud_rate(baudrate));
serial->set_option(serial_port_base::character_size(8)); // 数据位
serial->set_option(serial_port_base::parity(serial_port_base::parity::none)); // 无校验位
serial->set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one)); // 1个停止位
is_initialized = true;
}
catch (const std::exception& e) {
is_initialized = false;
LOG(ERROR) << "[serial_interface] ([serial_interface]): Failed to init serialport: " << e.what();
}
}
bool serial_interface::isInitialized() {
return is_initialized;
}
void serial_interface::on_timeout(const boost::system::error_code& ec) {
if (!ec) {
timeout = true;
serial->cancel();
}
}
void serial_interface::on_read_complete(const boost::system::error_code& ec, size_t bytes_transferred, boost::system::error_code* out_ec, size_t* out_bytes_transferred) {
*out_ec = ec;
*out_bytes_transferred = bytes_transferred;
timer.cancel();
}
// 向串口写入数据
void serial_interface::write(const std::vector<unsigned char>& data) {
if (comm_mode == CommunicationMode::RS485) {
boost::asio::write(*serial, buffer(data));
} else if (comm_mode == CommunicationMode::CAN) {
// CAN模式下的写入逻辑，这里简化为和RS485相同，可根据实际情况修改
boost::asio::write(*serial, buffer(data));
}
}
// 从串口读取指定长度的数据
std::vector<unsigned char> serial_interface::read(size_t length) {
std::vector<unsigned char> buffer(length);
if (comm_mode == CommunicationMode::RS485) {
boost::asio::read(*serial, boost::asio::buffer(buffer.data(), length));
} else if (comm_mode == CommunicationMode::CAN) {
timeout = false;
int timeout_ms = 1000;
timer.expires_after(std::chrono::milliseconds(timeout_ms));
timer.async_wait(boost::bind(&serial_interface::on_timeout, this, boost::asio::placeholders::error));
boost::system::error_code ec;
size_t bytes_transferred = 0;
serial->async_read_some(boost::asio::buffer(buffer),
boost::bind(&serial_interface::on_read_complete, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred, &ec, &bytes_transferred));
io.run();
io.reset();
if (timeout) {
throw std::runtime_error("串口读取超时");
}
if (ec) {
throw boost::system::system_error(ec);
}
buffer.resize(bytes_transferred);
}
return buffer;
}
CommunicationMode serial_interface::getCommunicationMode()
{
return comm_mode;
}
// 根据寄存器名称获取地址
int getAddressByName(const std::string& reg_name) {
for (const auto& reg : regdict) {
if (reg.name == reg_name) {
return reg.address; // 返回匹配的地址
}
}
return -1; // 未找到寄存器名称，返回 -1
}
// 写寄存器函数
void write_register(serial_interface& serial, int address_decimal, const std::string& id_value, const std::vector<int>& values_to_write) {
if (values_to_write.size() > 6) {
std::cerr << "超过允许的值数量，最多只能写入6个值！" << std::endl;
return;
}
if (serial.getCommunicationMode() == CommunicationMode::RS485) {
// RS485模式下的写寄存器逻辑
std::vector<unsigned char> send_buffer;
send_buffer.push_back(0xEB);
send_buffer.push_back(0x90);
send_buffer.push_back(static_cast<unsigned char>(std::stoi(id_value))); // 将 ID 转换为 unsigned char
size_t num_values = values_to_write.size();
auto data_length = static_cast<unsigned char>(num_values * 2 + 3);
send_buffer.push_back(data_length);
send_buffer.push_back(0x12); // 写寄存器命令
send_buffer.push_back(static_cast<unsigned char>(address_decimal & 0xFF)); // 地址低八位
send_buffer.push_back(static_cast<unsigned char>((address_decimal >> 8) & 0xFF)); // 地址高八位
// 将要写入的值添加到数据包中
for (int value : values_to_write) {
send_buffer.push_back(static_cast<unsigned char>(value & 0xFF)); // 写入值低八位
send_buffer.push_back(static_cast<unsigned char>((value >> 8) & 0xFF)); // 写入值高八位
}
// 计算校验和，从 ID 部分开始
unsigned char checksum = 0;
for (size_t k = 2; k < send_buffer.size(); ++k) {
checksum += send_buffer[k];
}
send_buffer.push_back(checksum & 0xFF); // 添加校验和
// // 输出发送的数据
// std::cout << "发送的写入数据: ";
// for (auto byte : send_buffer) {
// std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " ";
// }
// std::cout << std::endl;
// 写入数据
serial.write(send_buffer);
// 读取并丢弃写入后的响应
try {
std::vector<unsigned char> discard_buffer = serial.read(9);
std::cout << "写操作后的响应数据已丢弃，字节数: " << discard_buffer.size() << std::endl;
} catch (const std::exception& e) {
std::cerr << "写操作后的响应数据读取失败: " << e.what() << std::endl;
}
}
else if (serial.getCommunicationMode() == CommunicationMode::CAN) {
// CAN模式下的写寄存器逻辑
std::vector<int> first_chunk(values_to_write.begin(), values_to_write.begin() + std::min(4, (int)values_to_write.size()));
std::vector<int> second_chunk(values_to_write.begin() + first_chunk.size(), values_to_write.end());
// 构造并发送第一段数据
auto construct_and_send = [&](const std::vector<int>& chunk, int addr) {
// 转换地址为二进制->十六进制
std::string result_bin = convertAddressToBinary_write(addr, id_value);
std::string result_hex = binaryToHex(result_bin);
std::vector<unsigned char> processed_data = processHexData(result_hex);
// 构造数据帧
std::vector<unsigned char> send_buffer;
send_buffer.push_back(0xAA);
send_buffer.push_back(0xAA);
send_buffer.insert(send_buffer.end(), processed_data.begin(), processed_data.end());
// 写入值
for (int value : chunk) {
send_buffer.push_back(value & 0xFF); // 低八位
send_buffer.push_back((value >> 8) & 0xFF); // 高八位
}
while (send_buffer.size() < 14) {
send_buffer.push_back(0x00);
}
// 添加固定字节
send_buffer.push_back(chunk.size() * 2); // 数据长度
send_buffer.push_back(0x00);
send_buffer.push_back(0x01);
send_buffer.push_back(0x00);
// 计算校验和
unsigned char checksum = 0;
for (size_t k = 2; k < send_buffer.size(); ++k) {
checksum += send_buffer[k];
}
send_buffer.push_back(checksum & 0xFF);
send_buffer.push_back(0x55);
send_buffer.push_back(0x55);
// 打印并发送
// std::cout << "发送的完整数据: ";
// for (auto byte : send_buffer) {
// std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " ";
// }
// std::cout << std::endl;
serial.write(send_buffer);
// 读掉响应数据，不处理
try {
std::vector<unsigned char> discard_buffer = serial.read(32);
} catch (const std::exception& e) {
std::cerr << "写操作后的响应数据读取失败: " << e.what() << std::endl;
}
};
// 发送第一段
construct_and_send(first_chunk, address_decimal);
// 如果有第二段值，发送第二段
if (!second_chunk.empty()) {
construct_and_send(second_chunk, address_decimal + 8); // 地址后移 8
}
}
}
// 读寄存器函数
std::vector<unsigned char> read_register(serial_interface& serial, int address_decimal, const std::string& id_value,
size_t register_length, size_t length_to_read) {
std::vector<unsigned char> send_buffer;
if (serial.getCommunicationMode() == CommunicationMode::RS485) {
// RS485模式下的读寄存器逻辑
send_buffer.push_back(0xEB);
send_buffer.push_back(0x90);
send_buffer.push_back(static_cast<unsigned char>(std::stoi(id_value))); // 将 ID 转换为 unsigned char
send_buffer.push_back(0x04);
send_buffer.push_back(0x11);
send_buffer.push_back(static_cast<unsigned char>(address_decimal & 0xFF)); // 地址低八位
send_buffer.push_back(static_cast<unsigned char>((address_decimal >> 8) & 0xFF)); // 地址高八位
send_buffer.push_back(register_length); // 读取寄存器长度
// 计算校验和
unsigned char checksum = 0;
for (size_t k = 2; k < send_buffer.size(); ++k) {
checksum += send_buffer[k];
}
send_buffer.push_back(checksum & 0xFF);
// std::cout << "发送的完整数据: ";
// for (auto byte : send_buffer) {
// std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " ";
// }
// std::cout << std::endl;
serial.write(send_buffer); // 写入串口
// 读取响应数据
std::vector<unsigned char> received_data = serial.read(length_to_read);
// // 打印接收到的原始响应数据
// std::cout << "接收到的原始响应数据: ";
// for (auto byte : received_data) {
// std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " ";
// }
// std::cout << std::endl;
return received_data;
}
return send_buffer;
}
std::vector<int> read_register_can(serial_interface& serial, int address_decimal, const std::string& id_value, size_t register_length, size_t length_to_read, bool parse_as_short) {
std::vector<int> parsed_values;
size_t bytes_read = 0;
while (bytes_read < register_length) {
std::vector<unsigned char> send_buffer;
// 生成地址相关数据
std::string result_bin = convertAddressToBinary_read(address_decimal, id_value);
std::string result_hex = binaryToHex(result_bin);
std::vector<unsigned char> processed_data = processHexData(result_hex);
// 动态计算数据长度控制字段（关键调整）
size_t current_remaining = register_length - bytes_read;
size_t data_length = (parse_as_short)
? std::min(8UL, (current_remaining + 1) & ~1UL) // 按short对齐到偶数
: std::min(8UL, current_remaining); // 按byte无需对齐
// 构建发送缓冲区
send_buffer.push_back(0xAA);
send_buffer.push_back(0xAA);
send_buffer.insert(send_buffer.end(), processed_data.begin(), processed_data.end());
send_buffer.push_back(static_cast<unsigned char>(data_length)); // 动态数据长度
send_buffer.insert(send_buffer.end(), 7, 0x00);
send_buffer.push_back(0x01);
send_buffer.push_back(0x00);
send_buffer.push_back(0x01);
send_buffer.push_back(0x00);
// 计算校验和
unsigned char checksum = 0;
for (size_t k = 2; k < send_buffer.size(); ++k) {
checksum += send_buffer[k];
}
send_buffer.push_back(checksum & 0xFF);
send_buffer.push_back(0x55);
send_buffer.push_back(0x55);
// 发送并接收数据
serial.write(send_buffer);
std::vector<unsigned char> received_data = serial.read(length_to_read);
// 定位有效数据起始位置
size_t start_pos = 0;
while (start_pos < received_data.size() && received_data[start_pos] != 0xAA) {
start_pos++;
}
// 根据解析类型处理数据
if (parse_as_short) {
size_t shorts_to_read = data_length / 2;
for (size_t j = start_pos + 6; j < start_pos + 6 + data_length; j += 2) {
if (j + 1 < received_data.size()) {
int value = (received_data[j + 1] << 8) | received_data[j];
if (value > 6000) value = 0;
parsed_values.push_back(value);
bytes_read += 2;
}
}
} else {
for (size_t j = start_pos + 6; j < start_pos + 6 + data_length; j++) {
if (j < received_data.size()) {
parsed_values.push_back(received_data[j]);
bytes_read++;
}
}
}
address_decimal += static_cast<int>(data_length); // 地址后移8字节
}
return parsed_values;
}
std::vector<unsigned char> read_register_can_single(serial_interface& serial, int address_decimal, const std::string& id_value, size_t register_length, size_t length_to_read) {
std::vector<unsigned char> all_parsed_values;
size_t bytes_read = 0;
while (bytes_read < register_length) {
std::vector<unsigned char> send_buffer;
std::vector<unsigned char> parsed_values;
std::string result_bin = convertAddressToBinary_read(address_decimal, id_value);
std::string result_hex = binaryToHex(result_bin);
std::vector<unsigned char> processed_data = processHexData(result_hex);
send_buffer.push_back(0xAA);
send_buffer.push_back(0xAA);
send_buffer.insert(send_buffer.end(), processed_data.begin(), processed_data.end());
send_buffer.push_back(0x08); // 数据长度控制
send_buffer.insert(send_buffer.end(), 7, 0x00);
send_buffer.push_back(0x01);
send_buffer.push_back(0x00);
send_buffer.push_back(0x01);
send_buffer.push_back(0x00);
unsigned char checksum = 0;
for (size_t k = 2; k < send_buffer.size(); ++k) {
checksum += send_buffer[k];
}
send_buffer.push_back(checksum & 0xFF); // 添加校验和
send_buffer.push_back(0x55);
send_buffer.push_back(0x55);
serial.write(send_buffer);
// 读取响应数据
std::vector<unsigned char> received_data = serial.read(length_to_read);
// 找到第一个 0xAA 的位置
size_t start_pos = 0;
while (start_pos < received_data.size() && received_data[start_pos] != 0xAA) {
start_pos++;
}
// 从第一个 0xAA 开始解析数据，每次最多读取 8 字节
size_t bytes_to_read = std::min(8UL, register_length - bytes_read);
for (size_t j = start_pos + 6; j < start_pos + 6 + bytes_to_read; j++) {
if (j < received_data.size()) {
parsed_values.push_back(received_data[j]);
}
}
all_parsed_values.insert(all_parsed_values.end(), parsed_values.begin(), parsed_values.end());
bytes_read += parsed_values.size();
address_decimal += 8; // 地址后移 8 字节
}
return all_parsed_values;
}
std::vector<int> parse_register(const std::vector<unsigned char>& data,int start_byte, size_t register_length,int per_byte){
std::vector<int> parsed_values;
for (size_t j = 7; j < register_length + 7; j += 2) {
if (j + 1 < data.size()) {
int value = 0;
//根据数据位数获取数据
if (per_byte == 1) {
value = data[j];
}
else if (per_byte == 2) {
value = (data[j + 1] << 8) | data[j];
}
if (value > 6000) {
value = 0;
}
parsed_values.push_back(value);
}
}
return parsed_values;
}
std::vector<std::string> parse_error(unsigned char errorCode) {
std::vector<std::string> faults;
// 直接判断每一位是否为1
if (errorCode & (1 << 0)) { // Bit0: 堵转故障
faults.emplace_back("堵转故障");
}
if (errorCode & (1 << 1)) { // Bit1: 过温故障
faults.emplace_back("过温故障");
}
if (errorCode & (1 << 2)) { // Bit2: 过流故障
faults.emplace_back("过流故障");
}
if (errorCode & (1 << 3)) { // Bit3: 电机异常
faults.emplace_back("电机异常");
}
if (errorCode & (1 << 4)) { // Bit4: 通讯故障
faults.emplace_back("通讯故障");
}
return faults;
}
}

7
src/hardware_manager/CMakeLists.txt
View File

@ -1,12 +1,13 @@
add_library(hardware_manager SHARED
hardware_factory.cpp
hardware_manager.cpp
../hardware/can/can_manager.cpp
../hardware/serial/serial_manager.cpp
)
target_include_directories(hardware_manager PUBLIC
${PROJECT_SOURCE_DIR}/include
)
target_link_libraries(hardware_manager PRIVATE
cmvr_es::hardware::can
cmvr_es::hardware::serial
)
add_library(cmvr_es::hardware_manager ALIAS hardware_manager)

2
src/hardware_manager/hardware_factory.cpp
View File

@ -6,7 +6,7 @@
#include <glog/logging.h>
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
template std::shared_ptr<CanManager> HardwareFactory::create<CanManager>(const XmlNode&);

3
src/hardware_manager/hardware_manager.cpp
View File

@ -4,8 +4,9 @@
#include "hardware_manager/hardware_manager.h"
using namespace std;
using namespace cmvr::device;
using namespace cmvr::hardware;
std::shared_ptr<HardWareManager> HardWareManager::instance_ = nullptr;
std::once_flag HardWareManager::init_flag_;
HardWareManager::HardWareManager(const XmlNode& cfg) {
try {

143
src/main.cpp
View File

@ -8,6 +8,7 @@
#include <iostream>
#include <glog/logging.h>
#include "device_manager/device_manager.h"
#include "hardware_manager/hardware_factory.h"
#include "monitor/monitor_manager.h"
#include "service/grpc_service/grpc_camera_service.h"
#include "service/grpc_service/grpc_system_service.h"
@ -17,28 +18,36 @@
#include "utils/base/logger.h"
#include "service/grpc_service/grpc_head_service.h"
#include "service/grpc_service/grpc_humanoid_robot_service.h"
#include "service/grpc_service/grpc_hlc_service.h"
#include "service/http_service/httpclient.h"
#include "json/json.h"
#include "hardware_manager/hardware_manager.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
void runServer(const XmlNode &cfg){
using namespace cmvr::device;
using namespace cmvr::service;
using namespace cmvr::monitor;
using namespace cmvr::hardware;
if (!cfg.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
return;
}
try {
//启用反射
grpc::reflection::InitProtoReflectionServerBuilderPlugin();
auto hardware_cfg = cfg.getChild("HardwareManager");
HardWareManager::getInstance(hardware_cfg);
auto dmgr_cfg = cfg.getChild("DeviceManager");
DeviceManager::getInstance(dmgr_cfg);
auto mmgr_cfg = cfg.getChild("MonitorManager");
MonitorManager::getInstance(mmgr_cfg);
auto grpc_cfg = cfg.getChild("gRPCServer");
auto server_cfg = cfg.getChild("Server");
auto grpc_cfg = server_cfg.getChild("gRPCServer");
string port = grpc_cfg.getAttrDefault("port", "50051");
std::string address("0.0.0.0:" + port);
@ -49,7 +58,6 @@ void runServer(const XmlNode &cfg){
auto dexhand_service = gRPCDexHandServiceImpl();
auto biohand_service = gRPCMBioHeadServiceImpl();
auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl();
auto hlc_service = gRPCHlcServiceImpl();
grpc::ServerBuilder builder;
builder.AddListeningPort(address, grpc::InsecureServerCredentials());
@ -60,7 +68,6 @@ void runServer(const XmlNode &cfg){
builder.RegisterService(&dexhand_service);
builder.RegisterService(&biohand_service);
builder.RegisterService(&humanoid_robot_service);
builder.RegisterService(&hlc_service);
// 🔥 关键！启用反射
//grpc::reflection::InitProtoReflectionServerBuilderPlugin();
@ -73,130 +80,6 @@ void runServer(const XmlNode &cfg){
}
}
int test_httpclient() {
try {
std::cout << "HTTP Client Example" << std::endl;
std::cout << "===================" << std::endl;
// 创建HTTP客户端
cmvr::HttpClient client;
// 设置基础URL - 使用Reqres.in替代JSONPlaceholder
client.setBaseUrl("https://reqres.in/api");
// 设置超时时间为15秒（增加超时时间以应对网络波动）
client.setTimeout(15000);
// 示例1: GET请求 - 获取用户列表
std::cout << "\n=== Example 1: GET Request (List Users) ===" << std::endl;
cmvr::HttpResponse response = client.get("/users?page=1");
std::cout << "Status: " << response.statusCode << " " << response.statusText << std::endl;
std::cout << "Content-Type: " << response.headers["Content-Type"] << std::endl;
std::cout << "Response Body: " << std::endl << response.body << std::endl;
// 示例2: POST JSON请求 - 创建用户
std::cout << "\n=== Example 2: POST JSON Request (Create User) ===" << std::endl;
// 使用JsonCpp创建JSON数据
Json::Value postData;
postData["name"] = "John Doe";
postData["job"] = "Software Engineer";
// 将JSON转换为字符串
Json::StreamWriterBuilder writer;
std::string jsonString = Json::writeString(writer, postData);
response = client.postJson("/users", {}, jsonString);
std::cout << "Status: " << response.statusCode << " " << response.statusText << std::endl;
std::cout << "Content-Type: " << response.headers["Content-Type"] << std::endl;
std::cout << "Response Body: " << std::endl << response.body << std::endl;
// 示例3: 解析JSON响应 - 获取单个用户
std::cout << "\n=== Example 3: Parsing JSON Response (Single User) ===" << std::endl;
response = client.get("/users/2");
if (response.statusCode == 200) {
// 使用JsonCpp解析JSON字符串
Json::Value user;
Json::CharReaderBuilder reader;
std::string errors;
std::istringstream stream(response.body);
bool parsingSuccessful = Json::parseFromStream(reader, stream, &user, &errors);
if (parsingSuccessful) {
Json::Value data = user["data"];
std::cout << "User ID: " << data["id"].asInt() << std::endl;
std::cout << "Email: " << data["email"].asString() << std::endl;
std::cout << "First Name: " << data["first_name"].asString() << std::endl;
std::cout << "Last Name: " << data["last_name"].asString() << std::endl;
} else {
std::cerr << "Failed to parse JSON: " << errors << std::endl;
}
}
// 示例4: PUT请求 - 更新用户
std::cout << "\n=== Example 4: PUT Request (Update User) ===" << std::endl;
Json::Value updateData;
updateData["name"] = "Jane Doe";
updateData["job"] = "Senior Software Engineer";
std::string updateJson = Json::writeString(writer, updateData);
response = client.postJson("/users/2", {}, updateJson); // Reqres.in使用POST处理PUT请求
std::cout << "Status: " << response.statusCode << " " << response.statusText << std::endl;
std::cout << "Content-Type: " << response.headers["Content-Type"] << std::endl;
std::cout << "Response Body: " << std::endl << response.body << std::endl;
// 示例5: 测试HttpBin.org - 获取请求头信息
std::cout << "\n=== Example 5: HttpBin.org (Request Headers) ===" << std::endl;
// 切换到HttpBin.org
client.setBaseUrl("https://httpbin.org");
// 添加自定义请求头
client.clearHeaders();
client.addHeader("X-Test-Header", "Hello from HttpClient");
client.addHeader("Accept", "application/json");
response = client.get("/headers");
std::cout << "Status: " << response.statusCode << " " << response.statusText << std::endl;
std::cout << "Content-Type: " << response.headers["Content-Type"] << std::endl;
std::cout << "Response Body: " << std::endl << response.body << std::endl;
// 示例6: 测试超时与错误处理
std::cout << "\n=== Example 6: Timeout & Error Handling ===" << std::endl;
// 设置较短的超时时间
client.setTimeout(2000); // 2秒超时
try {
// 请求一个会延迟响应的端点
response = client.get("/delay/5"); // 服务器会延迟5秒响应
std::cout << "Status: " << response.statusCode << " " << response.statusText << std::endl;
} catch (const cmvr::HttpException& e) {
std::cout << "Error (Expected Timeout): " << e.what() << std::endl;
}
// 恢复正常超时
client.setTimeout(15000);
} catch (const cmvr::HttpException& e) {
std::cerr << "HTTP Error: " << e.what() << std::endl;
return 1;
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
return 0;
}
int main(int argc, char* argv[]) {
std::string config_path;
if(argc == 1) {
@ -207,7 +90,7 @@ int main(int argc, char* argv[]) {
return 1;
}
std::string exe_dir = dirname(exe_path);
config_path = exe_dir + "/../config/cabin_robot.xml";
config_path = exe_dir + "/../config/dev_config_v2.xml";
std::cout << "Using default config path: " << config_path << std::endl;
}
else if (argc == 2){

25
src/service/grpc_service/CMakeLists.txt
View File

@ -6,7 +6,6 @@ add_library(service
grpc_head_service.cpp
grpc_dexhand_service.cpp
grpc_humanoid_robot_service.cpp
grpc_hlc_service.cpp
)
target_include_directories(service PUBLIC ${PROJECT_SOURCE_DIR}/include)
@ -61,27 +60,3 @@ target_link_libraries(grpc_humanoid_robot_client_test
${OpenCV_LIBS}
)
add_executable(grpc_hlc_client_test
grpc_hlc_client_test.cpp
)
target_link_libraries(grpc_hlc_client_test
PRIVATE
cmvr_es::device::canbus
cmvr_es::device::ti5motor
cmvr_es::device::humanoid_robot
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

46
src/service/grpc_service/grpc_hlc_client_test.cpp
View File

@ -1,46 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#include "gtest/gtest.h"
#include <glog/logging.h>
#include <grpcpp/grpcpp.h>
#include "../../../include/service/grpc_service/grpc_hlc_service.h"
#include "google/protobuf/timestamp.pb.h"
#include <iostream>
#include <google/protobuf/util/time_util.h>
using namespace cmvr::api;
TEST(GrpcHlcClientTest, MyTest) {
// 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
auto stub = cmvr::api::HlcService::NewStub(channel);
grpc::ClientContext context;
cmvr::api::Touch_Request request;
cmvr::api::Touch_Response response;
request.mutable_header()->set_device_id("hc01");
*request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
request.set_u(12);
request.set_v(13);
request.set_max_force(1300);
// 调用
grpc::Status status = stub->touch(&context, request, &response);
if (status.ok()) {
LOG(INFO) << "Touch RPC succeeded." << std::endl;
LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl;
LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl;
LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl;
} else {
LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl;
}
}

40
src/service/grpc_service/grpc_hlc_service.cpp
View File

@ -1,40 +0,0 @@
//
// Created by lgv on 2025/8/25.
//
#include "service/grpc_service/grpc_hlc_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "controller/touch_controller.h"
using namespace cmvr::service;
using namespace cmvr::device;
using namespace cmvr::api;
using google::protobuf::util::TimeUtil;
gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){}
grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
grpc::Status ret = grpc::Status::OK;
try {
std::shared_ptr<AbstractRobot> robot = nullptr;
auto cam = dmgr_.getDevice<AbstractCamera>("cam4");
auto hand = dmgr_.getDevice<AbstractDexHand>("hand1");
ctrl::TouchController touch_controller(robot,hand,cam);;
// touch_controller.touch(request->u(),request->v(),request->max_force());
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}

26
src/utils/solver/qp_solver.cpp
View File

@ -3,7 +3,7 @@
using namespace cmvr::ctrl;
template<int DOF>
CartesianController<DOF>::CartesianController(std::shared_ptr<RobotT> robot, double dsafe, double lambda)
QPSolver<DOF>::QPSolver(std::shared_ptr<RobotT> robot, double dsafe, double lambda)
: robot_(std::move(robot)), d_safe_(dsafe), lambda_(lambda), solver_()
{
dist_req_.enable_nearest_points = true;
@ -11,13 +11,13 @@ CartesianController<DOF>::CartesianController(std::shared_ptr<RobotT> robot, dou
// === configuration setters ==================================================
template<int DOF>
void CartesianController<DOF>::setCollisionPairs(const std::vector<CollisionPair>& p)
void QPSolver<DOF>::setCollisionPairs(const std::vector<CollisionPair>& p)
{
pairs_ = p;
}
template<int DOF>
void CartesianController<DOF>::setPositionLimits(const std::vector<std::optional<double>>& qmin,
void QPSolver<DOF>::setPositionLimits(const std::vector<std::optional<double>>& qmin,
const std::vector<std::optional<double>>& qmax)
{
if (qmin.size()==DOF) q_min_ = qmin;
@ -25,13 +25,13 @@ void CartesianController<DOF>::setPositionLimits(const std::vector<std::optional
}
template<int DOF>
void CartesianController<DOF>::setVelocityLimits(const std::vector<std::optional<double>>& qdmax)
void QPSolver<DOF>::setVelocityLimits(const std::vector<std::optional<double>>& qdmax)
{
if (qdmax.size()==DOF) qd_max_ = qdmax;
}
template<int DOF>
void CartesianController<DOF>::setAccelerationLimits(const std::vector<std::optional<double>>& qddmax)
void QPSolver<DOF>::setAccelerationLimits(const std::vector<std::optional<double>>& qddmax)
{
if (qddmax.size()==DOF) qdd_max_ = qddmax;
}
@ -39,7 +39,7 @@ void CartesianController<DOF>::setAccelerationLimits(const std::vector<std::opti
// === helpers ===============================================================
template<int DOF>
inline void CartesianController<DOF>::clampVec(
inline void QPSolver<DOF>::clampVec(
VecD& v, const std::vector<std::optional<double>>& lo,const std::vector<std::optional<double>>& hi) const
{
for (int i=0;i<DOF;++i) {
@ -51,7 +51,7 @@ inline void CartesianController<DOF>::clampVec(
// === main compute ===========================================================
template<int DOF>
bool CartesianController<DOF>::compute(
bool QPSolver<DOF>::compute(
std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& base_link,
const std::vector<PoseTarget>& targets,
@ -114,7 +114,7 @@ bool CartesianController<DOF>::compute(
// === solveIK ==============================================================
template<int DOF>
bool CartesianController<DOF>::solveIK(
bool QPSolver<DOF>::solveIK(
std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& base_link,
const std::vector<PoseTarget>& targets,
@ -198,7 +198,7 @@ bool CartesianController<DOF>::solveIK(
try {
dq = solver_.Solve();
} catch (const std::exception& e) {
std::cerr << "[CartesianController] QP failed: " << e.what() << std::endl;
std::cerr << "[QPSolver] QP failed: " << e.what() << std::endl;
return false;
}
@ -218,7 +218,7 @@ bool CartesianController<DOF>::solveIK(
// === distanceConstraint =====================================================
template<int DOF>
bool CartesianController<DOF>::distanceConstraint(
bool QPSolver<DOF>::distanceConstraint(
const CollisionPair& cp,
std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::vector<std::shared_ptr<fcl::CollisionObjectd>>& objs,
@ -258,6 +258,6 @@ bool CartesianController<DOF>::distanceConstraint(
return true; // 需要加入不等式
}
template class cmvr::ctrl::CartesianController<7>;
template class cmvr::ctrl::CartesianController<14>;
template class cmvr::ctrl::CartesianController<20>;
template class cmvr::ctrl::QPSolver<7>;
template class cmvr::ctrl::QPSolver<14>;
template class cmvr::ctrl::QPSolver<20>;

1670
third_party/AuboSdk/linux/include/AuboRobotMetaType.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

357
third_party/AuboSdk/linux/include/aubo/aubo_api.h vendored Normal file
View File

@ -0,0 +1,357 @@
/** @file aubo_api.h
* @brief \~chinese 机器人及外部轴等控制API接口，如获取机器人列表、获取系统信息等等
* @brief \~english API for controlling the robot and external axis
*/
#ifndef AUBO_SDK_AUBO_API_INTERFACE_H
#define AUBO_SDK_AUBO_API_INTERFACE_H
#include <aubo/system_info.h>
#include <aubo/runtime_machine.h>
#include <aubo/register_control.h>
#include <aubo/robot_interface.h>
#include <aubo/global_config.h>
#include <aubo/math.h>
#include <aubo/socket.h>
#include <aubo/serial.h>
#include <aubo/axis_interface.h>
namespace arcs {
namespace common_interface {
class ARCS_ABI_EXPORT AuboApi
{
public:
AuboApi();
virtual ~AuboApi();
/**
* \chinese
* 获取纯数学相关接口
*
* @return MathPtr对象的指针
*
* @par Python函数原型
* getMath(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.Math
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* MathPtr ptr = rpc_cli->getMath();
* @endcode
* \endchinese
*
*\english
* Get pure mathematic related API
*
* @return Shared pointer to a Math object
*
* @par Python function prototype
* getMath(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.Math
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* MathPtr ptr = rpc_cli->getMath();
* @endcode
*\endenglish
*/
MathPtr getMath();
/**
* \chinese
* 获取系统信息
*
* @return SystemInfoPtr对象的指针
*
* @par Python函数原型
* getSystemInfo(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.SystemInfo
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SystemInfoPtr ptr = rpc_cli->getSystemInfo();
* @endcode
* \endchinese
*
* \english
* Get system info
*
* @return Shared pointer to SystemInfo object
*
* @par Python function prototype
* getSystemInfo(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.SystemInfo
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SystemInfoPtr ptr = rpc_cli->getSystemInfo();
* @endcode
* \endenglish
*/
SystemInfoPtr getSystemInfo();
/**
* \chinese
* 获取运行时接口
*
* @return RuntimeMachinePtr对象的指针
*
* @par Python函数原型
* getRuntimeMachine(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.RuntimeMachine
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* RuntimeMachinePtr ptr = rpc_cli->getRuntimeMachine();
* @endcode
* \endchinese
*
* \english
* Get runtime api
*
* @return Shared pointer to RuntimeMachine object
* Python function prototype
* getRuntimeMachine(self: pyaubo_sdk.AuboApi) -> pyaubo_sdk.RuntimeMachine
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* RuntimeMachinePtr ptr = rpc_cli->getRuntimeMachine();
* @endcode
* \endenglish
*/
RuntimeMachinePtr getRuntimeMachine();
/**
* \chinese
* 对外寄存器接口
*
* @return RegisterControlPtr对象的指针
*
* @par Python函数原型
* getRegisterControl(self: pyaubo_sdk.AuboApi) ->
* pyaubo_sdk.RegisterControl
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* RegisterControlPtr ptr = rpc_cli->getRegisterControl();
* @endcode
* \endchinese
*
* \english
* External registers api
*
* @return Shared pointer to RegisterControl object
*
* @par Python function prototype
* getRegisterControl(self: pyaubo_sdk.AuboApi) ->
* pyaubo_sdk.RegisterControl
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* RegisterControlPtr ptr = rpc_cli->getRegisterControl();
* @endcode
* \endenglish
*/
RegisterControlPtr getRegisterControl();
/**
* \chinese
* 获取机器人列表
*
* @return 机器人列表
*
* @par Python函数原型
* getRobotNames(self: pyaubo_sdk.AuboApi) -> List[str]
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* auto robot_name = rpc_cli->getRobotNames().front();
* @endcode
*
* @par JSON-RPC请求示例
* {"jsonrpc":"2.0","method":"getRobotNames","params":[],"id":1}
*
* @par JSON-RPC响应示例
* {"id":1,"jsonrpc":"2.0","result":["rob1"]}
* \endchinese
*
* \english
* Get robot list
*
* @return robot list
*
* @par Python function prototype
* getRobotNames(self: pyaubo_sdk.AuboApi) -> List[str]
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* auto robot_name = rpc_cli->getRobotNames().front();
* @endcode
*
* @par JSON-RPC request example
* {"jsonrpc":"2.0","method":"getRobotNames","params":[],"id":1}
*
* @par JSON-RPC response example
* {"id":1,"jsonrpc":"2.0","result":["rob1"]}
* \endenglish
*/
std::vector<std::string> getRobotNames();
/**
* \chinese
* 根据名字获取 RobotInterfacePtr 接口
*
* @param name 机器人名字
* @return RobotInterfacePtr对象的指针
*
* @par Python函数原型
* getRobotInterface(self: pyaubo_sdk.AuboApi, arg0: str) ->
* pyaubo_sdk.RobotInterface
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* auto robot_name = rpc_cli->getRobotNames().front();
* RobotInterfacePtr ptr = rpc_cli->getRobotInterface(robot_name);
* @endcode
* \endchinese
*
* \english
* Get RobotInterfacePtr based on name
*
* @param name Robot name
* @return Shared pointer to a RobotInterface object
*
* @par Python function prototype
* getRobotInterface(self: pyaubo_sdk.AuboApi, arg0: str) ->
* pyaubo_sdk.RobotInterface
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* auto robot_name = rpc_cli->getRobotNames().front();
* RobotInterfacePtr ptr = rpc_cli->getRobotInterface(robot_name);
* @endcode
* \endenglish
*/
RobotInterfacePtr getRobotInterface(const std::string &name);
/**
* \~chinese 获取外部轴列表 \~english Get external axis list
*
* @return
*/
std::vector<std::string> getAxisNames();
/**
* \chinese
* 获取外部轴接口
*
* @param name
* @return
* \endchinese
*
* \english
* Get external axis interface
*
* @param name
* @return
* \endenglish
*/
AxisInterfacePtr getAxisInterface(const std::string &name);
/// \~chinese 获取独立 IO 模块接口 \~english Get independent IO module interface
/**
* \chinese
* 获取 socket
* @return SocketPtr对象的指针
*
* @par Python函数原型
* getSocket(self: pyaubo_sdk.AuboApi) -> arcs::common_interface::Socket
* @endcode
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SocketPtr ptr = rpc_cli->getSocket();
* @endcode
* \endchinese
*
* \english
* Get socket
* @return Shared pointer to a socket object
*
* @par Python function prototype
* getSocket(self: pyaubo_sdk.AuboApi) -> arcs::common_interface::Socket
* @endcode
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SocketPtr ptr = rpc_cli->getSocket();
* @endcode
* \endenglish
*/
SocketPtr getSocket();
/**
* \chinese
* @return SerialPtr对象的指针
*
* @par Python函数原型
* getSerial(self: pyaubo_sdk.AuboApi) -> arcs::common_interface::Serial
*
* @par C++示例
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SerialPtr ptr = rpc_cli->getSerial();
* @endcode
* \endchinese
*
* \english
* @return Shared pointer to Serial object
*
* @par Python function prototype
* getSerial(self: pyaubo_sdk.AuboApi) -> arcs::common_interface::Serial
*
* @par C++ example
* @code
* auto rpc_cli = std::make_shared<RpcClient>();
* SerialPtr ptr = rpc_cli->getSerial();
* @endcode
* \endenglish
*/
SerialPtr getSerial();
/**
* \~chinese 获取同步运动接口 \~english Get syncronous move interface
*
* \~chinese @return SyncMovePtr对象的指针
* \~english @return Shared pointer to SyncMove object
*/
SyncMovePtr getSyncMove(const std::string &name);
/**
* \~chinese 获取告警信息接口
* \~english Get alert interface
*
* \~chinese @return TracePtr对象的指针
* \~english @return Shared pointer of trace object
*/
TracePtr getTrace(const std::string &name);
protected:
void *d_{ nullptr };
};
using AuboApiPtr = std::shared_ptr<AuboApi>;
} // namespace common_interface
} // namespace arcs
#endif // AUBO_SDK_AUBO_API_H

381
third_party/AuboSdk/linux/include/aubo/axis_interface.h vendored Normal file
View File

@ -0,0 +1,381 @@
/** @file axes.h
* @brief 外部轴接口
*/
#ifndef AUBO_SDK_AXIS_INTERFACE_H
#define AUBO_SDK_AXIS_INTERFACE_H
#include <aubo/sync_move.h>
#include <aubo/trace.h>
namespace arcs {
namespace common_interface {
/**
* \~chinese 外部轴API接口
* \~english External axis API interface
*/
class ARCS_ABI_EXPORT AxisInterface
{
public:
AxisInterface();
virtual ~AxisInterface();
/**
* \~chinese 通电
* \~english Power on
* @return
*/
int poweronExtAxis();
/**
* \~chinese 断电
* \~english Power off
* @return
*/
int poweroffExtAxis();
/**
* \~chinese 使能
* \~english Enable
* @return
*/
int enableExtAxis();
/**
* \~chinese 设置外部轴的安装位姿(相对于世界坐标系)
* \~chinese @param pose
* \~english Set mounting pose of external axis (wrt world frame)
* \~english @param pose
* @return
*/
int setExtAxisMountingPose(const std::vector<double> &pose);
/**
* \chinese 运动到指定点， 旋转或者平移
*
* @param pos
* @param v
* @param a
* @param duration
* @return
* \endchinese
*
* \english move to pos, rotation or linear
*
* @param pos
* @param v
* @param a
* @param duration
* @return
* \endenglish
*/
int moveExtJoint(double pos, double v, double a, double duration);
/**
* \chinese
* 制定目标运动速度
*
* @param v
* @param a
* @param duration
* @return
* \endchinese
*
* \english
* Set target speed, acceleration and duration
* @param v
* @param a
* @param duration
* @return
* \endenglish
*/
int speedExtJoint(double v, double a, double duration);
int stopExtJoint(double a);
/**
* \~chinese 获取外部轴的类型 0代表是旋转 1代表平移
* \~english Get external axis type: 0 for rotation, 1 for linear
* @return
*/
int getExtAxisType();
/**
* \chinese
* 获取当前外部轴的状态
*
* @return 当前外部轴的状态
* \endchinese
*
* \english
* Get external axis status
*
* @return Current exteral axis status
* \endenglish
*/
AxisModeType getAxisModeType();
/**
* \chinese
* 获取外部轴安装位姿
*
* @return 外部轴安装位姿
* \endchinese
*
* \english
* Get external axis mounting pose
*
* @return External axis pose
* \endenglish
*/
std::vector<double> getExtAxisMountingPose();
/**
* \chinese
* 获取相对于安装坐标系的位姿，外部轴可能为变位机或者导轨
*
* @return 相对于安装坐标系的位姿
* \endchinese
*
* \english
* Get pose wrt mounting coordinate system, axis can be positioner or linear rail
*
* @return Pose wrt mounting coordinate system
* \endenglish
*/
std::vector<double> getExtAxisPose();
/**
* \chinese
* 获取外部轴位置
*
* @return 外部轴位置
* \endchinese
*
* \english
* Get external axis position
*
* @return External axis position
* \endenglish
*/
double getExtAxisPosition();
/**
* \chinese
* 获取外部轴运行速度
*
* @return 外部轴运行速度
* \endchinese
*
* \english
* Get external axis speed
*
* @return External axis speed
* \endenglish
*/
double getExtAxisVelocity();
/**
* \chinese
* 获取外部轴运行加速度
*
* @return 外部轴运行加速度
* \endchinese
*
* \english
* Get external axis acceleration
*
* @return External axis acceleration
* \endenglish
*/
double getExtAxisAcceleration();
/**
* \chinese
* 获取外部轴电流
*
* @return 外部轴电流
* \endchinese
*
* \english
* Get external axis current
*
* @return External axis current
* \endenglish
*/
double getExtAxisCurrent();
/**
* \chinese
* 获取外部轴温度
*
* @return 外部轴温度
* \endchinese
*
* \english
* Get external axis temperature
*
* @return External axis temperature
* \endenglish
*/
double getExtAxisTemperature();
/**
* \chinese
* 获取外部轴电压
*
* @return 外部轴电压
* \endchinese
*
* \english
* Get external axis voltage
*
* @return External axis voltage
* \endenglish
*/
double getExtAxisBusVoltage();
/**
* \chinese
* 获取外部轴电流
*
* @return 外部轴电流
* \endchinese
*
* \english
* Get external axis current
*
* @return external axis current
* \endenglish
*/
double getExtAxisBusCurrent();
/**
* \chinese
* 获取外部轴最大位置
*
* @return 外部轴最大位置
* \endchinese
*
* \english
* Get external axis max position
*
* @return External axis max position
* \endenglish
*/
double getExtAxisMaxPosition();
/**
* \chinese
* 获取外部轴最小位置
*
* @return 外部轴最小位置
* \endchinese
*
* \english
* Get external axis min position
*
* @return External axis min position
* \endenglish
*/
double getExtMinPosition();
/**
* \chinese
* 获取外部轴最大速度
*
* @return 外部轴最大速度
* \endchinese
*
* \english
* Get external axis max speed
*
* @return External axis max speed
* \endenglish
*/
double getExtAxisMaxVelocity();
/**
* \chinese
* 获取外部轴最大加速度
*
* @return 外部轴最大加速度
* \endchinese
*
* \english
* Get external axis max acceleration
*
* @return External axis max acceleration
* \endenglish
*/
double getExtAxisMaxAcceleration();
/**
* \chinese
* 跟踪另一个外部轴的运动(禁止运动过程中使用)
*
* @param target_name 目标的外部轴名字
* @param phase 相位差
* @param err 跟踪运行的最大误差
* @return
* \endchinese
*
* \english
* Follow motion of another external axis （not to be used during motion）
*
* @param target_name name of target axis
* @param phase phase difference
* @param err max error when following motion
* @return
* \endenglish
*/
int followAnotherAxis(const std::string &target_name, double phase,
double err);
/**
* \~chinese @brief stopFollowAnotherAxis(禁止运动过程中使用)
* \~english @brief stopFollowAnotherAxis(not to be used during motion)
* @return
*/
int stopFollowAnotherAxis();
/**
* \chinese
* 获取外部轴错误码
*
* @return 外部轴错误码
* \endchinese
*
* \english
* Get external axis error code
*
* @return External axis error code
* \endenglish
*/
int getErrorCode();
/**
* \chinese
* 重置外部轴错误
*
* @return
* \endchinese
*
* \english
* Reset axis error
*
* @return
* \endenglish
*/
int clearAxisError();
protected:
void *d_;
};
using AxisInterfacePtr = std::shared_ptr<AxisInterface>;
} // namespace common_interface
} // namespace arcs
#endif // AUBO_SDK_AXIS_INTERFACE_H

114
third_party/AuboSdk/linux/include/aubo/error_stack/error_stack.h vendored Normal file
View File

@ -0,0 +1,114 @@
/** @file error_stack.h
* @brief 汇总错误码
*/
#ifndef AUBO_SDK_ERROR_STACK_H
#define AUBO_SDK_ERROR_STACK_H
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <aubo/global_config.h>
// 格式化占位符，默认是 fmt 的格式
#ifndef _PH1_
#define _PH1_ "{}"
#define _PH2_ "{}"
#define _PH3_ "{}"
#define _PH4_ "{}"
#endif
namespace arcs {
namespace error_stack {
constexpr int ARCS_ABI_EXPORT codeCompose(int aa, int bb, int cccc)
{
return (int)((aa * 1000000) + (bb * 10000) + cccc);
}
constexpr int ARCS_ABI_EXPORT mod(int x)
{
return (x % 1000000);
}
#include <aubo/error_stack/hal_error.h>
#include <aubo/error_stack/rtm_error.h>
#include <aubo/error_stack/system_error.h>
#define ARCS_ERROR_CODES \
SYSTEM_ERRORS \
JOINT_ERRORS \
SAFETY_INTERFACE_BOARD_ERRORS \
RTM_ERRORS \
TOOL_ERRORS \
PEDSTRAL_ERRORS \
HARDWARE_INTERFACE_ERRORS \
_D(ARCS_MAX_ERROR_CODE, -1, "Max error code", "suggest...")
// 错误代码枚举
enum ErrorCodes
{
#define _D(n, v, s, r) n = (int)v,
ARCS_ERROR_CODES
#undef _D
};
inline int str2ErrorCode(const char *err_code_name)
{
#define _D(n, v, s, r) \
if (strcmp(#n, err_code_name) == 0) \
return v;
ARCS_ERROR_CODES
#undef _D
return ARCS_MAX_ERROR_CODE;
}
inline const char *errorCode2Str(int err_code)
{
static const char *errcode_str[] = {
#define _D(n, v, s, r) s,
ARCS_ERROR_CODES
#undef _D
};
enum arcs_index
{
#define _D(n, v, s, r) n##_INDEX,
ARCS_ERROR_CODES
#undef _D
};
int index = -1;
#define _D(n, v, s, r) \
if (err_code == v) \
index = n##_INDEX;
ARCS_ERROR_CODES
#undef _D
if (index == -1) {
index = ARCS_MAX_ERROR_CODE_INDEX;
}
return errcode_str[(unsigned)index];
}
inline std::ostream &dump(std::ostream &os)
{
#define _D(n, v, s, r) \
os << std::setw(20) << #n << "\t" << v << "\t" << s << "\t" << r \
<< std::endl;
ARCS_ERROR_CODES
#undef _D
return os;
}
} // namespace error_stack
} // namespace arcs
#endif // AUBO_SDK_ERROR_STACK_H

Some files were not shown because too many files have changed in this diff Show More