talk to Go1 robot without lcm
This commit is contained in:
Your Name
parent
3c0fe3c97d
commit
1ad059e38d
52
README.md
52
README.md
|
|
@ -1,10 +1,9 @@
|
|||
Packages Version: v3.4.0
|
||||
Packages Version: v3.5.0
|
||||
|
||||
# Introduction
|
||||
This package can send control command to real robot from ROS. You can do low-level control(namely control all joints on robot) and high-level control(namely control the walking direction and speed of robot).
|
||||
|
||||
This version is suitable for unitree_legged_sdk v3.4, namely Go1 robot.
|
||||
As for Aliengo or A1, please use the v3.2 release version of this package and unitree_legged_sdk v3.2.
|
||||
This version is suitable for unitree_legged_sdk v3.5.1, namely Go1 robot.
|
||||
|
||||
## Packages:
|
||||
|
||||
|
|
@ -12,12 +11,14 @@ Basic message function: `unitree_legged_msgs`
|
|||
|
||||
The interface between ROS and real robot: `unitree_legged_real`
|
||||
|
||||
## Environment
|
||||
We recommand users to run this package in Ubuntu 18.04 and ROS melodic environment
|
||||
|
||||
# Dependencies
|
||||
* [unitree_legged_sdk](https://github.com/unitreerobotics): v3.4
|
||||
* [unitree_legged_sdk](https://github.com/unitreerobotics): v3.5.1
|
||||
|
||||
# Configuration
|
||||
Before compiling this package, users have to modify the path to unitree_legged_sdk under the CMakeLists.txt of unitree_legged_real.
|
||||
Just search the "/home/bian/Robot_SDK/unitree_legged_sdk" and change it to your own path. If you are going to compile this package on a computer which is not AMD64 platform, then you have to search "libunitree_legged_sdk.so" under the CMakeLists.txt and change it to the .so file name which is suitable to your computer.
|
||||
Before compiling this package, users have to download unitree_legged-sdk v.3.5.1 to `~/catkin_ws/src` folder, which is the same folder where you put this package into
|
||||
|
||||
# Build
|
||||
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then:
|
||||
|
|
@ -51,20 +52,37 @@ Where the port name have to be changed to your own.
|
|||
# Run the package
|
||||
You can control your real Go1 robot from ROS by this package.
|
||||
|
||||
Before you run expamle program, please run command
|
||||
|
||||
```
|
||||
roslaunch unitree_legged_real real.launch ctrl_level:=highlevel
|
||||
```
|
||||
This command will launch a LCM server. The `ctrl_level` means the control level, which can be `lowlevel` or `highlevel`(case does not matter), and the default value is `highlevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose.
|
||||
|
||||
In order to send message to robot, you need to run the controller in another terminal:
|
||||
or
|
||||
```
|
||||
rosrun unitree_legged_real position_lcm
|
||||
```
|
||||
We offered some examples. When you run the low level controller, please make sure the robot is hanging up. The low level contains:
|
||||
```
|
||||
position_lcm
|
||||
velocity_lcm
|
||||
torque_lcm
|
||||
roslaunch unitree_legged_real real.launch ctrl_level:=lowlevel
|
||||
```
|
||||
|
||||
And when you run the high level controller, please make sure the robot is standing on the ground. The high level exmaple only has `walk_lcm`.
|
||||
It depends which control mode you want to use.
|
||||
|
||||
Then, if you want to run high-level control mode, you can run example_walk node like this
|
||||
```
|
||||
rosrun unitree_legged_real example_walk
|
||||
```
|
||||
|
||||
If you want to run low-level control mode, you can run example_position program node like this
|
||||
```
|
||||
rosrun unitree_legged_real example_postion
|
||||
```
|
||||
|
||||
You can also run the node state_sub to subscribe the feedback information from Go1 robot
|
||||
```
|
||||
rosrun unitree_legged_real state_sub
|
||||
```
|
||||
|
||||
And before you do the low-level control, please press L2+A to sit the robot down and then press L1+L2+start to make the robot into
|
||||
mode in which you can do joint-level control, finally make sure you hang the robot up before you run low-level control.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,33 +1,23 @@
|
|||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint16 commVersion # Old version Aliengo does not have
|
||||
uint16 robotID # Old version Aliengo does not have
|
||||
uint32 SN # Old version Aliengo does not have
|
||||
uint8 bandWidth # Old version Aliengo does not have
|
||||
uint8 mode # 0. idle, default stand
|
||||
# 1. force stand (controlled by dBodyHeight + ypr)
|
||||
# 2. target velocity walking (controlled by velocity + yawSpeed)
|
||||
# 3. target position walking (controlled by position + ypr[0])
|
||||
# 4. path mode walking (reserve for future release)
|
||||
# 5. position stand down.
|
||||
# 6. position stand up
|
||||
# 7. damping mode
|
||||
# 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
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
uint8 mode
|
||||
|
||||
uint8 gaitType
|
||||
uint8 speedLevel
|
||||
float32 footRaiseHeight
|
||||
float32 bodyHeight
|
||||
float32[2] position
|
||||
float32[3] euler
|
||||
float32[2] velocity
|
||||
float32 yawSpeed
|
||||
BmsCmd bms
|
||||
LED[4] led
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve # Old version Aliengo does not have
|
||||
int32 crc
|
||||
uint32 reserve
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,23 +1,28 @@
|
|||
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint16 commVersion # Old version Aliengo does not have
|
||||
uint16 robotID # Old version Aliengo does not have
|
||||
uint32 SN # Old version Aliengo does not have
|
||||
uint8 bandWidth # Old version Aliengo does not have
|
||||
uint8 mode
|
||||
float32 progress # new on Go1, reserve
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
IMU imu
|
||||
uint8 gaitType # new on Go1, 0.idle 1.trot 2.trot running 3.climb stair
|
||||
float32 footRaiseHeight # (unit: m, default: 0.08m), foot up height while walking
|
||||
float32[3] position # (unit: m), from own odometry in inertial frame, usually drift
|
||||
float32 bodyHeight # (unit: m, default: 0.28m)
|
||||
float32[3] velocity # (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
|
||||
float32 yawSpeed # (unit: rad/s), rotateSpeed in body frame
|
||||
Cartesian[4] footPosition2Body # foot position relative to body
|
||||
Cartesian[4] footSpeed2Body # foot speed relative to body
|
||||
#int8[20] temperature
|
||||
MotorState[20] motorState
|
||||
BmsState bms
|
||||
int16[4] footForce # Old version Aliengo is different
|
||||
int16[4] footForceEst # Old version Aliengo does not have
|
||||
int16[4] footForce
|
||||
int16[4] footForceEst
|
||||
uint8 mode
|
||||
float32 progress
|
||||
uint8 gaitType
|
||||
float32 footRaiseHeight
|
||||
float32[3] position
|
||||
float32 bodyHeight
|
||||
float32[3] velocity
|
||||
float32 yawSpeed
|
||||
float32[4] rangeObstacle
|
||||
Cartesian[4] footPosition2Body
|
||||
Cartesian[4] footSpeed2Body
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve # Old version Aliengo does not have
|
||||
uint32 reserve
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,4 +1,5 @@
|
|||
float32[4] quaternion
|
||||
float32[3] gyroscope
|
||||
float32[3] accelerometer
|
||||
float32[3] rpy
|
||||
int8 temperature
|
||||
|
|
@ -1,13 +1,14 @@
|
|||
uint8 levelFlag
|
||||
uint16 commVersion # Old version Aliengo does not have
|
||||
uint16 robotID # Old version Aliengo does not have
|
||||
uint32 SN # Old version Aliengo does not have
|
||||
uint8 bandWidth # Old version Aliengo does not have
|
||||
MotorCmd[20] motorCmd
|
||||
BmsCmd bms # new on Go1, battery command
|
||||
# LED[4] led
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve # Old version Aliengo does not have
|
||||
uint32 crc
|
||||
|
||||
Cartesian[4] ff # will delete # Old version Aliengo does not have
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
MotorCmd[20] motorCmd
|
||||
BmsCmd bms
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,21 +1,18 @@
|
|||
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint16 commVersion # Old version Aliengo does not have
|
||||
uint16 robotID # Old version Aliengo does not have
|
||||
uint32 SN # Old version Aliengo does not have
|
||||
uint8 bandWidth # Old version Aliengo does not have
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
IMU imu
|
||||
MotorState[20] motorState
|
||||
BmsState bms # new on Go1, battery state
|
||||
int16[4] footForce # force sensors # Old version Aliengo is different
|
||||
int16[4] footForceEst # force sensors # Old version Aliengo does not have
|
||||
uint32 tick # reference real-time from motion controller (unit: us)
|
||||
uint8[40] wirelessRemote # wireless commands
|
||||
uint32 reserve # Old version Aliengo does not have
|
||||
uint32 crc
|
||||
BmsState bms
|
||||
int16[4] footForce
|
||||
int16[4] footForceEst
|
||||
uint32 tick
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
|
||||
# Old version Aliengo does not have:
|
||||
Cartesian[4] eeForceRaw
|
||||
Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization
|
||||
Cartesian position # will delete
|
||||
Cartesian velocity # will delete
|
||||
Cartesian velocity_w # will delete
|
||||
uint32 crc
|
||||
|
|
|
|||
|
|
@ -9,37 +9,47 @@ find_package(catkin REQUIRED COMPONENTS
|
|||
unitree_legged_msgs
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}")
|
||||
if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*")
|
||||
set(ARCH amd64)
|
||||
else()
|
||||
set(ARCH arm64)
|
||||
endif()
|
||||
set(LEGGED_SDK_NAME -pthread libunitree_legged_sdk_${ARCH}.so lcm)
|
||||
|
||||
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
|
||||
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-O3 -fPIC")
|
||||
|
||||
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${Boost_INCLUDE_DIR}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${CMAKE_SOURCE_DIR}/unitree_legged_sdk-master/include
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-O3")
|
||||
|
||||
include_directories(/home/$ENV{USER}/Robot_SDK/unitree_legged_sdk/include)
|
||||
link_directories(/home/$ENV{USER}/Robot_SDK/unitree_legged_sdk/lib)
|
||||
string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_amd64.so)
|
||||
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
|
||||
|
||||
add_executable(lcm_server /home/$ENV{USER}/Robot_SDK/unitree_legged_sdk/examples/lcm_server.cpp)
|
||||
target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(lcm_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk-master/lib)
|
||||
|
||||
add_executable(position_lcm src/exe/position_mode.cpp)
|
||||
target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(position_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(velocity_lcm src/exe/velocity_mode.cpp)
|
||||
target_link_libraries(velocity_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(velocity_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(torque_lcm src/exe/torque_mode.cpp)
|
||||
target_link_libraries(torque_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(torque_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(walk_lcm src/exe/walk_mode.cpp)
|
||||
target_link_libraries(walk_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
add_executable(example_walk src/exe/example_walk.cpp)
|
||||
target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(example_position src/exe/example_position.cpp)
|
||||
target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(state_sub src/exe/state_sub.cpp)
|
||||
target_link_libraries(state_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(state_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(ros_udp src/exe/ros_udp.cpp)
|
||||
target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(ros_udp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
|
|
|
|||
|
|
@ -16,217 +16,365 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
|||
#include <unitree_legged_msgs/BmsState.h>
|
||||
#include <unitree_legged_msgs/IMU.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.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_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
|
||||
{
|
||||
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.accelerometer[0];
|
||||
ros.accelerometer[1] = lcm.accelerometer[1];
|
||||
ros.accelerometer[2] = lcm.accelerometer[2];
|
||||
ros.temperature = lcm.temperature;
|
||||
return ros;
|
||||
UNITREE_LEGGED_SDK::BmsCmd cmd;
|
||||
|
||||
cmd.off = msg.off;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
unitree_legged_msgs::MotorState ros;
|
||||
ros.mode = lcm.mode;
|
||||
ros.q = lcm.q;
|
||||
ros.dq = lcm.dq;
|
||||
ros.ddq = lcm.ddq;
|
||||
ros.tauEst = lcm.tauEst;
|
||||
ros.q_raw = lcm.q_raw;
|
||||
ros.dq_raw = lcm.dq_raw;
|
||||
ros.ddq_raw = lcm.ddq_raw;
|
||||
ros.temperature = lcm.temperature;
|
||||
ros.reserve[0] = lcm.reserve[0];
|
||||
ros.reserve[1] = lcm.reserve[1];
|
||||
return ros;
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
cmd.head[i] = msg->head[i];
|
||||
cmd.SN[i] = msg->SN[i];
|
||||
cmd.version[i] = msg->version[i];
|
||||
cmd.position[i] = msg->position[i];
|
||||
cmd.velocity[i] = msg->velocity[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.euler[i] = msg->euler[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
cmd.led[i].r = msg->led[i].r;
|
||||
cmd.led[i].g = msg->led[i].g;
|
||||
cmd.led[i].b = msg->led[i].b;
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
|
||||
cmd.levelFlag = msg->levelFlag;
|
||||
cmd.frameReserve = msg->frameReserve;
|
||||
cmd.bandWidth = msg->bandWidth;
|
||||
cmd.mode = msg->mode;
|
||||
cmd.gaitType = msg->gaitType;
|
||||
cmd.speedLevel = msg->speedLevel;
|
||||
cmd.footRaiseHeight = msg->footRaiseHeight;
|
||||
cmd.bodyHeight = msg->bodyHeight;
|
||||
cmd.yawSpeed = msg->yawSpeed;
|
||||
cmd.reserve = msg->reserve;
|
||||
cmd.crc = msg->crc;
|
||||
|
||||
cmd.bms = rosMsg2Cmd(msg->bms);
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType)
|
||||
UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::MotorCmd lcm;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.q = ros.q;
|
||||
lcm.dq = ros.dq;
|
||||
lcm.tau = ros.tau;
|
||||
lcm.Kp = ros.Kp;
|
||||
lcm.Kd = ros.Kd;
|
||||
lcm.reserve[0] = ros.reserve[0];
|
||||
lcm.reserve[1] = ros.reserve[1];
|
||||
lcm.reserve[2] = ros.reserve[2];
|
||||
return lcm;
|
||||
UNITREE_LEGGED_SDK::MotorCmd cmd;
|
||||
|
||||
cmd.mode = msg.mode;
|
||||
cmd.q = msg.q;
|
||||
cmd.dq = msg.dq;
|
||||
cmd.tau = msg.tau;
|
||||
cmd.Kp = msg.Kp;
|
||||
cmd.Kd = msg.Kd;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
|
||||
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
unitree_legged_msgs::LowState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = lcm.commVersion;
|
||||
ros.robotID = lcm.robotID;
|
||||
ros.SN = lcm.SN;
|
||||
ros.bandWidth = lcm.bandWidth;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
for(int i = 0; i<20; i++){
|
||||
ros.motorState[i] = ToRos(lcm.motorState[i]);
|
||||
UNITREE_LEGGED_SDK::LowCmd cmd;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
cmd.head[i] = msg->head[i];
|
||||
cmd.SN[i] = msg->SN[i];
|
||||
cmd.version[i] = msg->version[i];
|
||||
}
|
||||
ros.bms = ToRos(lcm.bms);
|
||||
for(int i = 0; i<4; i++){
|
||||
ros.footForce[i] = lcm.footForce[i];
|
||||
ros.footForceEst[i] = lcm.footForceEst[i];
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
ros.tick = lcm.tick;
|
||||
for(int i = 0; i<40; i++){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]);
|
||||
}
|
||||
ros.reserve = lcm.reserve;
|
||||
ros.crc = lcm.crc;
|
||||
return ros;
|
||||
|
||||
cmd.bms = rosMsg2Cmd(msg->bms);
|
||||
|
||||
cmd.levelFlag = msg->levelFlag;
|
||||
cmd.frameReserve = msg->frameReserve;
|
||||
cmd.bandWidth = msg->bandWidth;
|
||||
cmd.reserve = msg->reserve;
|
||||
cmd.crc = msg->crc;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType)
|
||||
unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::LowCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
lcm.commVersion = ros.commVersion;
|
||||
lcm.robotID = ros.robotID;
|
||||
lcm.SN = ros.SN;
|
||||
lcm.bandWidth = ros.bandWidth;
|
||||
for(int i = 0; i<20; i++){
|
||||
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
|
||||
}
|
||||
lcm.bms = ToLcm(ros.bms);
|
||||
for(int i = 0; i<40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
}
|
||||
lcm.reserve = ros.reserve;
|
||||
lcm.crc = ros.crc;
|
||||
return lcm;
|
||||
unitree_legged_msgs::MotorState ros_msg;
|
||||
|
||||
ros_msg.mode = state.mode;
|
||||
ros_msg.q = state.q;
|
||||
ros_msg.dq = state.dq;
|
||||
ros_msg.ddq = state.ddq;
|
||||
ros_msg.tauEst = state.tauEst;
|
||||
ros_msg.q_raw = state.q_raw;
|
||||
ros_msg.dq_raw = state.dq_raw;
|
||||
ros_msg.ddq_raw = state.ddq_raw;
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
ros_msg.reserve[0] = state.reserve[0];
|
||||
ros_msg.reserve[1] = state.reserve[1];
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
|
||||
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
|
||||
{
|
||||
unitree_legged_msgs::HighState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = lcm.commVersion;
|
||||
ros.robotID = lcm.robotID;
|
||||
ros.SN = lcm.SN;
|
||||
ros.bandWidth = lcm.bandWidth;
|
||||
ros.mode = lcm.mode;
|
||||
ros.progress = lcm.progress;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
ros.gaitType = lcm.gaitType;
|
||||
ros.footRaiseHeight = lcm.footRaiseHeight;
|
||||
ros.bodyHeight = lcm.bodyHeight;
|
||||
ros.yawSpeed = lcm.yawSpeed;
|
||||
ros.bms = ToRos(lcm.bms);
|
||||
ros.reserve = lcm.reserve;
|
||||
ros.crc = lcm.crc;
|
||||
unitree_legged_msgs::IMU ros_msg;
|
||||
|
||||
for(int i(0); i<3; ++i){
|
||||
ros.position[i] = lcm.position[i];
|
||||
ros.velocity[i] = lcm.velocity[i];
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.quaternion[i] = state.quaternion[i];
|
||||
}
|
||||
|
||||
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.footForceEst[i] = lcm.footForceEst[i];
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
ros_msg.gyroscope[i] = state.gyroscope[i];
|
||||
ros_msg.accelerometer[i] = state.accelerometer[i];
|
||||
ros_msg.rpy[i] = state.rpy[i];
|
||||
}
|
||||
|
||||
// for(int i(0); i<20; ++i){
|
||||
// ros.temperature[i] = lcm.temperature[i];
|
||||
// }
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
for(int i(0); i<40; ++i){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
}
|
||||
|
||||
return ros;
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType)
|
||||
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
lcm.commVersion = ros.commVersion;
|
||||
lcm.robotID = ros.robotID;
|
||||
lcm.SN = ros.SN;
|
||||
lcm.bandWidth = ros.bandWidth;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.gaitType = ros.gaitType;
|
||||
lcm.speedLevel = ros.speedLevel;
|
||||
lcm.footRaiseHeight = ros.footRaiseHeight;
|
||||
lcm.bodyHeight = ros.bodyHeight;
|
||||
lcm.yawSpeed = ros.yawSpeed;
|
||||
lcm.bms = ToLcm(ros.bms);
|
||||
lcm.reserve = ros.reserve;
|
||||
lcm.crc = ros.crc;
|
||||
unitree_legged_msgs::BmsState ros_msg;
|
||||
|
||||
for(int i(0); i<2; ++i){
|
||||
lcm.postion[i] = ros.postion[i];
|
||||
lcm.velocity[i] = ros.velocity[i];
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
|
||||
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
|
||||
}
|
||||
|
||||
for(int i(0); i<3; ++i){
|
||||
lcm.euler[i] = ros.euler[i];
|
||||
for (int i(0); i < 10; i++)
|
||||
{
|
||||
ros_msg.cell_vol[i] = state.cell_vol[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;
|
||||
}
|
||||
ros_msg.version_h = state.version_h;
|
||||
ros_msg.version_l = state.version_l;
|
||||
ros_msg.bms_status = state.bms_status;
|
||||
ros_msg.SOC = state.SOC;
|
||||
ros_msg.current = state.current;
|
||||
ros_msg.cycle = state.cycle;
|
||||
|
||||
for(int i = 0; i<40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
}
|
||||
|
||||
return lcm;
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state)
|
||||
{
|
||||
unitree_legged_msgs::LowState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.head[i] = state.head[i];
|
||||
ros_msg.SN[i] = state.SN[i];
|
||||
ros_msg.version[i] = state.version[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
|
||||
}
|
||||
|
||||
ros_msg.imu = state2rosMsg(state.imu);
|
||||
|
||||
ros_msg.bms = state2rosMsg(state.bms);
|
||||
|
||||
ros_msg.tick = state.tick;
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
|
||||
{
|
||||
unitree_legged_msgs::Cartesian ros_msg;
|
||||
|
||||
ros_msg.x = state.x;
|
||||
ros_msg.y = state.y;
|
||||
ros_msg.z = state.z;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
|
||||
{
|
||||
unitree_legged_msgs::HighState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.head[i] = state.head[i];
|
||||
ros_msg.SN[i] = state.SN[i];
|
||||
ros_msg.version[i] = state.version[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
ros_msg.rangeObstacle[i] = state.rangeObstacle[i];
|
||||
ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
|
||||
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
|
||||
}
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
ros_msg.position[i] = state.position[i];
|
||||
ros_msg.velocity[i] = state.velocity[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
|
||||
}
|
||||
|
||||
ros_msg.imu = state2rosMsg(state.imu);
|
||||
|
||||
ros_msg.bms = state2rosMsg(state.bms);
|
||||
|
||||
ros_msg.levelFlag = state.levelFlag;
|
||||
ros_msg.frameReserve = state.frameReserve;
|
||||
ros_msg.bandWidth = state.bandWidth;
|
||||
ros_msg.mode = state.mode;
|
||||
ros_msg.progress = state.progress;
|
||||
ros_msg.gaitType = state.gaitType;
|
||||
ros_msg.footRaiseHeight = state.footRaiseHeight;
|
||||
ros_msg.bodyHeight = state.bodyHeight;
|
||||
ros_msg.yawSpeed = state.yawSpeed;
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
class Custom
|
||||
{
|
||||
public:
|
||||
UDP low_udp;
|
||||
UDP high_udp;
|
||||
|
||||
HighCmd high_cmd = {0};
|
||||
HighState high_state = {0};
|
||||
|
||||
LowCmd low_cmd = {0};
|
||||
LowState low_state = {0};
|
||||
|
||||
public:
|
||||
Custom()
|
||||
: low_udp(LOWLEVEL),
|
||||
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
}
|
||||
|
||||
void cmdAssignment(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
|
||||
high_cmd = rosMsg2Cmd(msg);
|
||||
}
|
||||
|
||||
void cmdAssignment(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
|
||||
low_cmd = rosMsg2Cmd(msg);
|
||||
}
|
||||
|
||||
void stateAssignment(unitree_legged_msgs::LowState &low_state_ros)
|
||||
{
|
||||
|
||||
low_state_ros = state2rosMsg(low_state);
|
||||
}
|
||||
|
||||
void stateAssignment(unitree_legged_msgs::HighState &high_state_ros)
|
||||
{
|
||||
|
||||
high_state_ros = state2rosMsg(high_state);
|
||||
}
|
||||
|
||||
void highUdpSend()
|
||||
{
|
||||
// printf("high udp send is running\n");
|
||||
|
||||
high_udp.SetSend(high_cmd);
|
||||
high_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpSend()
|
||||
{
|
||||
|
||||
low_udp.SetSend(low_cmd);
|
||||
low_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpRecv()
|
||||
{
|
||||
|
||||
low_udp.Recv();
|
||||
low_udp.GetRecv(low_state);
|
||||
}
|
||||
|
||||
void highUdpRecv()
|
||||
{
|
||||
// printf("high udp recv is running\n");
|
||||
|
||||
high_udp.Recv();
|
||||
high_udp.GetRecv(high_state);
|
||||
}
|
||||
};
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
|
|
@ -1,8 +1,7 @@
|
|||
<launch>
|
||||
<arg name="ctrl_level" default="highlevel"/>
|
||||
|
||||
<node pkg="unitree_legged_real" type="lcm_server" name="node_lcm_server"
|
||||
respawn="false" output="screen" args="$(arg ctrl_level)" />
|
||||
<node pkg="unitree_legged_real" type="ros_udp" name="node_ros_udp" output="screen" args="$(arg ctrl_level)"/>
|
||||
|
||||
<param name="control_level" value="$(arg ctrl_level)"/>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_postition_without_lcm");
|
||||
|
||||
std::cout << "Communication level is set to LOW-level." << std::endl
|
||||
<< "WARNING: Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
int rate_count = 0;
|
||||
int sin_count = 0;
|
||||
float qInit[3] = {0};
|
||||
float qDes[3] = {0};
|
||||
float sin_mid_q[3] = {0.0, 1.2, -2.0};
|
||||
float Kp[3] = {0};
|
||||
float Kd[3] = {0};
|
||||
|
||||
unitree_legged_msgs::LowCmd low_cmd_ros;
|
||||
|
||||
bool initiated_flag = false; // initiate need time
|
||||
int count = 0;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::LowCmd>("low_cmd", 1);
|
||||
|
||||
low_cmd_ros.head[0] = 0xFE;
|
||||
low_cmd_ros.head[1] = 0xEF;
|
||||
low_cmd_ros.levelFlag = LOWLEVEL;
|
||||
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环
|
||||
low_cmd_ros.motorCmd[i].Kp = 0;
|
||||
low_cmd_ros.motorCmd[i].dq = VelStopF; // 禁止速度环
|
||||
low_cmd_ros.motorCmd[i].Kd = 0;
|
||||
low_cmd_ros.motorCmd[i].tau = 0;
|
||||
}
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
|
||||
if (initiated_flag == true)
|
||||
{
|
||||
motiontime += 2;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_0].tau = -0.65f;
|
||||
low_cmd_ros.motorCmd[FL_0].tau = +0.65f;
|
||||
low_cmd_ros.motorCmd[RR_0].tau = -0.65f;
|
||||
low_cmd_ros.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3);
|
||||
low_cmd_ros.motorCmd[FR_2].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_2].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_2].Kd = 1.0;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_0].q = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_0].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_0].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_0].Kd = 1.0;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_1].q = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_1].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_1].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_1].Kd = 1.0;
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
|
||||
count++;
|
||||
if (count > 10)
|
||||
{
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
}
|
||||
|
||||
pub.publish(low_cmd_ros);
|
||||
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_walk_without_lcm");
|
||||
|
||||
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
|
||||
<< "Make sure the robot is standing on the ground." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
|
||||
unitree_legged_msgs::HighCmd high_cmd_ros;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
|
||||
motiontime += 2;
|
||||
|
||||
high_cmd_ros.head[0] = 0xFE;
|
||||
high_cmd_ros.head[1] = 0xEF;
|
||||
high_cmd_ros.levelFlag = HIGHLEVEL;
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.speedLevel = 0;
|
||||
high_cmd_ros.footRaiseHeight = 0;
|
||||
high_cmd_ros.bodyHeight = 0;
|
||||
high_cmd_ros.euler[0] = 0;
|
||||
high_cmd_ros.euler[1] = 0;
|
||||
high_cmd_ros.euler[2] = 0;
|
||||
high_cmd_ros.velocity[0] = 0.0f;
|
||||
high_cmd_ros.velocity[1] = 0.0f;
|
||||
high_cmd_ros.yawSpeed = 0.0f;
|
||||
high_cmd_ros.reserve = 0;
|
||||
|
||||
if (motiontime > 0 && motiontime < 1000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = -0.3;
|
||||
}
|
||||
if (motiontime > 1000 && motiontime < 2000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = 0.3;
|
||||
}
|
||||
if (motiontime > 2000 && motiontime < 3000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = -0.2;
|
||||
}
|
||||
if (motiontime > 3000 && motiontime < 4000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = 0.2;
|
||||
}
|
||||
if (motiontime > 4000 && motiontime < 5000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = -0.2;
|
||||
}
|
||||
if (motiontime > 5000 && motiontime < 6000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = 0.2;
|
||||
}
|
||||
if (motiontime > 6000 && motiontime < 7000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = -0.2;
|
||||
}
|
||||
if (motiontime > 7000 && motiontime < 8000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.1;
|
||||
}
|
||||
if (motiontime > 8000 && motiontime < 9000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.0;
|
||||
}
|
||||
if (motiontime > 9000 && motiontime < 11000)
|
||||
{
|
||||
high_cmd_ros.mode = 5;
|
||||
}
|
||||
if (motiontime > 11000 && motiontime < 13000)
|
||||
{
|
||||
high_cmd_ros.mode = 6;
|
||||
}
|
||||
if (motiontime > 13000 && motiontime < 14000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
}
|
||||
if (motiontime > 14000 && motiontime < 18000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 2;
|
||||
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1
|
||||
high_cmd_ros.yawSpeed = 2;
|
||||
high_cmd_ros.footRaiseHeight = 0.1;
|
||||
// printf("walk\n");
|
||||
}
|
||||
if (motiontime > 18000 && motiontime < 20000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.velocity[0] = 0;
|
||||
}
|
||||
if (motiontime > 20000 && motiontime < 24000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 1;
|
||||
high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1
|
||||
high_cmd_ros.bodyHeight = 0.1;
|
||||
// printf("walk\n");
|
||||
}
|
||||
if (motiontime > 24000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
}
|
||||
|
||||
pub.publish(high_cmd_ros);
|
||||
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,165 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
double jointLinearInterpolation(double initPos, double targetPos, double rate)
|
||||
{
|
||||
double p;
|
||||
rate = std::min(std::max(rate, 0.0), 1.0);
|
||||
p = initPos*(1-rate) + targetPos*rate;
|
||||
return p;
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
int rate_count = 0;
|
||||
int sin_count = 0;
|
||||
float qInit[3]={0};
|
||||
float qDes[3]={0};
|
||||
float sin_mid_q[3] = {0.0, 1.2, -2.0};
|
||||
float Kp[3] = {0};
|
||||
float Kd[3] = {0};
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
bool initiated_flag = false; // initiate need time
|
||||
int count = 0;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
SendLowROS.motorCmd[i].q = PosStopF; // 禁止位置环
|
||||
SendLowROS.motorCmd[i].Kp = 0;
|
||||
SendLowROS.motorCmd[i].dq = VelStopF; // 禁止速度环
|
||||
SendLowROS.motorCmd[i].Kd = 0;
|
||||
SendLowROS.motorCmd[i].tau = 0;
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("FR_2 position: %f\n", RecvLowROS.motorState[FR_2].q);
|
||||
|
||||
if(initiated_flag == true){
|
||||
motiontime++;
|
||||
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
// printf("%d\n", motiontime);
|
||||
// printf("%d %f %f %f\n", FR_0, RecvLowROS.motorState[FR_0].q, RecvLowROS.motorState[FR_1].q, RecvLowROS.motorState[FR_2].q);
|
||||
// printf("%f %f \n", RecvLowROS.motorState[FR_0].mode, RecvLowROS.motorState[FR_1].mode);
|
||||
if( motiontime >= 0){
|
||||
// first, get record initial position
|
||||
// if( motiontime >= 100 && motiontime < 500){
|
||||
if( motiontime >= 0 && motiontime < 10){
|
||||
qInit[0] = RecvLowROS.motorState[FR_0].q;
|
||||
qInit[1] = RecvLowROS.motorState[FR_1].q;
|
||||
qInit[2] = RecvLowROS.motorState[FR_2].q;
|
||||
}
|
||||
if( motiontime >= 10 && motiontime < 400){
|
||||
// printf("%f %f %f\n", );
|
||||
rate_count++;
|
||||
double rate = rate_count/200.0; // needs count to 200
|
||||
Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0;
|
||||
Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0;
|
||||
|
||||
qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
|
||||
qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
|
||||
qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
|
||||
}
|
||||
double sin_joint1, sin_joint2;
|
||||
// last, do sine wave
|
||||
if( motiontime >= 400){
|
||||
sin_count++;
|
||||
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);
|
||||
qDes[0] = sin_mid_q[0];
|
||||
qDes[1] = sin_mid_q[1];
|
||||
qDes[2] = sin_mid_q[2] + sin_joint2;
|
||||
// qDes[2] = sin_mid_q[2];
|
||||
}
|
||||
|
||||
SendLowROS.motorCmd[FR_0].q = qDes[0];
|
||||
SendLowROS.motorCmd[FR_0].dq = 0;
|
||||
SendLowROS.motorCmd[FR_0].Kp = Kp[0];
|
||||
SendLowROS.motorCmd[FR_0].Kd = Kd[0];
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
|
||||
SendLowROS.motorCmd[FR_1].q = qDes[1];
|
||||
SendLowROS.motorCmd[FR_1].dq = 0;
|
||||
SendLowROS.motorCmd[FR_1].Kp = Kp[1];
|
||||
SendLowROS.motorCmd[FR_1].Kd = Kd[1];
|
||||
SendLowROS.motorCmd[FR_1].tau = 0.0f;
|
||||
|
||||
SendLowROS.motorCmd[FR_2].q = qDes[2];
|
||||
SendLowROS.motorCmd[FR_2].dq = 0;
|
||||
SendLowROS.motorCmd[FR_2].Kp = Kp[2];
|
||||
SendLowROS.motorCmd[FR_2].Kd = Kd[2];
|
||||
SendLowROS.motorCmd[FR_2].tau = 0.0f;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
|
||||
count++;
|
||||
if(count > 10){
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "position_ros_mode");
|
||||
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
}
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
#include <chrono>
|
||||
#include <pthread.h>
|
||||
|
||||
Custom custom;
|
||||
|
||||
ros::Subscriber sub_high;
|
||||
ros::Subscriber sub_low;
|
||||
|
||||
ros::Publisher pub_high;
|
||||
ros::Publisher pub_low;
|
||||
|
||||
long high_count = 0;
|
||||
long low_count = 0;
|
||||
|
||||
void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
printf("highCmdCallback is running !\t%ld\n", ::high_count);
|
||||
|
||||
custom.cmdAssignment(msg);
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
custom.stateAssignment(high_state_ros);
|
||||
|
||||
pub_high.publish(high_state_ros);
|
||||
|
||||
printf("highCmdCallback ending !\t%ld\n\n", ::high_count++);
|
||||
}
|
||||
|
||||
void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
|
||||
printf("lowCmdCallback is running !\t%ld\n", low_count);
|
||||
|
||||
custom.cmdAssignment(msg);
|
||||
|
||||
unitree_legged_msgs::LowState low_state_ros;
|
||||
|
||||
custom.stateAssignment(low_state_ros);
|
||||
|
||||
pub_low.publish(low_state_ros);
|
||||
|
||||
printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "ros_udp");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
if (strcasecmp(argv[1], "LOWLEVEL") == 0)
|
||||
{
|
||||
sub_low = nh.subscribe("low_cmd", 1, lowCmdCallback);
|
||||
pub_low = nh.advertise<unitree_legged_msgs::LowState>("low_state", 1);
|
||||
|
||||
LoopFunc loop_udpSend("low_udp_send", 0.002, 3, boost::bind(&Custom::lowUdpSend, &custom));
|
||||
LoopFunc loop_udpRecv("low_udp_recv", 0.002, 3, boost::bind(&Custom::lowUdpRecv, &custom));
|
||||
|
||||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("low level runing!\n");
|
||||
}
|
||||
else if (strcasecmp(argv[1], "HIGHLEVEL") == 0)
|
||||
{
|
||||
sub_high = nh.subscribe("high_cmd", 1, highCmdCallback);
|
||||
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
|
||||
|
||||
LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom));
|
||||
LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom));
|
||||
|
||||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("high level runing!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Control level name error! Can only be highlevel or lowlevel(not case sensitive)" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/MotorCmd.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_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.h>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &msg)
|
||||
{
|
||||
printf("yaw = %f\n", msg->imu.rpy[2]);
|
||||
}
|
||||
|
||||
void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &msg)
|
||||
{
|
||||
printf("FR_2_pos = %f\n", msg->motorState[FR_2].q);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "node_high_state_sub");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
ros::Subscriber high_sub = nh.subscribe("high_state", 1, highStateCallback);
|
||||
ros::Subscriber low_sub = nh.subscribe("low_state", 1, lowStateCallback);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,90 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime=0;
|
||||
float torque = 0;
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime++;
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("FL_1 position: %f\n", RecvLowROS.motorState[FL_1].q);
|
||||
|
||||
// gravity compensation
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
if( motiontime >= 500){
|
||||
torque = (0 - RecvLowROS.motorState[FL_1].q)*10.0f + (0 - RecvLowROS.motorState[FL_1].dq)*1.0f;
|
||||
if(torque > 5.0f) torque = 5.0f;
|
||||
if(torque < -5.0f) torque = -5.0f;
|
||||
|
||||
SendLowROS.motorCmd[FL_1].q = PosStopF;
|
||||
SendLowROS.motorCmd[FL_1].dq = VelStopF;
|
||||
SendLowROS.motorCmd[FL_1].Kp = 0;
|
||||
SendLowROS.motorCmd[FL_1].Kd = 0;
|
||||
SendLowROS.motorCmd[FL_1].tau = torque;
|
||||
}
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "torque_ros_mode");
|
||||
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
}
|
||||
|
|
@ -1,93 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime=0;
|
||||
float Kv = 1.5;
|
||||
float speed = 0;
|
||||
unsigned long int Tpi =0;
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime++;
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("motorState[FL_2] velocity: %f\n", RecvLowROS.motorState[FL_2].dq);
|
||||
|
||||
// gravity compensation
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
if( motiontime >= 500){
|
||||
SendLowROS.motorCmd[FL_2].q = PosStopF;
|
||||
SendLowROS.motorCmd[FL_2].dq = speed;
|
||||
SendLowROS.motorCmd[FL_2].Kp = 0;
|
||||
SendLowROS.motorCmd[FL_2].Kd = Kv;
|
||||
SendLowROS.motorCmd[FL_2].tau = 0.0f;
|
||||
Tpi++;
|
||||
// try 1 or 3
|
||||
speed = 3 * sin(4*M_PI*Tpi/1000.0);
|
||||
}
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "velocity_ros_mode");
|
||||
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
}
|
||||
|
|
@ -1,150 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pthread.h>
|
||||
#include <string>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
|
||||
<< "Make sure the robot is standing on the ground." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
// SetLevel(HIGHLEVEL);
|
||||
long motiontime = 0;
|
||||
TCmd SendHighLCM = {0};
|
||||
TState RecvHighLCM = {0};
|
||||
unitree_legged_msgs::HighCmd SendHighROS;
|
||||
unitree_legged_msgs::HighState RecvHighROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime = motiontime+2;
|
||||
roslcm.Get(RecvHighLCM);
|
||||
RecvHighROS = ToRos(RecvHighLCM);
|
||||
|
||||
SendHighROS.mode = 0;
|
||||
SendHighROS.gaitType = 0;
|
||||
SendHighROS.speedLevel = 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 > 0 && motiontime < 1000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[0] = -0.3;
|
||||
}
|
||||
if(motiontime > 1000 && motiontime < 2000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[0] = 0.3;
|
||||
}
|
||||
if(motiontime > 2000 && motiontime < 3000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[1] = -0.2;
|
||||
}
|
||||
if(motiontime > 3000 && motiontime < 4000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[1] = 0.2;
|
||||
}
|
||||
if(motiontime > 4000 && motiontime < 5000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[2] = -0.2;
|
||||
}
|
||||
if(motiontime > 5000 && motiontime < 6000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.euler[2] = 0.2;
|
||||
}
|
||||
if(motiontime > 6000 && motiontime < 7000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = -0.2;
|
||||
}
|
||||
if(motiontime > 7000 && motiontime < 8000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = 0.1;
|
||||
}
|
||||
if(motiontime > 8000 && motiontime < 9000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = 0.0;
|
||||
}
|
||||
if(motiontime > 9000 && motiontime < 11000){
|
||||
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;
|
||||
}
|
||||
|
||||
SendHighLCM = ToLcm(SendHighROS, SendHighLCM);
|
||||
roslcm.Send(SendHighLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "walk_ros_mode");
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue