Compare commits
5 Commits
| Author | SHA1 | Date |
|---|---|---|
LuoBin
|
b6227b8c60 | |
|
LuoBin
|
f608c3589d | |
Bin-Ro
|
b8c9beb055 | |
|
Bin-Ro
|
60673ae013 | |
|
LuoBin
|
e3e96db914 |
61
README.md
61
README.md
|
|
@ -1,9 +1,9 @@
|
|||
Packages Version: v3.8.0
|
||||
Packages Version: v3.8.4
|
||||
|
||||
# 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.5.1, namely Go1 robot.
|
||||
This version is suitable for unitree_legged_sdk v3.8.4, namely Aliengo robot.
|
||||
|
||||
## Packages:
|
||||
|
||||
|
|
@ -12,51 +12,24 @@ 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
|
||||
We recommand users to run this package in Ubuntu 20.04 and ROS neotic environment
|
||||
|
||||
## Dependencies
|
||||
## Dependency
|
||||
* [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk/releases)
|
||||
### Notice
|
||||
The newest release [v3.8.0](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.8.0) only supports for robot: Go1.
|
||||
|
||||
Check release [v3.3.4](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.3.4) for A1 support.
|
||||
|
||||
# Configuration
|
||||
Before compiling this package, please download the corresponding unitree_legged_sdk as noted above, and put it to your own workspace's source folder(e.g. `~/catkin_ws/src`). Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag.
|
||||
The release [v3.8.4](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.8.4) only supports for robot: Aliengo.
|
||||
|
||||
# Build
|
||||
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then:
|
||||
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then download the corresponding unitree_legged_sdk into `~/catkin_ws/src/unitree_ros_to_real`.Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag:
|
||||
```
|
||||
cd ~/catkin_ws
|
||||
catkin_make
|
||||
```
|
||||
|
||||
# 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 Go1 robot from ROS by this package.
|
||||
You can control your real Aliengo robot from ROS by this package.
|
||||
|
||||
Before you run expamle program, please run command
|
||||
Before you run expamle program, please run
|
||||
|
||||
```
|
||||
roslaunch unitree_legged_real real.launch ctrl_level:=highlevel
|
||||
|
|
@ -68,25 +41,15 @@ roslaunch unitree_legged_real real.launch ctrl_level:=lowlevel
|
|||
|
||||
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
|
||||
Then, if you want to run high-level control mode, you can run example_walk node
|
||||
```
|
||||
rosrun unitree_legged_real example_walk
|
||||
```
|
||||
|
||||
If you want to run low-level control mode, you can run example_position program node like this
|
||||
If you want to run low-level control mode, you can run example_position program node
|
||||
```
|
||||
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
|
||||
```
|
||||
|
||||
You can also run the launch file that enables you control robot via keyboard like you can do in turtlesim package
|
||||
```
|
||||
roslaunch unitree_legged_real keyboard_control.launch
|
||||
rosrun unitree_legged_real example_position
|
||||
```
|
||||
|
||||
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.
|
||||
mode in which you can do low-level control, finally make sure you hang the robot up before you run low-level control.
|
||||
|
|
|
|||
|
|
@ -12,8 +12,6 @@ add_message_files(
|
|||
FILES
|
||||
MotorCmd.msg
|
||||
MotorState.msg
|
||||
BmsCmd.msg
|
||||
BmsState.msg
|
||||
Cartesian.msg
|
||||
IMU.msg
|
||||
LED.msg
|
||||
|
|
|
|||
|
|
@ -1,2 +0,0 @@
|
|||
uint8 off # off 0xA5
|
||||
uint8[3] reserve
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
uint8 version_h
|
||||
uint8 version_l
|
||||
uint8 bms_status
|
||||
uint8 SOC # SOC 0-100%
|
||||
int32 current # mA
|
||||
uint16 cycle
|
||||
int8[2] BQ_NTC # x1 degrees centigrade
|
||||
int8[2] MCU_NTC # x1 degrees centigrade
|
||||
uint16[10] cell_vol # cell voltage mV
|
||||
|
|
@ -1,23 +1,18 @@
|
|||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
uint16 commVersion
|
||||
uint16 robotID
|
||||
uint32 SN
|
||||
uint8 bandWidth
|
||||
uint8 mode
|
||||
|
||||
uint8 gaitType
|
||||
uint8 speedLevel
|
||||
float32 footRaiseHeight
|
||||
float32 bodyHeight
|
||||
uint8 speedLevel
|
||||
float32 dFootRaiseHeight
|
||||
float32 dBodyHeight
|
||||
float32[2] position
|
||||
float32[3] euler
|
||||
float32[3] rpy
|
||||
float32[2] velocity
|
||||
float32 yawSpeed
|
||||
BmsCmd bms
|
||||
float32 yawSpeed
|
||||
LED[4] led
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,28 +1,21 @@
|
|||
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
IMU imu
|
||||
MotorState[20] motorState
|
||||
BmsState bms
|
||||
int16[4] footForce
|
||||
int16[4] footForceEst
|
||||
uint16 commVersion
|
||||
uint16 robotID
|
||||
uint32 SN
|
||||
uint8 bandWidth
|
||||
uint8 mode
|
||||
float32 progress
|
||||
uint8 gaitType
|
||||
float32 footRaiseHeight
|
||||
IMU imu
|
||||
float32[3] position
|
||||
float32 bodyHeight
|
||||
float32[3] velocity
|
||||
float32 yawSpeed
|
||||
float32[4] rangeObstacle
|
||||
float32 yawSpeed
|
||||
Cartesian[4] footPosition2Body
|
||||
Cartesian[4] footSpeed2Body
|
||||
int16[4] footForce
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
uint32 crc
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,14 +1,12 @@
|
|||
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
uint16 commVersion
|
||||
uint16 robotID
|
||||
uint32 SN
|
||||
uint8 bandWidth
|
||||
MotorCmd[20] motorCmd
|
||||
BmsCmd bms
|
||||
LED[4] led
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
uint32 crc
|
||||
|
||||
|
||||
uint32 crc
|
||||
|
|
@ -1,25 +1,22 @@
|
|||
|
||||
uint8[2] head
|
||||
uint8 levelFlag
|
||||
uint8 frameReserve
|
||||
|
||||
uint32[2] SN
|
||||
uint32[2] version
|
||||
uint16 bandWidth
|
||||
uint16 commVersion
|
||||
uint16 robotID
|
||||
uint32 SN
|
||||
uint8 bandWidth
|
||||
IMU imu
|
||||
MotorState[20] motorState
|
||||
BmsState bms
|
||||
int16[4] footForce
|
||||
int16[4] footForceEst
|
||||
uint32 tick
|
||||
int16[4] footForceEst
|
||||
uint32 tick
|
||||
uint8[40] wirelessRemote
|
||||
uint32 reserve
|
||||
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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ else()
|
|||
set(ARCH arm64)
|
||||
endif()
|
||||
|
||||
link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH})
|
||||
link_directories(${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/lib/cpp/${ARCH})
|
||||
|
||||
set(EXTRA_LIBS -pthread libunitree_legged_sdk.so)
|
||||
|
||||
|
|
@ -27,16 +27,11 @@ set(CMAKE_CXX_FLAGS "-O3 -fPIC")
|
|||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${CMAKE_SOURCE_DIR}/unitree_legged_sdk/include
|
||||
${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/include
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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})
|
||||
|
|
@ -45,19 +40,9 @@ 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})
|
||||
|
||||
add_executable(control_via_keyboard src/exe/control_via_keyboard.cpp)
|
||||
target_link_libraries(control_via_keyboard ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(control_via_keyboard ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(twist_sub src/exe/twist_sub.cpp)
|
||||
target_link_libraries(twist_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(twist_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
|
|
|
|||
|
|
@ -6,76 +6,56 @@ 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/BmsCmd.h>
|
||||
#include <unitree_legged_msgs/BmsState.h>
|
||||
#include <unitree_legged_msgs/IMU.h>
|
||||
#include "unitree_legged_msgs/Cartesian.h"
|
||||
#include "unitree_legged_msgs/HighCmd.h"
|
||||
#include "unitree_legged_msgs/HighState.h"
|
||||
#include "unitree_legged_msgs/IMU.h"
|
||||
#include "unitree_legged_msgs/LED.h"
|
||||
#include "unitree_legged_msgs/LowCmd.h"
|
||||
#include "unitree_legged_msgs/LowState.h"
|
||||
#include "unitree_legged_msgs/MotorCmd.h"
|
||||
#include "unitree_legged_msgs/MotorState.h"
|
||||
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "ros/ros.h"
|
||||
|
||||
UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
|
||||
|
||||
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::BmsCmd cmd;
|
||||
unitree_legged_msgs::Cartesian ros_msg;
|
||||
|
||||
cmd.off = msg.off;
|
||||
ros_msg.x = state.x;
|
||||
ros_msg.y = state.y;
|
||||
ros_msg.z = state.z;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
unitree_legged_msgs::IMU ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
for (std::size_t i(0); i < 4; ++i)
|
||||
ros_msg.quaternion[i] = state.quaternion[i];
|
||||
|
||||
for (std::size_t i(0); i < 3; ++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];
|
||||
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 < 3; i++)
|
||||
{
|
||||
cmd.euler[i] = msg->euler[i];
|
||||
}
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
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;
|
||||
}
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
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);
|
||||
UNITREE_LEGGED_SDK::LED rosMsg2Cmd(const unitree_legged_msgs::LED &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::LED cmd;
|
||||
cmd.r = msg.r;
|
||||
cmd.g = msg.g;
|
||||
cmd.b = msg.b;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
|
@ -91,45 +71,13 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
|
|||
cmd.Kp = msg.Kp;
|
||||
cmd.Kd = msg.Kd;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
for (std::size_t i(0); i < 3; ++i)
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
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];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]);
|
||||
}
|
||||
|
||||
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_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state)
|
||||
{
|
||||
|
|
@ -151,48 +99,78 @@ unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &sta
|
|||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd &msg)
|
||||
{
|
||||
unitree_legged_msgs::IMU ros_msg;
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
cmd.levelFlag = msg.levelFlag;
|
||||
cmd.commVersion = msg.commVersion;
|
||||
cmd.robotID = msg.robotID;
|
||||
cmd.SN = msg.SN;
|
||||
cmd.bandWidth = msg.bandWidth;
|
||||
cmd.mode = msg.mode;
|
||||
cmd.gaitType = msg.gaitType;
|
||||
cmd.speedLevel = msg.speedLevel;
|
||||
cmd.dFootRaiseHeight = msg.dFootRaiseHeight;
|
||||
cmd.dBodyHeight = msg.dBodyHeight;
|
||||
|
||||
for (std::size_t i(0); i < 2; ++i)
|
||||
{
|
||||
ros_msg.quaternion[i] = state.quaternion[i];
|
||||
cmd.position[i] = msg.position[i];
|
||||
cmd.velocity[i] = msg.velocity[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 (std::size_t i(0); i < 3; ++i)
|
||||
cmd.rpy[i] = msg.rpy[i];
|
||||
|
||||
ros_msg.temperature = state.temperature;
|
||||
cmd.yawSpeed = msg.yawSpeed;
|
||||
|
||||
for (std::size_t i(0); i < 4; ++i)
|
||||
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
|
||||
|
||||
|
||||
return ros_msg;
|
||||
for (std::size_t i(0); i < 40; ++i)
|
||||
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
|
||||
|
||||
cmd.reserve = msg.reserve;
|
||||
cmd.crc = msg.crc;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
|
||||
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
|
||||
{
|
||||
unitree_legged_msgs::BmsState ros_msg;
|
||||
unitree_legged_msgs::HighState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
ros_msg.levelFlag = state.levelFlag;
|
||||
ros_msg.commVersion = state.commVersion;
|
||||
ros_msg.robotID = state.robotID;
|
||||
ros_msg.SN = state.SN;
|
||||
ros_msg.bandWidth = state.bandWidth;
|
||||
ros_msg.mode = state.mode;
|
||||
ros_msg.imu = state2rosMsg(state.imu);
|
||||
|
||||
for(std::size_t i(0); i < 3; ++i)
|
||||
{
|
||||
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
|
||||
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
|
||||
ros_msg.position[i] = state.position[i];
|
||||
ros_msg.velocity[i] = state.velocity[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 10; i++)
|
||||
ros_msg.yawSpeed = state.yawSpeed;
|
||||
|
||||
for(std::size_t i(0); i < 4; ++i)
|
||||
{
|
||||
ros_msg.cell_vol[i] = state.cell_vol[i];
|
||||
ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
|
||||
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
}
|
||||
|
||||
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 (std::size_t i(0); i < 40; ++i)
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
|
@ -201,134 +179,69 @@ 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.commVersion = state.commVersion;
|
||||
ros_msg.robotID = state.robotID;
|
||||
ros_msg.SN = state.SN;
|
||||
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.imu = state2rosMsg(state.imu);
|
||||
|
||||
for (std::size_t i(0); i < 20; ++i)
|
||||
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
|
||||
|
||||
for (std::size_t i(0); i < 4; ++i)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
}
|
||||
|
||||
ros_msg.tick = state.tick;
|
||||
|
||||
for (std::size_t i(0); i < 40; ++i)
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg)
|
||||
|
||||
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
UNITREE_LEGGED_SDK::LowCmd cmd;
|
||||
|
||||
cmd.head[0] = 0xFE;
|
||||
cmd.head[1] = 0xEF;
|
||||
cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL;
|
||||
cmd.mode = 0;
|
||||
cmd.gaitType = 0;
|
||||
cmd.speedLevel = 0;
|
||||
cmd.footRaiseHeight = 0;
|
||||
cmd.bodyHeight = 0;
|
||||
cmd.euler[0] = 0;
|
||||
cmd.euler[1] = 0;
|
||||
cmd.euler[2] = 0;
|
||||
cmd.velocity[0] = 0.0f;
|
||||
cmd.velocity[1] = 0.0f;
|
||||
cmd.yawSpeed = 0.0f;
|
||||
cmd.reserve = 0;
|
||||
cmd.levelFlag = msg.levelFlag;
|
||||
cmd.commVersion = msg.commVersion;
|
||||
cmd.robotID = msg.robotID;
|
||||
cmd.SN = msg.SN;
|
||||
cmd.bandWidth = msg.bandWidth;
|
||||
|
||||
cmd.velocity[0] = msg->linear.x;
|
||||
cmd.velocity[1] = msg->linear.y;
|
||||
cmd.yawSpeed = msg->angular.z;
|
||||
for (std::size_t i(0); i < 20; ++i)
|
||||
cmd.motorCmd[i] = rosMsg2Cmd(msg.motorCmd[i]);
|
||||
|
||||
|
||||
cmd.mode = 2;
|
||||
cmd.gaitType = 1;
|
||||
for(std::size_t i(0); i < 4; ++i)
|
||||
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
|
||||
|
||||
for (std::size_t i(0); i < 40; ++i)
|
||||
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
|
||||
|
||||
cmd.reserve = msg.reserve;
|
||||
cmd.crc = msg.crc;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
|
|
|
|||
|
|
@ -1,8 +0,0 @@
|
|||
<launch>
|
||||
|
||||
|
||||
<node pkg="unitree_legged_real" type="twist_sub" name="node_twist_sub" output="screen"/>
|
||||
<node pkg="unitree_legged_real" type="control_via_keyboard" name="node_control_via_keyboard" output="screen"/>
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,111 +0,0 @@
|
|||
#include "ros/ros.h"
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <termios.h>
|
||||
|
||||
int getch()
|
||||
{
|
||||
int ch;
|
||||
struct termios oldt;
|
||||
struct termios newt;
|
||||
|
||||
// Store old settings, and copy to new settings
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
|
||||
// Make required changes and apply the settings
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
newt.c_iflag |= IGNBRK;
|
||||
newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
|
||||
newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
|
||||
newt.c_cc[VMIN] = 0;
|
||||
newt.c_cc[VTIME] = 1;
|
||||
tcsetattr(fileno(stdin), TCSANOW, &newt);
|
||||
|
||||
// Get the current character
|
||||
ch = getchar();
|
||||
|
||||
// Reapply old settings
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
|
||||
return ch;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "keyboard_input_node");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
||||
|
||||
geometry_msgs::Twist twist;
|
||||
|
||||
long count = 0;
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
twist.linear.x = 0.0;
|
||||
twist.linear.y = 0.0;
|
||||
twist.linear.z = 0.0;
|
||||
twist.angular.x = 0.0;
|
||||
twist.angular.y = 0.0;
|
||||
twist.angular.z = 0.0;
|
||||
|
||||
int ch = 0;
|
||||
|
||||
ch = getch();
|
||||
|
||||
printf("%ld\n", count++);
|
||||
printf("ch = %d\n\n", ch);
|
||||
|
||||
switch (ch)
|
||||
{
|
||||
case 'q':
|
||||
printf("already quit!\n");
|
||||
return 0;
|
||||
|
||||
case 'w':
|
||||
twist.linear.x = 0.5;
|
||||
printf("move forward!\n");
|
||||
break;
|
||||
|
||||
case 's':
|
||||
twist.linear.x = -0.5;
|
||||
printf("move backward!\n");
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
twist.linear.y = 0.5;
|
||||
printf("move left!\n");
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
twist.linear.y = -0.5;
|
||||
printf("move right!\n");
|
||||
break;
|
||||
|
||||
case 'j':
|
||||
twist.angular.z = 1.0;
|
||||
printf("turn left!\n");
|
||||
break;
|
||||
|
||||
case 'l':
|
||||
twist.angular.z = -1.0;
|
||||
printf("turn right!\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Stop!\n");
|
||||
break;
|
||||
}
|
||||
|
||||
pub.publish(twist);
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -6,6 +6,26 @@
|
|||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
unitree_legged_msgs::LowState low_state_ros;
|
||||
|
||||
void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &state)
|
||||
{
|
||||
static long count = 0;
|
||||
ROS_INFO("lowStateCallback %ld", count++);
|
||||
|
||||
low_state_ros = *state;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_postition_without_lcm");
|
||||
|
|
@ -18,27 +38,28 @@ int main(int argc, char **argv)
|
|||
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};
|
||||
double time_consume = 0;
|
||||
int rate_count = 0;
|
||||
int sin_count = 0;
|
||||
int motiontime = 0;
|
||||
float dt = 0.002;
|
||||
|
||||
unitree_legged_msgs::LowCmd low_cmd_ros;
|
||||
|
||||
bool initiated_flag = false; // initiate need time
|
||||
int count = 0;
|
||||
// bool initiated_flag = false; // initiate need time
|
||||
// int count = 0;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::LowCmd>("low_cmd", 1);
|
||||
ros::Subscriber sub = nh.subscribe("low_state", 1, lowStateCallback);
|
||||
|
||||
low_cmd_ros.head[0] = 0xFE;
|
||||
low_cmd_ros.head[1] = 0xEF;
|
||||
low_cmd_ros.levelFlag = LOWLEVEL;
|
||||
|
||||
for (int i = 0; i < 12; i++)
|
||||
for (std::size_t i = 0; i < 20; ++i)
|
||||
{
|
||||
low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环
|
||||
|
|
@ -51,37 +72,80 @@ int main(int argc, char **argv)
|
|||
while (ros::ok())
|
||||
{
|
||||
|
||||
if (initiated_flag == true)
|
||||
printf("FR_joint_pos: %f %f %f\n", low_state_ros.motorState[FR_0].q, low_state_ros.motorState[FR_1].q, low_state_ros.motorState[FR_2].q);
|
||||
|
||||
|
||||
motiontime += 2;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_0].tau = -1.6f;
|
||||
|
||||
if (motiontime >= 0)
|
||||
{
|
||||
motiontime += 2;
|
||||
// first, get record initial position
|
||||
// if( motiontime >= 100 && motiontime < 500){
|
||||
if (motiontime >= 0 && motiontime < 10)
|
||||
{
|
||||
qInit[0] = low_state_ros.motorState[FR_0].q;
|
||||
qInit[1] = low_state_ros.motorState[FR_1].q;
|
||||
qInit[2] = low_state_ros.motorState[FR_2].q;
|
||||
}
|
||||
// second, move to the origin point of a sine movement with Kp Kd
|
||||
// if( motiontime >= 500 && motiontime < 1500){
|
||||
if (motiontime >= 10 && motiontime < 400)
|
||||
{
|
||||
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;
|
||||
Kp[0] = 20.0;
|
||||
Kp[1] = 20.0;
|
||||
Kp[2] = 20.0;
|
||||
Kd[0] = 2.0;
|
||||
Kd[1] = 2.0;
|
||||
Kd[2] = 2.0;
|
||||
|
||||
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;
|
||||
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
|
||||
float freq_Hz = 1;
|
||||
// float freq_Hz = 5;
|
||||
float freq_rad = freq_Hz * 2 * M_PI;
|
||||
float t = dt * sin_count;
|
||||
if (motiontime >= 400)
|
||||
{
|
||||
sin_count++;
|
||||
// sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
|
||||
// sin_joint2 = -0.9 * sin(3*M_PI*sin_count/1000.0);
|
||||
sin_joint1 = 0.6 * sin(t * freq_rad);
|
||||
sin_joint2 = -0.9 * sin(t * freq_rad);
|
||||
qDes[0] = sin_mid_q[0];
|
||||
qDes[1] = sin_mid_q[1] + sin_joint1;
|
||||
qDes[2] = sin_mid_q[2] + sin_joint2;
|
||||
// qDes[2] = sin_mid_q[2];
|
||||
}
|
||||
|
||||
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 = qDes[0];
|
||||
low_cmd_ros.motorCmd[FR_0].dq = 0;
|
||||
low_cmd_ros.motorCmd[FR_0].Kp = Kp[0];
|
||||
low_cmd_ros.motorCmd[FR_0].Kd = Kd[0];
|
||||
low_cmd_ros.motorCmd[FR_0].tau = -1.6f;
|
||||
|
||||
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 = qDes[1];
|
||||
low_cmd_ros.motorCmd[FR_1].dq = 0;
|
||||
low_cmd_ros.motorCmd[FR_1].Kp = Kp[1];
|
||||
low_cmd_ros.motorCmd[FR_1].Kd = Kd[1];
|
||||
low_cmd_ros.motorCmd[FR_1].tau = 0.0f;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
count++;
|
||||
if (count > 10)
|
||||
{
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
low_cmd_ros.motorCmd[FR_2].q = qDes[2];
|
||||
low_cmd_ros.motorCmd[FR_2].dq = 0;
|
||||
low_cmd_ros.motorCmd[FR_2].Kp = Kp[2];
|
||||
low_cmd_ros.motorCmd[FR_2].Kd = Kd[2];
|
||||
low_cmd_ros.motorCmd[FR_2].tau = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
pub.publish(low_cmd_ros);
|
||||
|
||||
|
|
@ -90,4 +154,4 @@ int main(int argc, char **argv)
|
|||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,6 +6,15 @@
|
|||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &state)
|
||||
{
|
||||
static long count = 0;
|
||||
ROS_INFO("highStateCallback %ld", count++);
|
||||
high_state_ros = *state;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_walk_without_lcm");
|
||||
|
|
@ -24,111 +33,176 @@ int main(int argc, char **argv)
|
|||
unitree_legged_msgs::HighCmd high_cmd_ros;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
|
||||
ros::Subscriber sub = nh.subscribe("high_state", 1000, highStateCallback);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
std::cout << "motiontime:\t" << motiontime << " " << high_state_ros.imu.rpy[2] << "\n";
|
||||
|
||||
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.position[0] = 0.0f;
|
||||
high_cmd_ros.position[1] = 0.0f;
|
||||
high_cmd_ros.yawSpeed = 0.0f;
|
||||
high_cmd_ros.reserve = 0;
|
||||
|
||||
if (motiontime > 0 && motiontime < 1000)
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.rpy[0] = 0;
|
||||
high_cmd_ros.rpy[1] = 0;
|
||||
high_cmd_ros.rpy[2] = 0;
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.dBodyHeight = 0;
|
||||
high_cmd_ros.dFootRaiseHeight = 0;
|
||||
|
||||
|
||||
if (motiontime == 2)
|
||||
{
|
||||
std::cout<<"begin sending commands."<<std::endl;
|
||||
}
|
||||
if (motiontime>1000 && motiontime <2000)
|
||||
{
|
||||
high_cmd_ros.mode = 1; // mode 1: force stand, control robot orientation and height
|
||||
high_cmd_ros.rpy[0] = 0.7;
|
||||
}
|
||||
if (motiontime>2000 && motiontime <3000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = -0.3;
|
||||
high_cmd_ros.rpy[0] = 0.;
|
||||
}
|
||||
if (motiontime > 1000 && motiontime < 2000)
|
||||
if (motiontime>3000 && motiontime <4000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = 0.3;
|
||||
high_cmd_ros.rpy[1] = 0.4;
|
||||
}
|
||||
if (motiontime > 2000 && motiontime < 3000)
|
||||
if (motiontime>4000 && motiontime <5000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = -0.2;
|
||||
high_cmd_ros.rpy[1] = 0.;
|
||||
}
|
||||
if (motiontime > 3000 && motiontime < 4000)
|
||||
if (motiontime>5000 && motiontime <6000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = 0.2;
|
||||
high_cmd_ros.rpy[2] = 0.5;
|
||||
}
|
||||
if (motiontime > 4000 && motiontime < 5000)
|
||||
if (motiontime>6000 && motiontime <7000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = -0.2;
|
||||
high_cmd_ros.rpy[2] = 0;
|
||||
}
|
||||
if (motiontime > 5000 && motiontime < 6000)
|
||||
if (motiontime>8000 && motiontime <9000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = 0.2;
|
||||
high_cmd_ros.dBodyHeight = 0.05;
|
||||
}
|
||||
if (motiontime > 6000 && motiontime < 7000)
|
||||
if (motiontime>9000 && motiontime <10000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = -0.2;
|
||||
high_cmd_ros.dBodyHeight = -0.15;
|
||||
}
|
||||
if (motiontime > 7000 && motiontime < 8000)
|
||||
if (motiontime>10000 && motiontime <11000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.1;
|
||||
high_cmd_ros.dBodyHeight = 0.;
|
||||
}
|
||||
if (motiontime > 8000 && motiontime < 9000)
|
||||
if (motiontime>11000 && motiontime <13000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.0;
|
||||
high_cmd_ros.mode = 2; // mode 2: following tareget velocity in body frame
|
||||
high_cmd_ros.gaitType = 0; // trot gait
|
||||
high_cmd_ros.velocity[0] = 0.2;
|
||||
high_cmd_ros.velocity[1] = 0;
|
||||
high_cmd_ros.yawSpeed = 0.3;
|
||||
}
|
||||
if (motiontime > 9000 && motiontime < 11000)
|
||||
if (motiontime>13000 && motiontime <15000)
|
||||
{
|
||||
high_cmd_ros.mode = 5;
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.velocity[0] = -0.2;
|
||||
high_cmd_ros.velocity[1] = 0;
|
||||
high_cmd_ros.yawSpeed = -0.1;
|
||||
high_cmd_ros.dFootRaiseHeight = 0.1; // increase swing height
|
||||
}
|
||||
if (motiontime > 11000 && motiontime < 13000)
|
||||
if (motiontime> 15000 && motiontime <17000)
|
||||
{
|
||||
high_cmd_ros.mode = 6;
|
||||
high_cmd_ros.mode = 0; // normal trot of gaitType 0 will stop automatically
|
||||
}
|
||||
if (motiontime > 13000 && motiontime < 14000)
|
||||
if (motiontime>17000 && motiontime <23000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.mode = 3; // mode 3: walk to target position in ground frame
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.speedLevel = 0; // adjust speedlevel
|
||||
high_cmd_ros.position[0] = 1;
|
||||
high_cmd_ros.position[1] = 0;
|
||||
high_cmd_ros.rpy[2] = 0;
|
||||
high_cmd_ros.dBodyHeight = -0.05; // lower body height
|
||||
}
|
||||
if (motiontime > 14000 && motiontime < 18000)
|
||||
if (motiontime> 23000 && motiontime <25000)
|
||||
{
|
||||
high_cmd_ros.mode = 0; // normal trot of gaitType 0 will stop automatically
|
||||
}
|
||||
if (motiontime>25000 && motiontime <32000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 2; // stairs walking gait
|
||||
high_cmd_ros.velocity[0] = 0.1; // avoid using higher forward speed than 0.37m/s for stair climbing gait
|
||||
high_cmd_ros.velocity[1] = 0.;
|
||||
high_cmd_ros.yawSpeed = 0;
|
||||
}
|
||||
if (motiontime>32000 && motiontime <40000)
|
||||
{
|
||||
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");
|
||||
high_cmd_ros.velocity[0] = -0.1; // avoid using higher backward speed than -0.2m/s for stair climbing gait
|
||||
high_cmd_ros.velocity[1] = 0.;
|
||||
high_cmd_ros.yawSpeed = 0;
|
||||
}
|
||||
if (motiontime > 18000 && motiontime < 20000)
|
||||
if (motiontime>40000 && motiontime <42000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.velocity[0] = 0;
|
||||
high_cmd_ros.mode = 1; // stairs walking gait will not automatically stop. change mode to force stand.
|
||||
}
|
||||
if (motiontime > 20000 && motiontime < 24000)
|
||||
|
||||
if (motiontime>42000 && motiontime <44000)
|
||||
{
|
||||
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");
|
||||
high_cmd_ros.mode = 5; // stand down
|
||||
}
|
||||
if (motiontime > 24000)
|
||||
if (motiontime>44000 && motiontime <46000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.mode = 6; // stand up
|
||||
}
|
||||
if (motiontime>46000 && motiontime <48000)
|
||||
{
|
||||
high_cmd_ros.mode = 5; // stand down
|
||||
}
|
||||
if (motiontime>48000 && motiontime <51000)
|
||||
{
|
||||
high_cmd_ros.mode = 7; // damping mode
|
||||
}
|
||||
if (motiontime>51000 && motiontime <53000)
|
||||
{
|
||||
high_cmd_ros.mode = 5; // stand down
|
||||
}
|
||||
if (motiontime>53000 && motiontime <55000)
|
||||
{
|
||||
high_cmd_ros.mode = 6; // stand up
|
||||
}
|
||||
if (motiontime>55000 && motiontime <57000)
|
||||
{
|
||||
high_cmd_ros.mode = 1; // force stand
|
||||
}
|
||||
if (motiontime>57000 && motiontime <65000)
|
||||
{
|
||||
high_cmd_ros.mode = 3; // mode 3: walk to target position in ground frame
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.speedLevel = 0; // adjust speedlevel
|
||||
high_cmd_ros.position[0] = 0;
|
||||
high_cmd_ros.position[1] = 0;
|
||||
high_cmd_ros.rpy[2] = 0;
|
||||
high_cmd_ros.dBodyHeight = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
pub.publish(high_cmd_ros);
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,10 @@
|
|||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
const int LOW_CMD_LENGTH = 610;
|
||||
const int LOW_STATE_LENGTH = 771;
|
||||
|
||||
class Custom
|
||||
{
|
||||
public:
|
||||
|
|
@ -26,8 +30,8 @@ public:
|
|||
Custom()
|
||||
:
|
||||
// low_udp(LOWLEVEL),
|
||||
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
|
||||
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
low_udp(8082, "192.168.123.10", 8007, LOW_CMD_LENGTH, LOW_STATE_LENGTH),
|
||||
high_udp(8081, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
|
|
@ -77,25 +81,25 @@ long low_count = 0;
|
|||
|
||||
void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
printf("highCmdCallback is running !\t%ld\n", ::high_count);
|
||||
|
||||
custom.high_cmd = rosMsg2Cmd(msg);
|
||||
printf("highCmdCallback is running !\t%ld\n", ::high_count++);
|
||||
|
||||
custom.high_cmd = rosMsg2Cmd(*msg);
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
high_state_ros = state2rosMsg(custom.high_state);
|
||||
|
||||
pub_high.publish(high_state_ros);
|
||||
|
||||
printf("highCmdCallback ending !\t%ld\n\n", ::high_count++);
|
||||
// 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);
|
||||
printf("lowCmdCallback is running !\t%ld\n", ::low_count++);
|
||||
|
||||
custom.low_cmd = rosMsg2Cmd(msg);
|
||||
custom.low_cmd = rosMsg2Cmd(*msg);
|
||||
|
||||
unitree_legged_msgs::LowState low_state_ros;
|
||||
|
||||
|
|
@ -103,7 +107,7 @@ void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
|||
|
||||
pub_low.publish(low_state_ros);
|
||||
|
||||
printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
|
||||
// printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
|
|
@ -123,6 +127,8 @@ int main(int argc, char **argv)
|
|||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
printf("LOWLEVEL is initialized\n");
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("low level runing!\n");
|
||||
|
|
@ -138,6 +144,8 @@ int main(int argc, char **argv)
|
|||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
printf("HIGHLEVEL is initialized\n");
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("high level runing!\n");
|
||||
|
|
|
|||
|
|
@ -1,39 +0,0 @@
|
|||
#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,112 +0,0 @@
|
|||
#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>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
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),
|
||||
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
|
||||
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
Custom custom;
|
||||
|
||||
ros::Subscriber sub_cmd_vel;
|
||||
ros::Publisher pub_high;
|
||||
|
||||
long cmd_vel_count = 0;
|
||||
|
||||
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg)
|
||||
{
|
||||
printf("cmdVelCallback is running!\t%ld\n", cmd_vel_count);
|
||||
|
||||
custom.high_cmd = rosMsg2Cmd(msg);
|
||||
|
||||
printf("cmd_x_vel = %f\n", custom.high_cmd.velocity[0]);
|
||||
printf("cmd_y_vel = %f\n", custom.high_cmd.velocity[1]);
|
||||
printf("cmd_yaw_vel = %f\n", custom.high_cmd.yawSpeed);
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
high_state_ros = state2rosMsg(custom.high_state);
|
||||
|
||||
pub_high.publish(high_state_ros);
|
||||
|
||||
printf("cmdVelCallback ending!\t%ld\n\n", cmd_vel_count++);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "twist_sub");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
|
||||
|
||||
sub_cmd_vel = nh.subscribe("cmd_vel", 1, cmdVelCallback);
|
||||
|
||||
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();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue