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

suitable for sdk 3.2

This commit is contained in:
Bian Zekun 2021-09-01 20:46:41 +08:00
commit ce58cb9c10
22 changed files with 1282 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
.vscode/

48
unitree_legged_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,48 @@
cmake_minimum_required(VERSION 2.8.3)
project(unitree_legged_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
std_msgs
geometry_msgs
sensor_msgs
)
add_message_files(
FILES
MotorCmd.msg
MotorState.msg
Cartesian.msg
IMU.msg
LED.msg
LowCmd.msg
LowState.msg
HighCmd.msg
HighState.msg
)
generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
sensor_msgs
)
catkin_package(
CATKIN_DEPENDS
message_runtime
std_msgs
geometry_msgs
sensor_msgs
)
#############
## Install ##
#############
# Mark topic names header files for installation
install(
DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

3
unitree_legged_msgs/msg/Cartesian.msg Normal file
View File

@ -0,0 +1,3 @@
float32 x
float32 y
float32 z

19
unitree_legged_msgs/msg/HighCmd.msg Normal file
View File

@ -0,0 +1,19 @@
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 forwardSpeed
float32 sideSpeed
float32 rotateSpeed
float32 bodyHeight
float32 footRaiseHeight
float32 yaw
float32 pitch
float32 roll
LED[4] led
uint8[40] wirelessRemote
uint8[40] AppRemote # Old version Aliengo does not have
uint32 reserve # Old version Aliengo does not have
int32 crc

26
unitree_legged_msgs/msg/HighState.msg Normal file
View File

@ -0,0 +1,26 @@
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
IMU imu
float32 forwardSpeed
float32 sideSpeed
float32 rotateSpeed
float32 bodyHeight
float32 updownSpeed
float32 forwardPosition # (will be float type next version) # Old version Aliengo is different
float32 sidePosition # (will be float type next version) # Old version Aliengo is different
Cartesian[4] footPosition2Body
Cartesian[4] footSpeed2Body
int16[4] footForce # Old version Aliengo is different
int16[4] footForceEst # Old version Aliengo does not have
uint32 tick
uint8[40] wirelessRemote
uint32 reserve # Old version Aliengo does not have
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

4
unitree_legged_msgs/msg/IMU.msg Normal file
View File

@ -0,0 +1,4 @@
float32[4] quaternion
float32[3] gyroscope
float32[3] accelerometer
int8 temperature

3
unitree_legged_msgs/msg/LED.msg Normal file
View File

@ -0,0 +1,3 @@
uint8 r
uint8 g
uint8 b

12
unitree_legged_msgs/msg/LowCmd.msg Normal file
View File

@ -0,0 +1,12 @@
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
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

20
unitree_legged_msgs/msg/LowState.msg Normal file
View File

@ -0,0 +1,20 @@
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
IMU imu
MotorState[20] motorState
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
# 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

7
unitree_legged_msgs/msg/MotorCmd.msg Normal file
View File

@ -0,0 +1,7 @@
uint8 mode # motor target mode
float32 q # motor target position
float32 dq # motor target velocity
float32 tau # motor target torque
float32 Kp # motor spring stiffness coefficient
float32 Kd # motor damper coefficient
uint32[3] reserve # motor target torque

10
unitree_legged_msgs/msg/MotorState.msg Normal file
View File

@ -0,0 +1,10 @@
uint8 mode # motor current mode
float32 q # motor current position（rad）
float32 dq # motor current speed（rad/s）
float32 ddq # motor current speed（rad/s）
float32 tauEst # current estimated output torque（N*m）
float32 q_raw # motor current position（rad）
float32 dq_raw # motor current speed（rad/s）
float32 ddq_raw # motor current speed（rad/s）
int8 temperature # motor temperature（slow conduction of temperature leads to lag）
uint32[2] reserve

19
unitree_legged_msgs/package.xml Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<package format="2">
<name>unitree_legged_msgs</name>
<version>0.0.0</version>
<description>
The test messgaes package.
</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>message_runtime</depend>
<depend>message_generation</depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
</package>

61
unitree_legged_real/CMakeLists.txt Executable file
View File

@ -0,0 +1,61 @@
cmake_minimum_required(VERSION 2.8.3)
project(unitree_legged_real)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
unitree_legged_msgs
)
catkin_package()
include_directories(
include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)
set(CMAKE_CXX_FLAGS "-O3")
if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1")
include_directories($ENV{ALIENGO_SDK_PATH}/include)
link_directories($ENV{ALIENGO_SDK_PATH}/lib)
string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so)
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
add_definitions(-DSDK3_1)
add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp)
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)
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})

87
unitree_legged_real/README.md Normal file
View File

@ -0,0 +1,87 @@
Packages Version: v3.2.1
# 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.2 and v3.1, namely A1 and Aliengo robot. Currently, all the A1 and Aliengo use v3.2. If your Aliengo is really old, then it may use v3.1.
As for Go1, please use the v3.4 release version of this package and unitree_legged_sdk v3.4.
# Dependencies
* [unitree_legged_msgs](https://github.com/unitreerobotics/unitree_ros): `unitree_legged_msgs` is a package of [unitree_ros](https://github.com/unitreerobotics/unitree_ros).
* [unitree_legged_sdk](https://github.com/unitreerobotics): If your robot is suitable for `unitree_legged_sdk`, then you do not need `aliengo_sdk`.
* [aliengo_sdk](https://github.com/unitreerobotics): If your robot is suitable for `aliengo_sdk`, then you do not need `unitree_legged_sdk`.
# Configuration
Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case.
If your use `unitree_legged_sdk`, then you need to set `UNITREE_SDK_VERSION=3_2` and the path `UNITREE_LEGGED_SDK_PATH`.
Otherwise, if you use `aliengo_sdk`, you need to set `UNITREE_SDK_VERSION=3_1` and the path `ALIENGO_SDK_PATH`.
```
source /opt/ros/melodic/setup.bash
source /usr/share/gazebo-8/setup.sh
source ~/catkin_ws/devel/setup.bash
export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH}
export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH}
export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH}
# 3_1, 3_2
export UNITREE_SDK_VERSION=3_2
export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk
export ALIENGO_SDK_PATH=~/aliengo_sdk
# amd64, arm32, arm64
export UNITREE_PLATFORM="amd64"
```
# Build
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then:
```
cd ~/catkin_ws
catkin_make
```
Before compiling `unitree_legged_real`, please make sure that the `unitree_legged_msgs` has been compiled.
# Setup the net connection
First, please connect the network cable between your PC and robot. Then run `ifconfig` in a terminal, you will find your port name. For example, `enx000ec6612921`.
Then, open the `ipconfig.sh` file under the folder `unitree_legged_real`, modify the port name to your own. And run the following commands:
```
sudo chmod +x ipconfig.sh
sudo ./ipconfig.sh
```
If you run the `ifconfig` again, you will find that port has `inet` and `netmask` now.
In order to set your port automatically, you can modify `interfaces`:
```
sudo gedit /etc/network/interfaces
```
And add the following 4 lines at the end:
```
auto enx000ec6612921
iface enx000ec6612921 inet static
address 192.168.123.162
netmask 255.255.255.0
```
Where the port name have to be changed to your own.
# Run the package
You can control your real robot(only A1 and Aliengo) from ROS by this package.
First you have to run the `real_launch` under root account:
```
sudo su
source /home/yourUserName/catkin_ws/devel/setup.bash
roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=highlevel firmwork:=3_2
```
Please watchout that the `/home/yourUserName` means the home directory of yourself. These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And 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. The `firmwork` means the firmwork version of the robot. The default value is `3_2` Now all the A1's firmwork version is `3_2`.
In order to send message to robot, you need to run the controller in another terminal(also under root account):
```
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
```
The `velocity_lcm` and `torque_lcm` have to run under root account too. Please use the same method as runing `real_launch`.
And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`.

338
unitree_legged_real/include/convert.h Executable file
View File

@ -0,0 +1,338 @@
/************************************************************************
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.
************************************************************************/
#ifndef _CONVERT_H_
#define _CONVERT_H_
#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/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"
unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::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.accelerometer[0];
ros.accelerometer[1] = lcm.accelerometer[1];
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;
return ros;
}
unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
{
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::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType)
{
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_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
{
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]);
}
for(int i = 0; i<4; i++){
ros.footForce[i] = lcm.footForce[i];
ros.footForceEst[i] = lcm.footForceEst[i];
}
ros.tick = lcm.tick;
for(int i = 0; i<40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.reserve = lcm.reserve;
ros.crc = lcm.crc;
return ros;
}
UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType)
{
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]);
}
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.reserve = ros.reserve;
lcm.crc = ros.crc;
return lcm;
}
unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
{
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.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;
ros.sidePosition = lcm.sidePosition;
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] = lcm.footForceEst[i];
}
ros.tick = lcm.tick;
for(int i = 0; i<40; i++){
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
}
ros.reserve = lcm.reserve;
ros.crc = lcm.crc;
return ros;
}
UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType)
{
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.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.AppRemote[i] = ros.AppRemote[i];
}
lcm.reserve = ros.reserve;
lcm.crc = ros.crc;
return lcm;
}
#endif
#endif // _CONVERT_H_

8
unitree_legged_real/ipconfig.sh Executable file
View File

@ -0,0 +1,8 @@
#!/bin/bash
sudo ifconfig lo multicast
sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo
sudo ifconfig enx000ec6612921 down
sudo ifconfig enx000ec6612921 up 192.168.123.162 netmask 255.255.255.0

12
unitree_legged_real/launch/real.launch Normal file
View File

@ -0,0 +1,12 @@
<launch>
<arg name="rname" default="a1"/>
<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"
respawn="false" output="screen" args="$(arg rname) $(arg ctrl_level)" />
<param name="robot_name" value="$(arg rname)"/>
<param name="control_level" value="$(arg ctrl_level)"/>
<param name="firmwork" value="$(arg firmwork)"/>
</launch>

20
unitree_legged_real/package.xml Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0"?>
<package format="2">
<name>unitree_legged_real</name>
<version>0.0.0</version>
<description>The unitree_legged_real package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<depend>unitree_legged_msgs</depend>
<depend>eigen</depend>
</package>

190
unitree_legged_real/src/exe/position_mode.cpp Executable file
View File

@ -0,0 +1,190 @@
/************************************************************************
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"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK;
#endif
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 >= 1700){
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");
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);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
}

114
unitree_legged_real/src/exe/torque_mode.cpp Executable file
View File

@ -0,0 +1,114 @@
/************************************************************************
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"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK;
#endif
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");
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);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
}

118
unitree_legged_real/src/exe/velocity_mode.cpp Executable file
View File

@ -0,0 +1,118 @@
/************************************************************************
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"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK;
#endif
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");
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);
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
}

162
unitree_legged_real/src/exe/walk_mode.cpp Executable file
View File

@ -0,0 +1,162 @@
/************************************************************************
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"
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK;
#endif
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);
// 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.roll = 0;
SendHighROS.pitch = 0;
SendHighROS.yaw = 0;
if(motiontime>1000 && motiontime<1500){
SendHighROS.mode = 1;
// SendHighROS.roll = 0.3f;
}
if(motiontime>1500 && motiontime<2000){
SendHighROS.mode = 1;
SendHighROS.pitch = 0.3f;
}
if(motiontime>2000 && motiontime<2500){
SendHighROS.mode = 1;
SendHighROS.yaw = 0.2f;
}
if(motiontime>2500 && motiontime<3000){
SendHighROS.mode = 1;
SendHighROS.bodyHeight = -0.3f;
}
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){
SendHighROS.mode = 2;
}
if(motiontime>5000 && motiontime<8500){
SendHighROS.mode = 2;
SendHighROS.forwardSpeed = 0.1f; // -1 ~ +1
}
if(motiontime>8500 && motiontime<12000){
SendHighROS.mode = 2;
SendHighROS.forwardSpeed = -0.2f; // -1 ~ +1
}
if(motiontime>12000 && motiontime<16000){
SendHighROS.mode = 2;
SendHighROS.rotateSpeed = 0.1f; // turn
}
if(motiontime>16000 && motiontime<20000){
SendHighROS.mode = 2;
SendHighROS.rotateSpeed = -0.1f; // turn
}
if(motiontime>20000 && motiontime<21000){
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");
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);
mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif
}