chengming
/
unitree_ros_to_real
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

before merge

This commit is contained in:
Bian Zekun 2021-09-02 18:24:10 +08:00
parent ef3bbed457
commit f8bbd5b1de
14 changed files with 212 additions and 398 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
unitree_legged_msgs/CMakeLists.txt
View File

@ -12,6 +12,8 @@ add_message_files(
FILES FILES
MotorCmd.msg MotorCmd.msg
MotorState.msg MotorState.msg
BmsCmd.msg
BmsState.msg
Cartesian.msg Cartesian.msg
IMU.msg IMU.msg
LED.msg LED.msg

2
unitree_legged_msgs/msg/BmsCmd.msg Normal file
View File

@ -0,0 +1,2 @@
uint8 off # off 0xA5
uint8[3] reserve

9
unitree_legged_msgs/msg/BmsState.msg Normal file
View File

@ -0,0 +1,9 @@
uint8 version_h
uint8 version_l
uint8 bms_status
uint8 SOC # SOC 0-100%
int32 current # mA
uint16 cycle
int8[2] BQ_NTC # x1 degrees centigrade
int8[2] MCU_NTC # x1 degrees centigrade
uint16[10] cell_vol # cell voltage mV

34
unitree_legged_msgs/msg/HighCmd.msg
View File

@ -3,17 +3,31 @@ uint16 commVersion # Old version Aliengo does not have
uint16 robotID # Old version Aliengo does not have uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have
uint8 mode uint8 mode # 0. idle, default stand
float32 forwardSpeed # 1. force stand (controlled by dBodyHeight + ypr)
float32 sideSpeed # 2. target velocity walking (controlled by velocity + yawSpeed)
float32 rotateSpeed # 3. target position walking (controlled by position + ypr[0])
float32 bodyHeight # 4. path mode walking (reserve for future release)
float32 footRaiseHeight # 5. position stand down.
float32 yaw # 6. position stand up
float32 pitch # 7. damping mode
float32 roll # 8. recovery stand
# 9. backflip
# 10. jumpYaw
# 11. straightHand
# 12. dance1
# 13. dance2
# 14. two leg stand
uint8 gaitType # 0.idle 1.trot 2.trot running 3.climb stair
uint8 speedLevel # 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
float32 footRaiseHeight # (unit: m, default: 0.08m), foot up height while walking
float32 bodyHeight # (unit: m, default: 0.28m),
float32[2] postion # (unit: m), desired position in inertial frame
float32[3] euler # (unit: rad), roll pitch yaw in stand mode
float32[2] velocity # (unit: m/s), forwardSpeed, sideSpeed in body frame
float32 yawSpeed # (unit: rad/s), rotateSpeed in body frame
BmsCmd bms
LED[4] led LED[4] led
uint8[40] wirelessRemote uint8[40] wirelessRemote
uint8[40] AppRemote # Old version Aliengo does not have
uint32 reserve # Old version Aliengo does not have uint32 reserve # Old version Aliengo does not have
int32 crc int32 crc

25
unitree_legged_msgs/msg/HighState.msg
View File

@ -4,23 +4,20 @@ uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have
uint8 mode uint8 mode
float32 progress # new on Go1, reserve
IMU imu IMU imu
float32 forwardSpeed uint8 gaitType # new on Go1, 0.idle 1.trot 2.trot running 3.climb stair
float32 sideSpeed float32 footRaiseHeight # (unit: m, default: 0.08m), foot up height while walking
float32 rotateSpeed float32[3] position # (unit: m), from own odometry in inertial frame, usually drift
float32 bodyHeight float32 bodyHeight # (unit: m, default: 0.28m)
float32 updownSpeed float32[3] velocity # (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
float32 forwardPosition # (will be float type next version) # Old version Aliengo is different float32 yawSpeed # (unit: rad/s), rotateSpeed in body frame
float32 sidePosition # (will be float type next version) # Old version Aliengo is different Cartesian[4] footPosition2Body # foot position relative to body
Cartesian[4] footPosition2Body Cartesian[4] footSpeed2Body # foot speed relative to body
Cartesian[4] footSpeed2Body int8[20] temperature
BmsState bms
int16[4] footForce # Old version Aliengo is different int16[4] footForce # Old version Aliengo is different
int16[4] footForceEst # Old version Aliengo does not have int16[4] footForceEst # Old version Aliengo does not have
uint32 tick
uint8[40] wirelessRemote uint8[40] wirelessRemote
uint32 reserve # Old version Aliengo does not have uint32 reserve # Old version Aliengo does not have
uint32 crc uint32 crc
# Under are not defined in SDK yet. # Old version Aliengo does not have
Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.
float32[12] jointP # for visualization

3
unitree_legged_msgs/msg/LowCmd.msg
View File

@ -4,7 +4,8 @@ uint16 robotID # Old version Aliengo does not have
uint32 SN # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have
MotorCmd[20] motorCmd MotorCmd[20] motorCmd
LED[4] led BmsCmd bms # new on Go1, battery command
# LED[4] led
uint8[40] wirelessRemote uint8[40] wirelessRemote
uint32 reserve # Old version Aliengo does not have uint32 reserve # Old version Aliengo does not have
uint32 crc uint32 crc

1
unitree_legged_msgs/msg/LowState.msg
View File

@ -5,6 +5,7 @@ uint32 SN # Old version Aliengo does not have
uint8 bandWidth # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have
IMU imu IMU imu
MotorState[20] motorState MotorState[20] motorState
BmsState bms # new on Go1, battery state
int16[4] footForce # force sensors # Old version Aliengo is different int16[4] footForce # force sensors # Old version Aliengo is different
int16[4] footForceEst # force sensors # Old version Aliengo does not have int16[4] footForceEst # force sensors # Old version Aliengo does not have
uint32 tick # reference real-time from motion controller (unit: us) uint32 tick # reference real-time from motion controller (unit: us)

28
unitree_legged_real/CMakeLists.txt
View File

@ -19,30 +19,14 @@ include_directories(
set(CMAKE_CXX_FLAGS "-O3") set(CMAKE_CXX_FLAGS "-O3")
if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1") include_directories(/home/bian/Robot_SDK/unitree_legged_sdk/include)
include_directories($ENV{ALIENGO_SDK_PATH}/include) link_directories(/home/bian/Robot_SDK/unitree_legged_sdk/lib)
link_directories($ENV{ALIENGO_SDK_PATH}/lib) string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk.so)
string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so)
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
add_definitions(-DSDK3_1) add_executable(lcm_server /home/bian/Robot_SDK/unitree_legged_sdk/examples/lcm_server.cpp)
target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp) add_dependencies(lcm_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
elseif( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_2")
include_directories($ENV{UNITREE_LEGGED_SDK_PATH}/include)
link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib)
string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so)
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
add_definitions(-DSDK3_2)
add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
add_executable(position_lcm src/exe/position_mode.cpp) add_executable(position_lcm src/exe/position_mode.cpp)
target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})

268
unitree_legged_real/include/convert.h
View File

@ -12,160 +12,46 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/HighState.h> #include <unitree_legged_msgs/HighState.h>
#include <unitree_legged_msgs/MotorCmd.h> #include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/MotorState.h> #include <unitree_legged_msgs/MotorState.h>
#include <unitree_legged_msgs/BmsCmd.h>
#include <unitree_legged_msgs/BmsState.h>
#include <unitree_legged_msgs/IMU.h> #include <unitree_legged_msgs/IMU.h>
#ifdef SDK3_1
#include "aliengo_sdk/aliengo_sdk.hpp"
unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){
unitree_legged_msgs::IMU ros;
ros.quaternion[0] = lcm.quaternion[0];
ros.quaternion[1] = lcm.quaternion[1];
ros.quaternion[2] = lcm.quaternion[2];
ros.quaternion[3] = lcm.quaternion[3];
ros.gyroscope[0] = lcm.gyroscope[0];
ros.gyroscope[1] = lcm.gyroscope[1];
ros.gyroscope[2] = lcm.gyroscope[2];
ros.accelerometer[0] = lcm.acceleration[0];
ros.accelerometer[1] = lcm.acceleration[1];
ros.accelerometer[2] = lcm.acceleration[2];
// ros.rpy[0] = lcm.rpy[0];
// ros.rpy[1] = lcm.rpy[1];
// ros.rpy[2] = lcm.rpy[2];
ros.temperature = lcm.temp;
return ros;
}
unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){
unitree_legged_msgs::MotorState ros;
ros.mode = lcm.mode;
ros.q = lcm.position;
ros.dq = lcm.velocity;
ros.ddq = 0;
ros.tauEst = lcm.torque;
ros.q_raw = 0;
ros.dq_raw = 0;
ros.ddq_raw = 0;
ros.temperature = lcm.temperature;
return ros;
}
aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){
aliengo::MotorCmd lcm;
lcm.mode = ros.mode;
lcm.position = ros.q;
lcm.velocity = ros.dq;
lcm.positionStiffness = ros.Kp;
lcm.velocityStiffness = ros.Kd;
lcm.torque = ros.tau;
return lcm;
}
unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){
unitree_legged_msgs::LowState ros;
ros.levelFlag = lcm.levelFlag;
ros.commVersion = 0;
ros.robotID = 0;
ros.SN = 0;
ros.bandWidth = 0;
ros.imu = ToRos(lcm.imu);
for(int i = 0; i < 20; i++){
ros.motorState[i] = ToRos(lcm.motorState[i]);
}
for(int i = 0; i < 4; i++){
ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = 0;
}
ros.tick = lcm.tick;
for(int i = 0; i < 40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.crc = lcm.crc;
return ros;
}
aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){
aliengo::LowCmd lcm;
lcm.levelFlag = ros.levelFlag;
for(int i = 0; i < 20; i++){
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
}
for(int i = 0; i < 4; i++){
lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b;
}
for(int i = 0; i < 40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
}
lcm.crc = ros.crc;
return lcm;
}
unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){
unitree_legged_msgs::HighState ros;
ros.levelFlag = lcm.levelFlag;
ros.commVersion = 0;
ros.robotID = 0;
ros.SN = 0;
ros.bandWidth = 0;
ros.mode = lcm.mode;
ros.imu = ToRos(lcm.imu);
ros.forwardSpeed = lcm.forwardSpeed;
ros.sideSpeed = lcm.sideSpeed;
ros.rotateSpeed = lcm.rotateSpeed;
ros.bodyHeight = lcm.bodyHeight;
ros.updownSpeed = lcm.updownSpeed;
ros.forwardPosition = lcm.forwardPosition.x;
ros.sidePosition = lcm.sidePosition.y;
for(int i = 0; i < 4; i++){
ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = 0;
}
ros.tick = lcm.tick;
for(int i = 0; i<40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.reserve = 0;
ros.crc = lcm.crc;
return ros;
}
aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){
aliengo::HighCmd lcm;
lcm.levelFlag = ros.levelFlag;
lcm.mode = ros.mode;
lcm.forwardSpeed = ros.forwardSpeed;
lcm.sideSpeed = ros.sideSpeed;
lcm.rotateSpeed = ros.rotateSpeed;
lcm.bodyHeight = ros.bodyHeight;
lcm.footRaiseHeight = ros.footRaiseHeight;
lcm.yaw = ros.yaw;
lcm.pitch = ros.pitch;
lcm.roll = ros.roll;
for(int i = 0; i < 4; i++){
lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b;
}
for(int i = 0; i < 40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
}
lcm.crc = ros.crc;
return lcm;
}
#endif
#ifdef SDK3_2
#include "unitree_legged_sdk/unitree_legged_sdk.h" #include "unitree_legged_sdk/unitree_legged_sdk.h"
unitree_legged_msgs::Cartesian ToRos(UNITREE_LEGGED_SDK::Cartesian& lcm){
unitree_legged_msgs::Cartesian ros;
ros.x = lcm.x;
ros.y = lcm.y;
ros.z = lcm.z;
return ros;
}
UNITREE_LEGGED_SDK::BmsCmd ToLcm(unitree_legged_msgs::BmsCmd& ros){
UNITREE_LEGGED_SDK::BmsCmd lcm;
lcm.off = ros.off;
for(int i(0); i<3; ++i){
lcm.reserve[i] = ros.reserve[i];
}
return lcm;
}
unitree_legged_msgs::BmsState ToRos(UNITREE_LEGGED_SDK::BmsState& lcm){
unitree_legged_msgs::BmsState ros;
ros.version_h = lcm.version_h;
ros.version_l = lcm.version_l;
ros.bms_status = lcm.bms_status;
ros.SOC = lcm.SOC;
ros.current = lcm.current;
ros.cycle = lcm.cycle;
for(int i(0); i<2; ++i){
ros.BQ_NTC[i] = lcm.BQ_NTC[i];
ros.MCU_NTC[i] = lcm.MCU_NTC[i];
}
for(int i(0); i<10; ++i){
ros.cell_vol[i] = lcm.cell_vol[i];
}
return ros;
}
unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm) unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
{ {
unitree_legged_msgs::IMU ros; unitree_legged_msgs::IMU ros;
@ -179,9 +65,6 @@ unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
ros.accelerometer[0] = lcm.accelerometer[0]; ros.accelerometer[0] = lcm.accelerometer[0];
ros.accelerometer[1] = lcm.accelerometer[1]; ros.accelerometer[1] = lcm.accelerometer[1];
ros.accelerometer[2] = lcm.accelerometer[2]; ros.accelerometer[2] = lcm.accelerometer[2];
// ros.rpy[0] = lcm.rpy[0];
// ros.rpy[1] = lcm.rpy[1];
// ros.rpy[2] = lcm.rpy[2];
ros.temperature = lcm.temperature; ros.temperature = lcm.temperature;
return ros; return ros;
} }
@ -230,6 +113,7 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
for(int i = 0; i<20; i++){ for(int i = 0; i<20; i++){
ros.motorState[i] = ToRos(lcm.motorState[i]); ros.motorState[i] = ToRos(lcm.motorState[i]);
} }
ros.bms = ToRos(lcm.bms);
for(int i = 0; i<4; i++){ for(int i = 0; i<4; i++){
ros.footForce[i] = lcm.footForce[i]; ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = lcm.footForceEst[i]; ros.footForceEst[i] = lcm.footForceEst[i];
@ -254,11 +138,7 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGE
for(int i = 0; i<20; i++){ for(int i = 0; i<20; i++){
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
} }
for(int i = 0; i<4; i++){ lcm.bms = ToLcm(ros.bms);
lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b;
}
for(int i = 0; i<40; i++){ for(int i = 0; i<40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i]; lcm.wirelessRemote[i] = ros.wirelessRemote[i];
} }
@ -276,30 +156,36 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
ros.SN = lcm.SN; ros.SN = lcm.SN;
ros.bandWidth = lcm.bandWidth; ros.bandWidth = lcm.bandWidth;
ros.mode = lcm.mode; ros.mode = lcm.mode;
ros.progress = lcm.progress;
ros.imu = ToRos(lcm.imu); ros.imu = ToRos(lcm.imu);
ros.forwardSpeed = lcm.forwardSpeed; ros.gaitType = lcm.gaitType;
ros.sideSpeed = lcm.sideSpeed; ros.footRaiseHeight = lcm.footRaiseHeight;
ros.rotateSpeed = lcm.rotateSpeed;
ros.bodyHeight = lcm.bodyHeight; ros.bodyHeight = lcm.bodyHeight;
ros.updownSpeed = lcm.updownSpeed; ros.yawSpeed = lcm.yawSpeed;
ros.forwardPosition = lcm.forwardPosition; ros.bms = ToRos(lcm.bms);
ros.sidePosition = lcm.sidePosition; ros.reserve = lcm.reserve;
for(int i = 0; i<4; i++){ ros.crc = lcm.crc;
ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y; for(int i(0); i<3; ++i){
ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z; ros.position[i] = lcm.position[i];
ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x; ros.velocity[i] = lcm.velocity[i];
ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y; }
ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
for(int i(0); i<4; ++i){
ros.footPosition2Body[i] = ToRos(lcm.footPosition2Body[i]);
ros.footSpeed2Body[i] = ToRos(lcm.footSpeed2Body[i]);
ros.footForce[i] = lcm.footForce[i]; ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = lcm.footForceEst[i]; ros.footForceEst[i] = lcm.footForceEst[i];
} }
ros.tick = lcm.tick;
for(int i = 0; i<40; i++){ for(int i(0); i<20; ++i){
ros.temperature[i] = lcm.temperature[i];
}
for(int i(0); i<40; ++i){
ros.wirelessRemote[i] = lcm.wirelessRemote[i]; ros.wirelessRemote[i] = lcm.wirelessRemote[i];
} }
ros.reserve = lcm.reserve;
ros.crc = lcm.crc;
return ros; return ros;
} }
@ -312,27 +198,35 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG
lcm.SN = ros.SN; lcm.SN = ros.SN;
lcm.bandWidth = ros.bandWidth; lcm.bandWidth = ros.bandWidth;
lcm.mode = ros.mode; lcm.mode = ros.mode;
lcm.forwardSpeed = ros.forwardSpeed; lcm.gaitType = ros.gaitType;
lcm.sideSpeed = ros.sideSpeed; lcm.speedLevel = ros.speedLevel;
lcm.rotateSpeed = ros.rotateSpeed;
lcm.bodyHeight = ros.bodyHeight;
lcm.footRaiseHeight = ros.footRaiseHeight; lcm.footRaiseHeight = ros.footRaiseHeight;
lcm.yaw = ros.yaw; lcm.bodyHeight = ros.bodyHeight;
lcm.pitch = ros.pitch; lcm.yawSpeed = ros.yawSpeed;
lcm.roll = ros.roll; lcm.bms = ToLcm(ros.bms);
lcm.reserve = ros.reserve;
lcm.crc = ros.crc;
for(int i(0); i<2; ++i){
lcm.postion[i] = ros.postion[i];
lcm.velocity[i] = ros.velocity[i];
}
for(int i(0); i<3; ++i){
lcm.euler[i] = ros.euler[i];
}
for(int i = 0; i<4; i++){ for(int i = 0; i<4; i++){
lcm.led[i].r = ros.led[i].r; lcm.led[i].r = ros.led[i].r;
lcm.led[i].g = ros.led[i].g; lcm.led[i].g = ros.led[i].g;
lcm.led[i].b = ros.led[i].b; lcm.led[i].b = ros.led[i].b;
} }
for(int i = 0; i<40; i++){ for(int i = 0; i<40; i++){
lcm.wirelessRemote[i] = ros.wirelessRemote[i]; lcm.wirelessRemote[i] = ros.wirelessRemote[i];
lcm.AppRemote[i] = ros.AppRemote[i];
} }
lcm.reserve = ros.reserve;
lcm.crc = ros.crc;
return lcm; return lcm;
} }
#endif
#endif // _CONVERT_H_ #endif // _CONVERT_H_

8
unitree_legged_real/launch/real.launch
View File

@ -1,12 +1,8 @@
<launch> <launch>
<arg name="rname" default="a1"/>
<arg name="ctrl_level" default="highlevel"/> <arg name="ctrl_level" default="highlevel"/>
<arg name="firmwork" default="3_2"/>
<node pkg="unitree_legged_real" type="lcm_server_$(arg firmwork)" name="node_lcm_server" <node pkg="unitree_legged_real" type="lcm_server" name="node_lcm_server"
respawn="false" output="screen" args="$(arg rname) $(arg ctrl_level)" /> respawn="false" output="screen" args="$(arg ctrl_level)" />
<param name="robot_name" value="$(arg rname)"/>
<param name="control_level" value="$(arg ctrl_level)"/> <param name="control_level" value="$(arg ctrl_level)"/>
<param name="firmwork" value="$(arg firmwork)"/>
</launch> </launch>

27
unitree_legged_real/src/exe/position_mode.cpp
View File

@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/LowState.h> #include <unitree_legged_msgs/LowState.h>
#include "convert.h" #include "convert.h"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
#endif
template<typename TLCM> template<typename TLCM>
void* update_loop(void* param) void* update_loop(void* param)
@ -116,7 +111,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
} }
double sin_joint1, sin_joint2; double sin_joint1, sin_joint2;
// last, do sine wave // last, do sine wave
if( motiontime >= 1700){ if( motiontime >= 400){
sin_count++; sin_count++;
sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0); sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0); sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0);
@ -164,27 +159,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
int main(int argc, char *argv[]){ int main(int argc, char *argv[]){
ros::init(argc, argv, "position_ros_mode"); ros::init(argc, argv, "position_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
#ifdef SDK3_1
aliengo::Control control(aliengo::LOWLEVEL);
aliengo::LCM roslcm;
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
#endif
#ifdef SDK3_2
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
// UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
// UNITREE_LEGGED_SDK::InitEnvironment();
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm); mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
} }

24
unitree_legged_real/src/exe/torque_mode.cpp
View File

@ -11,12 +11,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/LowState.h> #include <unitree_legged_msgs/LowState.h>
#include "convert.h" #include "convert.h"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
#endif
template<typename TLCM> template<typename TLCM>
void* update_loop(void* param) void* update_loop(void* param)
@ -89,26 +84,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
int main(int argc, char *argv[]){ int main(int argc, char *argv[]){
ros::init(argc, argv, "torque_ros_mode"); ros::init(argc, argv, "torque_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
#ifdef SDK3_1
aliengo::Control control(aliengo::LOWLEVEL);
aliengo::LCM roslcm;
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
#endif
#ifdef SDK3_2
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
// UNITREE_LEGGED_SDK::InitEnvironment();
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm); mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
} }

25
unitree_legged_real/src/exe/velocity_mode.cpp
View File

@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/LowState.h> #include <unitree_legged_msgs/LowState.h>
#include "convert.h" #include "convert.h"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
#endif
template<typename TLCM> template<typename TLCM>
void* update_loop(void* param) void* update_loop(void* param)
@ -92,27 +87,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
int main(int argc, char *argv[]){ int main(int argc, char *argv[]){
ros::init(argc, argv, "velocity_ros_mode"); ros::init(argc, argv, "velocity_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
#ifdef SDK3_1
aliengo::Control control(aliengo::LOWLEVEL);
aliengo::LCM roslcm;
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
#endif
#ifdef SDK3_2
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
// UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
// UNITREE_LEGGED_SDK::InitEnvironment();
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm); mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
} }

136
unitree_legged_real/src/exe/walk_mode.cpp
View File

@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#include <unitree_legged_msgs/HighState.h> #include <unitree_legged_msgs/HighState.h>
#include "convert.h" #include "convert.h"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
#endif
template<typename TLCM> template<typename TLCM>
void* update_loop(void* param) void* update_loop(void* param)
@ -56,73 +51,85 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
motiontime = motiontime+2; motiontime = motiontime+2;
roslcm.Get(RecvHighLCM); roslcm.Get(RecvHighLCM);
RecvHighROS = ToRos(RecvHighLCM); RecvHighROS = ToRos(RecvHighLCM);
// printf("%f\n", RecvHighROS.forwardSpeed);
SendHighROS.forwardSpeed = 0.0f;
SendHighROS.sideSpeed = 0.0f;
SendHighROS.rotateSpeed = 0.0f;
SendHighROS.bodyHeight = 0.0f;
SendHighROS.mode = 0; SendHighROS.mode = 0;
SendHighROS.roll = 0; SendHighROS.gaitType = 0;
SendHighROS.pitch = 0; SendHighROS.speedLevel = 0;
SendHighROS.yaw = 0; SendHighROS.footRaiseHeight = 0;
SendHighROS.bodyHeight = 0;
SendHighROS.euler[0] = 0;
SendHighROS.euler[1] = 0;
SendHighROS.euler[2] = 0;
SendHighROS.velocity[0] = 0.0f;
SendHighROS.velocity[1] = 0.0f;
SendHighROS.yawSpeed = 0.0f;
SendHighROS.reserve = 0;
if(motiontime>1000 && motiontime<1500){ if(motiontime > 0 && motiontime < 1000){
SendHighROS.mode = 1; SendHighROS.mode = 1;
// SendHighROS.roll = 0.3f; SendHighROS.euler[0] = -0.3;
} }
if(motiontime > 1000 && motiontime < 2000){
if(motiontime>1500 && motiontime<2000){
SendHighROS.mode = 1; SendHighROS.mode = 1;
SendHighROS.pitch = 0.3f; SendHighROS.euler[0] = 0.3;
} }
if(motiontime > 2000 && motiontime < 3000){
if(motiontime>2000 && motiontime<2500){
SendHighROS.mode = 1; SendHighROS.mode = 1;
SendHighROS.yaw = 0.2f; SendHighROS.euler[1] = -0.2;
} }
if(motiontime > 3000 && motiontime < 4000){
if(motiontime>2500 && motiontime<3000){
SendHighROS.mode = 1; SendHighROS.mode = 1;
SendHighROS.bodyHeight = -0.3f; SendHighROS.euler[1] = 0.2;
} }
if(motiontime>3000 && motiontime<3500){
SendHighROS.mode = 1;
SendHighROS.bodyHeight = 0.3f;
}
if(motiontime>3500 && motiontime<4000){
SendHighROS.mode = 1;
SendHighROS.bodyHeight = 0.0f;
}
if(motiontime > 4000 && motiontime < 5000){ if(motiontime > 4000 && motiontime < 5000){
SendHighROS.mode = 2; SendHighROS.mode = 1;
SendHighROS.euler[2] = -0.2;
} }
if(motiontime > 5000 && motiontime < 6000){
if(motiontime>5000 && motiontime<8500){ SendHighROS.mode = 1;
SendHighROS.mode = 2; SendHighROS.euler[2] = 0.2;
SendHighROS.forwardSpeed = 0.1f; // -1 ~ +1
} }
if(motiontime > 6000 && motiontime < 7000){
if(motiontime>8500 && motiontime<12000){ SendHighROS.mode = 1;
SendHighROS.mode = 2; SendHighROS.bodyHeight = -0.2;
SendHighROS.forwardSpeed = -0.2f; // -1 ~ +1
} }
if(motiontime > 7000 && motiontime < 8000){
if(motiontime>12000 && motiontime<16000){ SendHighROS.mode = 1;
SendHighROS.mode = 2; SendHighROS.bodyHeight = 0.1;
SendHighROS.rotateSpeed = 0.1f; // turn
} }
if(motiontime > 8000 && motiontime < 9000){
if(motiontime>16000 && motiontime<20000){ SendHighROS.mode = 1;
SendHighROS.mode = 2; SendHighROS.bodyHeight = 0.0;
SendHighROS.rotateSpeed = -0.1f; // turn
} }
if(motiontime > 9000 && motiontime < 11000){
if(motiontime>20000 && motiontime<21000){ SendHighROS.mode = 5;
}
if(motiontime > 11000 && motiontime < 13000){
SendHighROS.mode = 6;
}
if(motiontime > 13000 && motiontime < 14000){
SendHighROS.mode = 0;
}
if(motiontime > 14000 && motiontime < 18000){
SendHighROS.mode = 2;
SendHighROS.gaitType = 2;
SendHighROS.velocity[0] = 0.4f; // -1 ~ +1
SendHighROS.yawSpeed = 2;
SendHighROS.footRaiseHeight = 0.1;
// printf("walk\n");
}
if(motiontime > 18000 && motiontime < 20000){
SendHighROS.mode = 0;
SendHighROS.velocity[0] = 0;
}
if(motiontime > 20000 && motiontime < 24000){
SendHighROS.mode = 2;
SendHighROS.gaitType = 1;
SendHighROS.velocity[0] = 0.2f; // -1 ~ +1
SendHighROS.bodyHeight = 0.1;
// printf("walk\n");
}
if(motiontime>24000 ){
SendHighROS.mode = 1; SendHighROS.mode = 1;
} }
@ -136,27 +143,8 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm)
int main(int argc, char *argv[]){ int main(int argc, char *argv[]){
ros::init(argc, argv, "walk_ros_mode"); ros::init(argc, argv, "walk_ros_mode");
std::string firmwork;
ros::param::get("/firmwork", firmwork);
#ifdef SDK3_1
aliengo::Control control(aliengo::HIGHLEVEL);
aliengo::LCM roslcm;
mainHelper<aliengo::HighCmd, aliengo::HighState, aliengo::LCM>(argc, argv, roslcm);
#endif
#ifdef SDK3_2
std::string robot_name;
UNITREE_LEGGED_SDK::LeggedType rname;
ros::param::get("/robot_name", robot_name);
if(strcasecmp(robot_name.c_str(), "A1") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
// UNITREE_LEGGED_SDK::InitEnvironment();
UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL);
mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm); mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
} }