suitable for sdk 3.2
This commit is contained in:
Bian Zekun
commit
ce58cb9c10
|
|
@ -0,0 +1 @@
|
||||||
|
.vscode/
|
||||||
|
|
@ -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"
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
float32 x
|
||||||
|
float32 y
|
||||||
|
float32 z
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,4 @@
|
||||||
|
float32[4] quaternion
|
||||||
|
float32[3] gyroscope
|
||||||
|
float32[3] accelerometer
|
||||||
|
int8 temperature
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
uint8 r
|
||||||
|
uint8 g
|
||||||
|
uint8 b
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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})
|
||||||
|
|
@ -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`.
|
||||||
|
|
@ -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_
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue