EmbodiedAI
/
z1_controller
mirror of https://github.com/unitreerobotics/z1_controller.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

first commit

This commit is contained in:
David Zhai 2022-07-20 11:11:38 +08:00
commit e64a878cdf
74 changed files with 12626 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
CMakeLists.txt Normal file
View File

@ -0,0 +1,144 @@
cmake_minimum_required(VERSION 3.0)
project(z1_controller)
add_definitions(-std=c++14)
set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3")
# set(GRIPPER MASS3KG) # 3kg
# set(GRIPPER NONE) # No Gripper
set(GRIPPER UNITREE_GRIPPER) # The gripper made by unitree
set(COMMUNICATION UDP) # UDP
# set(COMMUNICATION ROS) # ROS
# set(CTRL_PANEL KEYBOARD) # control by keyboard
set(CTRL_PANEL SDK) # control by SDK
# set(CTRL_PANEL JOYSTICK) # control by joystick
if(${COMMUNICATION} STREQUAL "UDP")
add_definitions(-DUDP)
set(SIMULATION OFF)
set(REAL_ROBOT ON)
elseif(${COMMUNICATION} STREQUAL "ROS")
add_definitions(-DROS)
set(SIMULATION ON)
set(REAL_ROBOT OFF)
else()
message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
endif()
if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
endif()
if(REAL_ROBOT)
add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
endif()
if(SIMULATION)
add_definitions(-DCOMPILE_WITH_SIMULATION)
add_definitions(-DRUN_ROS)
find_package(gazebo REQUIRED)
endif()
if(DEBUG)
add_definitions(-DCOMPILE_DEBUG)
find_package(Python2 COMPONENTS Interpreter Development NumPy)
endif()
if(${GRIPPER} STREQUAL "NONE")
add_definitions(-DNO_GRIPPER)
elseif(${GRIPPER} STREQUAL "AG95")
add_definitions(-DAG95)
elseif(${GRIPPER} STREQUAL "MASS3KG")
add_definitions(-DMASS3KG)
elseif(${GRIPPER} STREQUAL "UNITREE_GRIPPER")
add_definitions(-DUNITREE_GRIPPER)
else()
message(FATAL_ERROR "[CMake ERROR] The GRIPPER is error")
endif()
if(${CTRL_PANEL} STREQUAL "KEYBOARD")
add_definitions(-DCTRL_BY_KEYBOARD)
elseif(${CTRL_PANEL} STREQUAL "JOYSTICK")
add_definitions(-DCTRL_BY_JOYSTICK)
elseif(${CTRL_PANEL} STREQUAL "SDK")
add_definitions(-DCTRL_BY_SDK)
else()
message(FATAL_ERROR "[CMake ERROR] The CTRL_PANEL is error")
endif()
if(SIMULATION)
find_package(catkin REQUIRED COMPONENTS
controller_manager
genmsg
joint_state_controller
robot_state_publisher
roscpp
gazebo_ros
std_msgs
tf
geometry_msgs
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(
${GAZEBO_LIBRARY_DIRS}
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
endif()
set(EIGEN_PATH /usr/include/eigen3)
include_directories(
include
thirdparty
../z1_sdk/include
${Boost_INCLUDE_DIR}
${EIGEN_PATH}
)
link_directories(lib)
# # add_library
# file(GLOB_RECURSE ADD_SRC_LIST
# src/*/*.cpp
# src/*/*/*.cpp
# thirdparty/*/src/*.cpp
# thirdparty/*/src/*.cc
# ../z1_sdk/src/*.cpp
# )
# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) # SHARED:CMAKE_LIBRARY_OUTPUT_DIRECTORY STATIC:CMAKE_ARCHIVE_OUTPUT_DIRECTORY
# add_library(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 SHARED ${ADD_SRC_LIST})
# target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${catkin_LIBRARIES} rbdl -pthread)
# if(SIMULATION)
# add_dependencies(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# endif()
# add_executable and target_link_libraries
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build) # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
add_executable(z1_ctrl main.cpp)
target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${CTRL_PANEL}_${COMMUNICATION}_Linux64.so)
# debug
if(DEBUG)
find_package(PythonLibs REQUIRED)
include_directories(${PYTHON_INCLUDE_DIRS})
target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${PYTHON_LIBRARIES})
endif()
# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)

223
README.md Normal file
View File

@ -0,0 +1,223 @@
# v3.6.1
The z1_controller is mainly used to control the robot arm and communicate with the Z1 SDK
## Notice
support robot: Z1(v3.6.1)
not support robot: Z1(v3.4, v3.5)
## Dependencies
- [ROS](https://www.ros.org/)（Melodic）
- build-essential
```bash
sudo apt install build-essential
```
- Boost (version 1.5.4 or higher)
```bash
dpkg -S /usr/include/boost/version.hpp # check boost version
sudo apt install libboost-dev # install boost
```
- CMake (version 2.8.3 or higher)
```bash
cmake --version # check cmake version
sudo apt install cmake # install cmake
```
- [Eigen](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) (version 3.3.9 or higher)
```bash
cd eigen-3.3.9
mkdir build && cd build
cmake ..
sudo make install
sudo ln -s /usr/local/include/eigen3 /usr/include/eigen3
sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen
```
- [RBDL](https://github.com/rbdl/rbdl/releases/tag/v2.6.0) (version 2.6.0)
```bash
cd rbdl-2.6.0
mkdir build && cd build
cmake -D CMAKE_BUILD_TYPE=Release ..
sudo make install
sudo sh -c "echo '/usr/local/lib' >> /etc/ld.so.conf"
sudo ldconfig
```
## Build
```bash
# config /<path to>/z1_controller/CMakeList.txt
cd /<path to>/z1_controller && mkdir build && cd build
cmake ..
make -j4
```
## Usage
The default IP of the robot is 192.168.123.110, you need to change the IP of your PC before using the SDK so that your PC can ping to the robot.
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, enpxxx.
```bash
sudo ifconfig enpxxx down # enpxxx is your PC port
sudo ifconfig enpxxx 192.168.123.162/24
sudo ifconfig enpxxx up
ping 192.168.123.110
```
Then run program.
```
cd /<path to>/z1_controller/build
sudo ./z1_ctrl
```
- FSM(finite-state machine)
| State | Keyswitch | Switchable |
| ----------- | --------- | --------------------------- |
| BACKTOSTART | ～ | 1 2 |
| PASSIVE | 1 | ~ 2 3 = |
| JOINTCTRL | 2 | ~ 1 3 4 5 6 7 8 9 0 - |
| CARTESIAN | 3 | ~ 1 2 4 5 6 9 |
| MoveJ | 4 | ~ 1 2 3 5 6 9 |
| MoveL | 5 | ~ 1 2 3 4 6 9 |
| MOVEC | 6 | ~ 1 2 3 4 5 9 |
| TEACH | 7 | ~ 1 2 |
| TEACHREPEAT | 8 | automatically switches to 2 |
| SAVESTATE | 9 | automatically switches to 2 |
| TOSTATE | 0 | automatically switches to 2 |
| TRAJECTORY | - | ~ 1 2 |
| CALIBRATION | = | automatically switches to 2 |
| NEXT | ] | next state |
#### FSM Details
- Key ~ (BACKTOSTART) : All motors return to initial positions
- Key1（PASSIVE）: All motor enter passive state(The state of the Z1 startup)
- Key2（ JOINTCTRL）
| Joint ID | 0 | 1 | 2 | 3 | 4 | 5 | Gripper |
| :------------------------------ | :---------------------- | :---------------------- | :---------------------- | :---------------------- | :---------------------- | :---------------------- | ----------------------- |
| Keyboard | Q/A | W/ S | D/ E | R/F | T/G | Y/H | up/down |
| Joint Action <br />(right hand) | positive<br />/negative | positive<br />/negative | positive<br />/negative | positive<br />/negative | positive<br />/negative | positive<br />/negative | positive<br />/negative |
- Key3（CARTESIAN）: The reference coordinate system is cartesian
| Keyboard | Q/A | W/S | E/D | R/F | T/G | Y/H |
| ------------ | ---------------------- | ---------------- | ------------- | ----------------------- | ------------------------ | ---------------------- |
| Key Function | forward<br />/backward | right<br />/left | up<br />/down | roll <br />(right hand) | pitch<br /> (right hand) | yaw<br /> (right hand) |
- Key4（MoveJ）:
```
Key4—— Enter the desired end pose（roll pitch yaw x y z）——The Z1 joint rotate to the joint target point（After Z1 arrived the target point, it automatically switches to the joint space control state）
```
- Key5（MoveL）:
```
Key5——Enter the desired end pose（roll pitch yaw x y z）——The Z1 follows the generated straight trajectory to the target point（After Z1 arrived the target point, it automatically switches to the joint space control state）
```
- Key6（MoveC）:
```
Key6——Enter the desired middle and end pose（roll pitch yaw x y z）——The Z1 follows the generated arc trajectory to the target point（After Z1 arrived the target point, it automatically switches to the joint space control state）
```
- Key7（TEACH）:
```
Key7——Enter the teaching trajectory label —— Drag Z1 —— Press Key2 to complete teaching.
```
- Key8（TEACHREPEAT）:
```
Key8———— Enter the saved teaching trajectory label—— Z1 repeate the teaching trajectory
```
- Key9（SAVESTATE）:
```
Key9——Enter the current pose label —— Z1 automatically switches to the joint space control state
```
- Key0（TOSTATE）:
```
Key0——Enter the pose label to save（After Z1 arrived the target point, it automatically switches to the joint space control state）
```
- Key -（TRAJECTORY）:
```
Key - —— Z1 repeats in a written trajectory
```
- Key=（CALIBRATION)；
```
Key= —— Set the current position as the initial position, and enter the joint space control state after setting
```
- Key ] （NEXT）: used to debug joystick contrl
```
Key]—— Enter next state
```
## SDK
If you want to develop your own control methods of Z1, you can refer to the SDK(z1_sdk).
We have written an example of keyboard control based on SDK, you can use it fllowing the steps below.
### State change
- First, set(CTRL_PANEL SDK) # z1_ws/src/z1_controller/CMakeList.txt，and then rebuild the z1_ws to generate z1_ctrl, then open a teminal to run z1_ctrl
```
cd /<path to>/z1_controller/build
sudo ./z1_ctrl
```
- Sencond, build the z1_sdk, and then open another terminal to run example.
```
cd /<path to>/z1_sdk && mkdir build && cd build
cmake ..
make -j4
./example_state_send
```
### Low level control
```
sudo ./z1_ctrl # Running in a terminal
./example_lowCmd_send # Running in another terminal
```
### Keyboard control
```
sudo ./z1_ctrl # Running in a terminal
./example_keyboard_send # Running in another terminal
```
### State control
```
sudo ./z1_ctrl # Running in a terminal
./example_keyboard_send # Running in another terminal
```

59
config/savedArmStates.csv Executable file
View File

@ -0,0 +1,59 @@
forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.424746, 0.002468, 0.999923, 0.012173, 0.000358, 0.026329, -0.012234, 0.999578, 0.394252, 0.000000, 0.000000, 0.000000, 1.000000,
start, -0.000336, 0.001634, 0.008171, 0.064640, 0.000248, 0.000230, 0.997230, 0.000106, 0.074376, 0.085128, -0.000087, 1.000000, -0.000255, 0.000008, -0.074376, 0.000248, 0.997230, 0.133277, 0.000000, 0.000000, 0.000000, 1.000000,
highRiseLeft, -0.756000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, 0.403000, 0.633000, 0.661000, -0.102000, 0.476000, -0.762000, 0.440000, 0.178000, 0.782000, 0.137000, -0.608000, 0.610000, 0.000000, 0.000000, 0.000000, 1.000000,
lowPickLeft, 0.590000, 2.088000, -0.899000, 0.591000, 0.906000, -0.641000, -0.558000, -0.521000, 0.646000, 0.221000, 0.155000, 0.699000, 0.698000, 0.198000, -0.816000, 0.489000, -0.309000, 0.071000, 0.000000, 0.000000, 0.000000, 1.000000,
startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000,
videoHighRise, 0.804000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, -0.471646, 0.768269, -0.432796, -0.179591, 0.408109, 0.625271, 0.665195, -0.100048, 0.781663, 0.137109, -0.608444, 0.610103, 0.000000, 0.000000, 0.000000, 1.000000,
videoToLeft, 2.316499, 2.223266, -1.131086, 0.590578, 0.905720, -0.640975, -0.121092, -0.577184, -0.807586, -0.278129, -0.514682, -0.659161, 0.548278, 0.239010, -0.848787, 0.482042, -0.217247, 0.058040, 0.000000, 0.000000, 0.000000, 1.000000,
video_02, -0.042000, 1.416000, -1.484000, 0.000000, -0.279000, 0.000000, 0.939568, 0.041988, -0.339778, 0.322181, -0.039485, 0.999118, 0.014279, -0.013540, 0.340078, 0.000000, 0.940397, 0.545628, 0.000000, 0.000000, 0.000000, 1.000000,
video_03, 0.918960, 2.171685, -1.471256, 0.000000, 0.427000, 0.000000, 0.260242, -0.794971, 0.547992, 0.300209, 0.341030, 0.606647, 0.718107, 0.393404, -0.903312, 0.000000, 0.428984, 0.149885, 0.000000, 0.000000, 0.000000, 1.000000,
video_01, -0.871858, 0.870714, -0.722365, 0.055117, -0.678396, -0.070888, 0.528563, 0.793043, -0.302826, 0.095419, -0.682618, 0.609129, 0.403725, -0.118699, 0.504632, -0.006679, 0.863309, 0.420252, 0.000000, 0.000000, 0.000000, 1.000000,
test, 0.368525, 2.393267, -1.148771, -0.953744, 0.368296, 0.368360, 0.704081, -0.517416, 0.486365, 0.522431, 0.657831, 0.733192, -0.172300, 0.259328, -0.267449, 0.441259, 0.856599, 0.093448, 0.000000, 0.000000, 0.000000, 1.000000,
repeatLow, 0.000000, 1.931603, -0.835792, 0.000000, 0.390233, 0.000000, 0.084651, 0.000000, 0.996411, 0.311108, 0.000000, 1.000000, 0.000000, 0.000000, -0.996411, 0.000000, 0.084651, 0.092545, 0.000000, 0.000000, 0.000000, 1.000000,
repeatHigh, 0.000000, 1.940186, -1.486322, 0.000000, 0.391440, 0.000000, 0.663504, 0.000000, 0.748173, 0.474034, 0.000000, 1.000000, 0.000000, 0.000000, -0.748173, 0.000000, 0.663504, 0.269367, 0.000000, 0.000000, 0.000000, 1.000000,
winestart, -0.550922, 2.147376, -0.743038, -0.055447, -1.430589, 1.647373, 0.879196, -0.054860, -0.473292, 0.332236, -0.475751, -0.046881, -0.878330, -0.197922, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000,
wineget, -0.609784, 2.218792, -0.937567, -0.117771, -1.309136, 1.671756, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000,
wine1, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.639763, 0.995012, -0.045873, -0.088587, 0.410574, 0.006629, 0.916446, -0.400104, -0.113175, 0.099539, 0.397521, 0.912178, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000,
wine2, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.319763, 0.995012, -0.015678, -0.098520, 0.410574, 0.006629, 0.995782, -0.091510, -0.113175, 0.099539, 0.090400, 0.990919, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000,
wine3, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 1.829763, 0.995012, -0.099290, 0.009662, 0.410574, 0.006629, -0.030838, -0.999502, -0.113175, 0.099539, 0.994581, -0.030026, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000,
wine4, -0.609784, 2.103729, -0.954345, -0.123154, -1.178247, 1.688475, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.155627, 0.000000, 0.000000, 0.000000, 1.000000,
wine2_5, -0.270810, 1.913207, -0.952264, -0.313249, -1.085456, 0.511294, 0.995012, -0.038831, -0.091892, 0.410574, 0.006629, 0.944836, -0.327477, -0.086775, 0.099539, 0.325234, 0.940380, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000,
doorstart, 0.431042, 1.563797, -1.529086, 1.322867, -0.534746, -1.320230, 0.991589, 0.086371, -0.096392, 0.358317, -0.087764, 0.996088, -0.010297, 0.112472, 0.095125, 0.018670, 0.995290, 0.496540, 0.000000, 0.000000, 0.000000, 1.000000,
doorstartflat, 0.520059, 1.879836, -1.920901, 1.443620, -0.682288, -1.471548, 0.980922, 0.157107, -0.114496, 0.436491, -0.159056, 0.987237, -0.008031, 0.180614, 0.111773, 0.026089, 0.993391, 0.503541, 0.000000, 0.000000, 0.000000, 1.000000,
doorpre, 0.431042, 1.475972, -1.314848, 1.466425, -0.591235, -1.494022, 0.984623, 0.149591, 0.090224, 0.332506, -0.157354, 0.983780, 0.086116, 0.094216, -0.075878, -0.098989, 0.992191, 0.441392, 0.000000, 0.000000, 0.000000, 1.000000,
doorstartflat2, 1.085467, 1.374296, -1.381275, 1.454993, -1.117510, -1.514257, 0.993818, 0.028773, -0.107233, 0.198932, -0.029880, 0.999515, -0.008726, 0.193041, 0.106930, 0.011876, 0.994196, 0.503075, 0.000000, 0.000000, 0.000000, 1.000000,
doorpre2, 1.277563, 1.233555, -1.023053, 1.407057, -1.338097, -1.314977, 0.993818, 0.028774, -0.107232, 0.147332, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.426275, 0.000000, 0.000000, 0.000000, 1.000000,
doorstart2, 0.894370, 1.364286, -1.338585, 1.417931, -0.932419, -1.446631, 0.993818, 0.028774, -0.107232, 0.233132, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.492875, 0.000000, 0.000000, 0.000000, 1.000000,
swardhit01, 0.046000, 1.293000, -1.235000, 0.000000, -0.616000, 0.000000, 0.847418, -0.045984, -0.528931, 0.278086, 0.039009, 0.998942, -0.024348, 0.012801, 0.529491, 0.000000, 0.848316, 0.518108, 0.000000, 0.000000, 0.000000, 1.000000,
swardmiddle01, 0.824000, 1.151000, -0.931000, 0.000000, -0.604000, 0.000000, 0.629821, -0.733869, -0.254484, 0.163849, 0.680424, 0.679291, -0.274931, 0.177013, 0.374632, 0.000000, 0.927174, 0.438554, 0.000000, 0.000000, 0.000000, 1.000000,
swardstart01, 0.000000, 0.861000, -0.455000, 0.000000, -0.518000, 0.000000, 0.993735, 0.000000, -0.111766, 0.154853, 0.000000, 1.000000, 0.000000, 0.000000, 0.111766, 0.000000, 0.993735, 0.304846, 0.000000, 0.000000, 0.000000, 1.000000,
swardpush01, -0.246000, 1.701000, -1.361000, 1.042000, -0.442000, 0.000000, 0.806358, 0.402127, -0.433683, 0.403978, -0.583253, 0.419186, -0.695773, -0.138065, -0.097995, 0.813988, 0.572555, 0.385327, 0.000000, 0.000000, 0.000000, 1.000000,
swardhit02, -0.060000, 2.293000, -2.333000, 0.056000, -0.052000, 0.000000, 0.993808, 0.057636, -0.094994, 0.613902, -0.062615, 0.996770, -0.050289, -0.037159, 0.091789, 0.055926, 0.994207, 0.430516, 0.000000, 0.000000, 0.000000, 1.000000,
swardthroat, 0.018000, 1.842000, -2.130000, -1.372000, -1.373000, 1.128000, 0.116093, -0.744644, -0.657288, 0.368208, 0.963437, 0.245315, -0.107751, 0.099110, 0.241479, -0.620746, 0.745897, 0.588202, 0.000000, 0.000000, 0.000000, 1.000000,
swardswing, 0.000000, 1.947000, -0.899000, -1.022000, -0.948000, 0.000000, 0.658370, -0.739194, -0.141916, 0.382308, 0.692974, 0.521661, 0.497652, 0.066664, -0.293829, -0.425983, 0.855688, 0.167033, 0.000000, 0.000000, 0.000000, 1.000000,
swardstart, 0.000000, 1.695335, -1.003223, -0.000000, -0.980112, 0.000000, 0.958814, 0.000000, -0.284035, 0.392642, 0.000000, 1.000000, 0.000000, 0.000000, 0.284035, 0.000000, 0.958814, 0.324838, 0.000000, 0.000000, 0.000000, 1.000000,
swardmiddle02, -0.009336, 1.762476, -0.547022, 0.071902, -1.570413, -0.026650, 0.934527, 0.085894, -0.345372, 0.306754, -0.080568, 0.996304, 0.029776, -0.009775, 0.346653, -0.000001, 0.937993, 0.218133, 0.000000, 0.000000, 0.000000, 1.000000,
swardend, -0.009336, 0.640476, -0.385022, 0.071902, -0.550413, -0.026650, 0.956080, 0.035230, -0.290981, 0.105389, -0.046503, 0.998408, -0.031915, -0.004599, 0.289393, 0.044045, 0.956196, 0.309459, 0.000000, 0.000000, 0.000000, 1.000000,
swardwalk, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000,
nokiaend, -0.106199, 2.620994, -1.475180, 0.022797, -0.839897, -1.203857, 0.946225, -0.232455, 0.224997, 0.559486, -0.117937, 0.399763, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, -0.002630, 0.000000, 0.000000, 0.000000, 1.000000,
nokiamiddle, -0.136319, 2.083721, -1.059621, -0.017853, -0.718157, -1.184266, 0.946225, -0.232455, 0.224996, 0.455086, -0.117937, 0.399764, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, 0.150371, 0.000000, 0.000000, 0.000000, 1.000000,
nokiastart, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000,
teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000,
djistart, 0.000000, 1.616000, -0.324000, 0.000000, -1.285000, 0.000000, 0.999976, 0.000000, 0.007000, 0.242349, 0.000000, 1.000000, 0.000000, 0.000000, -0.007000, 0.000000, 0.999976, 0.179422, 0.000000, 0.000000, 0.000000, 1.000000,
anyway, 0.242441, 0.267866, -0.016100, -0.008059, -0.122301, 0.005914, 0.962394, -0.241280, 0.124828, 0.050463, 0.239018, 0.970454, 0.033019, 0.012577, -0.129107, -0.001941, 0.991629, 0.153768, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000,
fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000,
5kg01, -0.582000, 2.072006, -0.858000, 0.000000, 0.241000, 0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.003142, 0.000000, 0.000000, 0.000000, 1.000000,
5kg02, -0.582000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000,
5kg03, 0.524000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000,
5kg04, 0.524000, 2.008825, -0.874538, 0.000000, 0.320718, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.025942, 0.000000, 0.000000, 0.000000, 1.000000,
hold_light, -0.000024, 1.098000, -1.618000, 0.000000, 0.340994, 0.000000, 0.984021, 0.000024, -0.178051, 0.161036, -0.000023, 1.000000, 0.000004, -0.000004, 0.178051, 0.000000, 0.984021, 0.613488, 0.000000, 0.000000, 0.000000, 1.000000,
give_light, -0.000024, 2.396000, -2.156000, 0.000000, 0.340994, 0.000000, 0.835917, 0.000024, 0.548855, 0.631917, -0.000020, 1.000000, -0.000013, -0.000015, -0.548855, 0.000000, 0.835917, 0.261657, 0.000000, 0.000000, 0.000000, 1.000000,
dance_swing_right, 0.987976, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.546488, -0.834914, -0.065344, 0.034540, 0.829009, 0.550381, -0.099125, 0.052396, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000,
dance_swing_left, -0.970024, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.561282, 0.824899, -0.067113, 0.035475, -0.819065, 0.565280, 0.097936, -0.051768, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000,
001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000,
002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000,
003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000,
starFlat, -0.003451, 0.000501, 0.009250, -0.064964, 0.000525, 0.000590, 0.998472, 0.002895, -0.055186, 0.085395, -0.002921, 0.999996, -0.000400, -0.000217, 0.055184, 0.000560, 0.998476, 0.161033, 0.000000, 0.000000, 0.000000, 1.000000,
yy, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000,
1 forward -0.000019 1.542859 -1.037883 -0.531308 0.002487 -0.012173 0.999650 -0.002146 -0.026357 0.424746 0.002468 0.999923 0.012173 0.000358 0.026329 -0.012234 0.999578 0.394252 0.000000 0.000000 0.000000 1.000000
2 start -0.000336 0.001634 0.008171 0.064640 0.000248 0.000230 0.997230 0.000106 0.074376 0.085128 -0.000087 1.000000 -0.000255 0.000008 -0.074376 0.000248 0.997230 0.133277 0.000000 0.000000 0.000000 1.000000
3 highRiseLeft -0.756000 0.500000 -1.496000 -0.942000 -0.879000 -1.004000 0.403000 0.633000 0.661000 -0.102000 0.476000 -0.762000 0.440000 0.178000 0.782000 0.137000 -0.608000 0.610000 0.000000 0.000000 0.000000 1.000000
4 lowPickLeft 0.590000 2.088000 -0.899000 0.591000 0.906000 -0.641000 -0.558000 -0.521000 0.646000 0.221000 0.155000 0.699000 0.698000 0.198000 -0.816000 0.489000 -0.309000 0.071000 0.000000 0.000000 0.000000 1.000000
5 startFlat 0.000000 0.000000 -0.000012 -0.074351 0.000000 0.000006 0.997236 -0.000000 -0.074294 0.086594 0.000000 1.000000 -0.000006 0.000000 0.074294 0.000006 0.997236 0.176788 0.000000 0.000000 0.000000 1.000000
6 videoHighRise 0.804000 0.500000 -1.496000 -0.942000 -0.879000 -1.004000 -0.471646 0.768269 -0.432796 -0.179591 0.408109 0.625271 0.665195 -0.100048 0.781663 0.137109 -0.608444 0.610103 0.000000 0.000000 0.000000 1.000000
7 videoToLeft 2.316499 2.223266 -1.131086 0.590578 0.905720 -0.640975 -0.121092 -0.577184 -0.807586 -0.278129 -0.514682 -0.659161 0.548278 0.239010 -0.848787 0.482042 -0.217247 0.058040 0.000000 0.000000 0.000000 1.000000
8 video_02 -0.042000 1.416000 -1.484000 0.000000 -0.279000 0.000000 0.939568 0.041988 -0.339778 0.322181 -0.039485 0.999118 0.014279 -0.013540 0.340078 0.000000 0.940397 0.545628 0.000000 0.000000 0.000000 1.000000
9 video_03 0.918960 2.171685 -1.471256 0.000000 0.427000 0.000000 0.260242 -0.794971 0.547992 0.300209 0.341030 0.606647 0.718107 0.393404 -0.903312 0.000000 0.428984 0.149885 0.000000 0.000000 0.000000 1.000000
10 video_01 -0.871858 0.870714 -0.722365 0.055117 -0.678396 -0.070888 0.528563 0.793043 -0.302826 0.095419 -0.682618 0.609129 0.403725 -0.118699 0.504632 -0.006679 0.863309 0.420252 0.000000 0.000000 0.000000 1.000000
11 test 0.368525 2.393267 -1.148771 -0.953744 0.368296 0.368360 0.704081 -0.517416 0.486365 0.522431 0.657831 0.733192 -0.172300 0.259328 -0.267449 0.441259 0.856599 0.093448 0.000000 0.000000 0.000000 1.000000
12 repeatLow 0.000000 1.931603 -0.835792 0.000000 0.390233 0.000000 0.084651 0.000000 0.996411 0.311108 0.000000 1.000000 0.000000 0.000000 -0.996411 0.000000 0.084651 0.092545 0.000000 0.000000 0.000000 1.000000
13 repeatHigh 0.000000 1.940186 -1.486322 0.000000 0.391440 0.000000 0.663504 0.000000 0.748173 0.474034 0.000000 1.000000 0.000000 0.000000 -0.748173 0.000000 0.663504 0.269367 0.000000 0.000000 0.000000 1.000000
14 winestart -0.550922 2.147376 -0.743038 -0.055447 -1.430589 1.647373 0.879196 -0.054860 -0.473292 0.332236 -0.475751 -0.046881 -0.878330 -0.197922 0.025997 0.997393 -0.067317 0.113427 0.000000 0.000000 0.000000 1.000000
15 wineget -0.609784 2.218792 -0.937567 -0.117771 -1.309136 1.671756 0.879196 -0.054860 -0.473292 0.366936 -0.475751 -0.046881 -0.878330 -0.243022 0.025997 0.997393 -0.067317 0.113427 0.000000 0.000000 0.000000 1.000000
16 wine1 -0.346864 1.931170 -0.980517 -0.398801 -1.091111 0.639763 0.995012 -0.045873 -0.088587 0.410574 0.006629 0.916446 -0.400104 -0.113175 0.099539 0.397521 0.912178 0.226441 0.000000 0.000000 0.000000 1.000000
17 wine2 -0.346864 1.931170 -0.980517 -0.398801 -1.091111 0.319763 0.995012 -0.015678 -0.098520 0.410574 0.006629 0.995782 -0.091510 -0.113175 0.099539 0.090400 0.990919 0.226441 0.000000 0.000000 0.000000 1.000000
18 wine3 -0.346864 1.931170 -0.980517 -0.398801 -1.091111 1.829763 0.995012 -0.099290 0.009662 0.410574 0.006629 -0.030838 -0.999502 -0.113175 0.099539 0.994581 -0.030026 0.226441 0.000000 0.000000 0.000000 1.000000
19 wine4 -0.609784 2.103729 -0.954345 -0.123154 -1.178247 1.688475 0.879196 -0.054860 -0.473292 0.366936 -0.475751 -0.046881 -0.878330 -0.243022 0.025997 0.997393 -0.067317 0.155627 0.000000 0.000000 0.000000 1.000000
20 wine2_5 -0.270810 1.913207 -0.952264 -0.313249 -1.085456 0.511294 0.995012 -0.038831 -0.091892 0.410574 0.006629 0.944836 -0.327477 -0.086775 0.099539 0.325234 0.940380 0.226441 0.000000 0.000000 0.000000 1.000000
21 doorstart 0.431042 1.563797 -1.529086 1.322867 -0.534746 -1.320230 0.991589 0.086371 -0.096392 0.358317 -0.087764 0.996088 -0.010297 0.112472 0.095125 0.018670 0.995290 0.496540 0.000000 0.000000 0.000000 1.000000
22 doorstartflat 0.520059 1.879836 -1.920901 1.443620 -0.682288 -1.471548 0.980922 0.157107 -0.114496 0.436491 -0.159056 0.987237 -0.008031 0.180614 0.111773 0.026089 0.993391 0.503541 0.000000 0.000000 0.000000 1.000000
23 doorpre 0.431042 1.475972 -1.314848 1.466425 -0.591235 -1.494022 0.984623 0.149591 0.090224 0.332506 -0.157354 0.983780 0.086116 0.094216 -0.075878 -0.098989 0.992191 0.441392 0.000000 0.000000 0.000000 1.000000
24 doorstartflat2 1.085467 1.374296 -1.381275 1.454993 -1.117510 -1.514257 0.993818 0.028773 -0.107233 0.198932 -0.029880 0.999515 -0.008726 0.193041 0.106930 0.011876 0.994196 0.503075 0.000000 0.000000 0.000000 1.000000
25 doorpre2 1.277563 1.233555 -1.023053 1.407057 -1.338097 -1.314977 0.993818 0.028774 -0.107232 0.147332 -0.029881 0.999515 -0.008725 0.168441 0.106929 0.011875 0.994196 0.426275 0.000000 0.000000 0.000000 1.000000
26 doorstart2 0.894370 1.364286 -1.338585 1.417931 -0.932419 -1.446631 0.993818 0.028774 -0.107232 0.233132 -0.029881 0.999515 -0.008725 0.168441 0.106929 0.011875 0.994196 0.492875 0.000000 0.000000 0.000000 1.000000
27 swardhit01 0.046000 1.293000 -1.235000 0.000000 -0.616000 0.000000 0.847418 -0.045984 -0.528931 0.278086 0.039009 0.998942 -0.024348 0.012801 0.529491 0.000000 0.848316 0.518108 0.000000 0.000000 0.000000 1.000000
28 swardmiddle01 0.824000 1.151000 -0.931000 0.000000 -0.604000 0.000000 0.629821 -0.733869 -0.254484 0.163849 0.680424 0.679291 -0.274931 0.177013 0.374632 0.000000 0.927174 0.438554 0.000000 0.000000 0.000000 1.000000
29 swardstart01 0.000000 0.861000 -0.455000 0.000000 -0.518000 0.000000 0.993735 0.000000 -0.111766 0.154853 0.000000 1.000000 0.000000 0.000000 0.111766 0.000000 0.993735 0.304846 0.000000 0.000000 0.000000 1.000000
30 swardpush01 -0.246000 1.701000 -1.361000 1.042000 -0.442000 0.000000 0.806358 0.402127 -0.433683 0.403978 -0.583253 0.419186 -0.695773 -0.138065 -0.097995 0.813988 0.572555 0.385327 0.000000 0.000000 0.000000 1.000000
31 swardhit02 -0.060000 2.293000 -2.333000 0.056000 -0.052000 0.000000 0.993808 0.057636 -0.094994 0.613902 -0.062615 0.996770 -0.050289 -0.037159 0.091789 0.055926 0.994207 0.430516 0.000000 0.000000 0.000000 1.000000
32 swardthroat 0.018000 1.842000 -2.130000 -1.372000 -1.373000 1.128000 0.116093 -0.744644 -0.657288 0.368208 0.963437 0.245315 -0.107751 0.099110 0.241479 -0.620746 0.745897 0.588202 0.000000 0.000000 0.000000 1.000000
33 swardswing 0.000000 1.947000 -0.899000 -1.022000 -0.948000 0.000000 0.658370 -0.739194 -0.141916 0.382308 0.692974 0.521661 0.497652 0.066664 -0.293829 -0.425983 0.855688 0.167033 0.000000 0.000000 0.000000 1.000000
34 swardstart 0.000000 1.695335 -1.003223 -0.000000 -0.980112 0.000000 0.958814 0.000000 -0.284035 0.392642 0.000000 1.000000 0.000000 0.000000 0.284035 0.000000 0.958814 0.324838 0.000000 0.000000 0.000000 1.000000
35 swardmiddle02 -0.009336 1.762476 -0.547022 0.071902 -1.570413 -0.026650 0.934527 0.085894 -0.345372 0.306754 -0.080568 0.996304 0.029776 -0.009775 0.346653 -0.000001 0.937993 0.218133 0.000000 0.000000 0.000000 1.000000
36 swardend -0.009336 0.640476 -0.385022 0.071902 -0.550413 -0.026650 0.956080 0.035230 -0.290981 0.105389 -0.046503 0.998408 -0.031915 -0.004599 0.289393 0.044045 0.956196 0.309459 0.000000 0.000000 0.000000 1.000000
37 swardwalk 0.000000 0.861000 -0.455000 0.000000 -1.068000 0.000000 0.788764 0.000000 -0.614696 0.135135 0.000000 1.000000 0.000000 0.000000 0.614696 0.000000 0.788764 0.353228 0.000000 0.000000 0.000000 1.000000
38 nokiaend -0.106199 2.620994 -1.475180 0.022797 -0.839897 -1.203857 0.946225 -0.232455 0.224997 0.559486 -0.117937 0.399763 0.908999 -0.061283 -0.301247 -0.886653 0.350851 -0.002630 0.000000 0.000000 0.000000 1.000000
39 nokiamiddle -0.136319 2.083721 -1.059621 -0.017853 -0.718157 -1.184266 0.946225 -0.232455 0.224996 0.455086 -0.117937 0.399764 0.908999 -0.061283 -0.301247 -0.886653 0.350851 0.150371 0.000000 0.000000 0.000000 1.000000
40 nokiastart 0.000000 0.861000 -0.455000 0.000000 -1.068000 0.000000 0.788764 0.000000 -0.614696 0.135135 0.000000 1.000000 0.000000 0.000000 0.614696 0.000000 0.788764 0.353228 0.000000 0.000000 0.000000 1.000000
41 teachstart -0.858000 1.951000 -0.825000 0.000000 -1.154000 0.000000 0.653696 0.756536 -0.018308 0.260474 -0.756240 0.653952 0.021180 -0.301334 0.027996 0.000000 0.999608 0.183326 0.000000 0.000000 0.000000 1.000000
42 djistart 0.000000 1.616000 -0.324000 0.000000 -1.285000 0.000000 0.999976 0.000000 0.007000 0.242349 0.000000 1.000000 0.000000 0.000000 -0.007000 0.000000 0.999976 0.179422 0.000000 0.000000 0.000000 1.000000
43 anyway 0.242441 0.267866 -0.016100 -0.008059 -0.122301 0.005914 0.962394 -0.241280 0.124828 0.050463 0.239018 0.970454 0.033019 0.012577 -0.129107 -0.001941 0.991629 0.153768 0.000000 0.000000 0.000000 1.000000
44 fulltest00 2.316194 0.013239 -0.001959 -0.000047 -0.278982 0.221496 -0.654115 -0.677458 0.336427 -0.022776 0.708636 -0.704403 -0.040645 0.024673 0.264516 0.211818 0.940832 0.174303 0.000000 0.000000 0.000000 1.000000
45 fulltest01 0.000194 1.621239 -1.455959 2.470727 1.492796 0.419496 0.205219 0.489994 0.847225 0.332273 0.619814 -0.735004 0.274956 0.059687 0.757440 0.468696 -0.454542 0.521459 0.000000 0.000000 0.000000 1.000000
46 fulltest02 -2.336194 3.117239 -3.099593 -2.478727 -1.516796 0.419496 0.415340 -0.220546 0.882526 -0.404129 -0.454627 0.789988 0.411380 -0.505948 -0.787913 -0.572083 0.227847 0.075097 0.000000 0.000000 0.000000 1.000000
47 5kg01 -0.582000 2.072006 -0.858000 0.000000 0.241000 0.000000 0.096511 0.549696 0.829771 0.280359 -0.063507 0.835365 -0.546015 -0.184485 -0.993304 0.000000 0.115532 0.003142 0.000000 0.000000 0.000000 1.000000
48 5kg02 -0.582000 1.854695 -0.950446 -0.000000 0.550757 -0.000000 0.096511 0.549696 0.829771 0.280359 -0.063507 0.835365 -0.546015 -0.184485 -0.993304 0.000000 0.115532 0.090142 0.000000 0.000000 0.000000 1.000000
49 5kg03 0.524000 1.854695 -0.950446 -0.000000 0.550757 -0.000000 0.100030 -0.500347 0.860027 0.290582 0.057806 0.865825 0.496997 0.167923 -0.993304 0.000000 0.115532 0.090142 0.000000 0.000000 0.000000 1.000000
50 5kg04 0.524000 2.008825 -0.874538 0.000000 0.320718 -0.000000 0.100030 -0.500347 0.860027 0.290582 0.057806 0.865825 0.496997 0.167923 -0.993304 0.000000 0.115532 0.025942 0.000000 0.000000 0.000000 1.000000
51 hold_light -0.000024 1.098000 -1.618000 0.000000 0.340994 0.000000 0.984021 0.000024 -0.178051 0.161036 -0.000023 1.000000 0.000004 -0.000004 0.178051 0.000000 0.984021 0.613488 0.000000 0.000000 0.000000 1.000000
52 give_light -0.000024 2.396000 -2.156000 0.000000 0.340994 0.000000 0.835917 0.000024 0.548855 0.631917 -0.000020 1.000000 -0.000013 -0.000015 -0.548855 0.000000 0.835917 0.261657 0.000000 0.000000 0.000000 1.000000
53 dance_swing_right 0.987976 0.692000 -1.152000 0.000000 0.340994 0.000000 0.546488 -0.834914 -0.065344 0.034540 0.829009 0.550381 -0.099125 0.052396 0.118725 0.000000 0.992927 0.505625 0.000000 0.000000 0.000000 1.000000
54 dance_swing_left -0.970024 0.692000 -1.152000 0.000000 0.340994 0.000000 0.561282 0.824899 -0.067113 0.035475 -0.819065 0.565280 0.097936 -0.051768 0.118725 0.000000 0.992927 0.505625 0.000000 0.000000 0.000000 1.000000
55 001 0.000000 0.922204 -1.215402 0.293198 0.000000 0.000000 1.000000 0.000000 -0.000000 0.200000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.500000 0.000000 0.000000 0.000000 1.000000
56 002 -1.035000 0.922204 -1.017402 -0.285802 0.000000 0.000000 0.473918 0.859862 -0.189839 0.103971 -0.798204 0.510526 0.319739 -0.175114 0.371849 0.000000 0.928293 0.541400 0.000000 0.000000 0.000000 1.000000
57 003 0.954000 0.910204 -0.984402 -0.192802 0.000000 0.000000 0.557929 -0.815736 -0.152611 0.121384 0.786832 0.578425 -0.215223 0.171185 0.263839 0.000000 0.964567 0.510707 0.000000 0.000000 0.000000 1.000000
58 starFlat -0.003451 0.000501 0.009250 -0.064964 0.000525 0.000590 0.998472 0.002895 -0.055186 0.085395 -0.002921 0.999996 -0.000400 -0.000217 0.055184 0.000560 0.998476 0.161033 0.000000 0.000000 0.000000 1.000000
59 yy 0.000000 0.922204 -1.215402 0.293198 0.000000 0.000000 1.000000 0.000000 -0.000000 0.200000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.500000 0.000000 0.000000 0.000000 1.000000

30
include/FSM/BaseState.h Executable file
View File

@ -0,0 +1,30 @@
#ifndef BASESTATE_H
#define BASESTATE_H
#include <string>
class BaseState{
public:
BaseState(int stateNameEnum, std::string stateNameString)
: _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){}
virtual ~BaseState(){};
virtual void enter() = 0;
virtual void run() = 0;
virtual void exit() = 0;
virtual int checkChange(int cmd) = 0;
bool isState(int stateEnum){
if(_stateNameEnum == stateEnum){
return true;
}else{
return false;
}
}
std::string getStateName(){return _stateNameString;}
protected:
int _stateNameEnum;
std::string _stateNameString;
};
#endif

77
include/FSM/FSMState.h Executable file
View File

@ -0,0 +1,77 @@
#ifndef FSMSTATE_H
#define FSMSTATE_H
#include <string>
#include <iostream>
#include <unistd.h>
#include "control/CtrlComponents.h"
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "common/enumClass.h"
#include "common/math/mathTools.h"
#include "common/math/mathTypes.h"
#include "unitree_arm_sdk/timer.h"
#include "model/ArmDynKineModel.h"
#include "FSM/FiniteStateMachine.h"
class FSMState : public BaseState{
public:
FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
virtual ~FSMState(){}
virtual void enter() = 0;
virtual void run() = 0;
virtual void exit() = 0;
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
protected:
void _armCtrl();
void _tauDynForward();
void _tauFriction();
void _qdFeedBack();
bool _collisionTest();
void _jointPosProtect();
void _jointSpeedProtect();
#ifdef UNITREE_GRIPPER
void _gripperCmd();
void _gripperCtrl();
double _gripperPos;
double _gripperW;
double _gripperTau;
double _gripperPosStep;
double _gripperTauStep;
double _gripperLinearFriction;
double _gripperCoulombFriction;
static double _gripperCoulombDirection;
double _gripperFriction;
#endif
CtrlComponents *_ctrlComp;
ArmFSMStateName _nextStateName;
IOInterface *_ioInter;
LowlevelCmd *_lowCmd;
LowlevelState *_lowState;
UserValue _userValue;
ArmDynKineModel *_armModel;
Vec6 _qPast, _qdPast, _q, _qd, _qdd;
Vec6 _tau, _endForce;
Vec6 _coulombFriction;
static Vec6 _coulombDirection;
Vec6 _linearFriction;
Vec6 _kpDiag, _kdDiag;
Mat6 _Kp, _Kd;
std::vector<double> _jointQMax;
std::vector<double> _jointQMin;
std::vector<double> _jointSpeedMax;
SendCmd _sendCmd;
RecvState _recvState;
};
#endif // FSMSTATE_H

42
include/FSM/FiniteStateMachine.h Executable file
View File

@ -0,0 +1,42 @@
#ifndef FSM_H
#define FSM_H
#include <vector>
#include "FSM/BaseState.h"
#include "unitree_arm_sdk/cmdPanel.h"
#include "unitree_arm_sdk/loop.h"
enum class FSMRunMode{
NORMAL,
CHANGE
};
/*
状态机初始状态为states的第一个元素
为了支持多个状态机从同一个CmdPanel获取触发信号，
加入了cmdChannel，不同状态机之间的cmdChannel必须不同，
否则可能会漏过状态触发
*/
class FiniteStateMachine{
public:
FiniteStateMachine(std::vector<BaseState*> states,
CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002);
virtual ~FiniteStateMachine();
private:
void _run();
static void* _staticRun(void* obj);
std::vector<BaseState*> _states;
FSMRunMode _mode;
bool _running;
BaseState* _currentState;
BaseState* _nextState;
int _nextStateEnum;
size_t _cmdChannel;
CmdPanel *_cmdPanel;
LoopFunc *_runThread;
};
#endif // FSM_H

13
include/FSM/State_BackToStart.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef STATE_BACKTOSTART_H
#define STATE_BACKTOSTART_H
#include "FSM/State_Trajectory.h"
class State_BackToStart : public State_Trajectory{
public:
State_BackToStart(CtrlComponents *ctrlComp);
private:
void _setTraj();
};
#endif // STATE_BACKTOSTART_H

18
include/FSM/State_Calibration.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef STATE_CALIBRATION_H
#define STATE_CALIBRATION_H
#include "FSM/FSMState.h"
class State_Calibration : public FSMState{
public:
State_Calibration(CtrlComponents *ctrlComp);
~State_Calibration(){}
void enter();
void run(){};
void exit(){};
int checkChange(int cmd);
private:
};
#endif // STATE_CALIBRATION_H

28
include/FSM/State_Cartesian.h Executable file
View File

@ -0,0 +1,28 @@
#ifndef CARTESIAN_H
#define CARTESIAN_H
#include "FSM/FSMState.h"
class State_Cartesian : public FSMState{
public:
State_Cartesian(CtrlComponents *ctrlComp);
~State_Cartesian(){}
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
HomoMat _endHomoGoal, _endHomoGoalPast;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
};
#endif // CARTESIAN_H

21
include/FSM/State_Dance.h Normal file
View File

@ -0,0 +1,21 @@
#ifndef STATE_DANCETRAJ_H
#define STATE_DANCETRAJ_H
#include "FSM/State_Trajectory.h"
class State_Dance : public State_Trajectory{
public:
State_Dance(CtrlComponents *ctrlComp,
TrajectoryManager *traj,
ArmFSMStateName stateEnum,
ArmFSMStateName nextState,
std::string stateString);
void exit();
int checkChange(int cmd);
private:
bool _freeBottom = false;
// ArmFSMStateName _nextState;
void _setTraj(){}
};
#endif

18
include/FSM/State_JointSpace.h Executable file
View File

@ -0,0 +1,18 @@
#ifndef JOINTSPACE_H
#define JOINTSPACE_H
#include "FSM/FSMState.h"
class State_JointSpace : public FSMState{
public:
State_JointSpace(CtrlComponents *ctrlComp);
~State_JointSpace(){}
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
double _angleStep;
};
#endif // JOINTSPACE_H

18
include/FSM/State_LowCmd.h Executable file
View File

@ -0,0 +1,18 @@
#ifndef LOWCMD_H
#define LOWCMD_H
#include "FSMState.h"
class State_LowCmd : public FSMState{
public:
State_LowCmd(CtrlComponents *ctrlComp);
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
std::vector<float> _kp;
std::vector<float> _kw;
};
#endif // LOWCMD_H

34
include/FSM/State_MoveC.h Executable file
View File

@ -0,0 +1,34 @@
#ifndef MOVEC_H
#define MOVEC_H
#include "FSM/FSMState.h"
#include "trajectory/CartesionSpaceTraj.h"
class State_MoveC : public FSMState{
public:
State_MoveC(CtrlComponents *ctrlComp);
~State_MoveC();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
Vec6 _pastPosture, _endPosture, _middlePostureGoal, _endPostureGoal, _endTwist;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
Vec6 _endPostureError;
// 轨迹相关变量
CartesionSpaceTraj *_cartesionTraj;
bool _reached, _timeReached, _taskReached, _pastTaskReached;
};
#endif // CARTESIAN_H

23
include/FSM/State_MoveJ.h Executable file
View File

@ -0,0 +1,23 @@
#ifndef MOVEJ_H
#define MOVEJ_H
#include "FSM/FSMState.h"
#include "trajectory/JointSpaceTraj.h"
class State_MoveJ : public FSMState{
public:
State_MoveJ(CtrlComponents *ctrlComp);
~State_MoveJ();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
double _costTime;
HomoMat _goalHomo;
JointSpaceTraj *_jointTraj;
bool _reached, _pastReached, _finalReached;
std::vector<Vec6> _result;
};
#endif // CARTESIAN_H

35
include/FSM/State_MoveL.h Executable file
View File

@ -0,0 +1,35 @@
#ifndef MOVEL_H
#define MOVEL_H
#include "FSM/FSMState.h"
#include "trajectory/CartesionSpaceTraj.h"
class State_MoveL : public FSMState{
public:
State_MoveL(CtrlComponents *ctrlComp);
~State_MoveL();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
Vec6 _pastPosture, _endPosture, _endTwist;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;
Vec6 _Pkp;
Vec6 _endPostureError;
// 轨迹相关变量
std::vector<std::vector<double> > _posture;
CartesionSpaceTraj *_cartesionTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H

15
include/FSM/State_Passive.h Executable file
View File

@ -0,0 +1,15 @@
#ifndef PASSIVE_H
#define PASSIVE_H
#include "FSMState.h"
class State_Passive : public FSMState{
public:
State_Passive(CtrlComponents *ctrlComp);
void enter();
void run();
void exit();
int checkChange(int cmd);
};
#endif // PASSIVE_H

21
include/FSM/State_SaveState.h Executable file
View File

@ -0,0 +1,21 @@
#ifndef SAVESTATE_H
#define SAVESTATE_H
#include "FSMState.h"
#include "common/utilities/CSVTool.h"
class State_SaveState : public FSMState{
public:
State_SaveState(CtrlComponents *ctrlComp);
~State_SaveState();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
CSVTool *_csv;
Vec6 _q;
HomoMat _endHomo;
};
#endif // SAVESTATE_H

25
include/FSM/State_Teach.h Executable file
View File

@ -0,0 +1,25 @@
#ifndef STATETEACH_H
#define STATETEACH_H
#include "FSM/FSMState.h"
#include "common/utilities/CSVTool.h"
class State_Teach : public FSMState{
public:
State_Teach(CtrlComponents *ctrlComp);
~State_Teach();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
CSVTool *_trajCSV;
size_t _stateID = 0;
Vec6 _KdDiag;
Vec6 _kpForStable;
Mat6 _Kd;
double _errorBias;
};
#endif // STATETEACH_H

27
include/FSM/State_TeachRepeat.h Executable file
View File

@ -0,0 +1,27 @@
#ifndef STATE_TEACHREPEAT_H
#define STATE_TEACHREPEAT_H
#include "FSM/FSMState.h"
#include "trajectory/TrajectoryManager.h"
#include "common/utilities/CSVTool.h"
class State_TeachRepeat : public FSMState{
public:
State_TeachRepeat(CtrlComponents *ctrlComp);
~State_TeachRepeat();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
JointSpaceTraj *_toStartTraj;
bool _reachedStart = false;
bool _finishedRepeat = false;
size_t _index = 0;
size_t _indexPast;
Vec6 _trajStartQ, _trajStartQd;
CSVTool *_csvFile;
};
#endif // STATE_TEACHREPEAT_H

24
include/FSM/State_ToState.h Executable file
View File

@ -0,0 +1,24 @@
#ifndef TOSTATE_H
#define TOSTATE_H
#include "FSM/FSMState.h"
#include "trajectory/JointSpaceTraj.h"
class State_ToState : public FSMState{
public:
State_ToState(CtrlComponents *ctrlComp);
~State_ToState();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
double _costTime;
HomoMat _goalHomo;
JointSpaceTraj *_jointTraj;
bool _reach, _pastReach;
std::string _goalName;
// CSVTool *_csv;
};
#endif // TOSTATE_H

36
include/FSM/State_Trajectory.h Executable file
View File

@ -0,0 +1,36 @@
#ifndef CARTESIANPATH_H
#define CARTESIANPATH_H
#include "FSM/FSMState.h"
#include "trajectory/EndHomoTraj.h"
#include "trajectory/TrajectoryManager.h"
class State_Trajectory : public FSMState{
public:
State_Trajectory(CtrlComponents *ctrlComp,
ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY,
std::string stateString = "trajectory");
~State_Trajectory();
void enter();
void run();
virtual void exit();
virtual int checkChange(int cmd);
protected:
// EndHomoTraj *_traj;
virtual void _setTraj();
TrajectoryManager *_traj;
HomoMat _goalHomo;
Vec6 _goalTwist;
JointSpaceTraj *_toStartTraj;
bool _reachedStart = false;
bool _finishedTraj = false;
std::vector<JointSpaceTraj*> _jointTraj;
std::vector<EndCircleTraj*> _circleTraj;
std::vector<StopForTime*> _stopTraj;
std::vector<EndLineTraj*> _lineTraj;
long long startTime;
};
#endif // CARTESIANPATH_H

97
include/common/enumClass.h Executable file
View File

@ -0,0 +1,97 @@
#ifndef ENUMCLASS_H
#define ENUMCLASS_H
// enum class UserCommand{
// // EXIT,
// NONE,
// START,
// L2_A, //used by robot
// L2_B, //used by robot
// L2_X, //used by robot
// R2_A, //Arm: joint space control
// R2_X, //Arm: Cartesian
// R2_Y, //
// R2_temp,
// ARM_PASSIVE, //Arm: passive
// ARM_CHECKJOINT_0,
// ARM_TRAJECTORY_0,
// ARM_SAVESTATE_0,
// ARM_TEACH_0,
// ARM_TEACHREPEAT_0,
// ARM_NEXT,
// ARM_BACKTOSTART,
// HEAD_JOINTCTRL,
// HEAD_TOSTART,
// HEAD_SAVE,
// HEAD_TURN,
// HEAD_CALI,
// HEAD_NEXT,
// HEAD_DANCE01,
// HEAD_DANCE02,
// HEAD_DANCE03,
// HEAD_DANCE04,
// HEAD_DANCE05,
// HEAD_DANCE06,
// HEAD_DANCE07
// };
enum class FrameType{
BODY,
HIP,
ENDEFFECTOR,
GLOBAL
};
enum class FSMMode{
NORMAL,
CHANGE
};
enum class ArmFSMStateName{
INVALID,
PASSIVE,
JOINTCTRL,
CARTESIAN,
MOVEJ,
MOVEL,
MOVEC,
TRAJECTORY,
TOSTATE,
SAVESTATE,
TEACH,
TEACHREPEAT,
CALIBRATION,
DANCE00,
DANCE01,
DANCE02,
DANCE03,
DANCE04,
DANCE05,
DANCE06,
DANCE07,
DANCE08,
DANCE09,
BACKTOSTART,
GRIPPER_OPEN,
GRIPPER_CLOSE,
NEXT,
LOWCMD
};
enum class JointMotorType{
SINGLE_MOTOR,
DOUBLE_MOTOR
};
enum class MotorMountType{
STATOR_FIRST,
OUTPUT_FIRST
};
enum class HeadType{
TIGER,
DOG
};
#endif // ENUMCLASS_H

482
include/common/math/mathTools.h Executable file
View File

@ -0,0 +1,482 @@
#ifndef MATHTOOLS_H
#define MATHTOOLS_H
#include <iostream>
// #include "common/mathTypes.h"
// template<typename T1, typename T2>
// inline T1 max(const T1 a, const T2 b){
// return (a > b ? a : b);
// }
// template<typename T1, typename T2>
// inline T1 min(const T1 a, const T2 b){
// return (a < b ? a : b);
// }
template<typename T>
T max(T value){
return value;
}
template<typename T, typename... Args>
inline T max(const T val0, const Args... restVal){
return val0 > (max<T>(restVal...)) ? val0 : max<T>(restVal...);
}
template<typename T>
T min(T value){
return value;
}
template<typename T, typename... Args>
inline T min(const T val0, const Args... restVal){
return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
}
inline Mat2 skew(const double& w){
Mat2 mat; mat.setZero();
mat(0, 1) = -w;
mat(1, 0) = w;
return mat;
}
inline Mat3 skew(const Vec3& v) {
Mat3 m;
m << 0, -v(2), v(1),
v(2), 0, -v(0),
-v(1), v(0), 0;
return m;
}
enum class TurnDirection{
NOMATTER,
POSITIVE,
NEGATIVE
};
/* first - second */
inline double angleError(double first, double second, TurnDirection direction = TurnDirection::NOMATTER){
double firstMod = fmod(first, 2.0*M_PI);
double secondMod = fmod(second, 2.0*M_PI);
if(direction == TurnDirection::NOMATTER){
if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){
return secondMod - firstMod;
}else{
return firstMod - secondMod;
}
}
else if(direction == TurnDirection::POSITIVE){
if(firstMod - secondMod < 0.0){
return 2*M_PI + firstMod - secondMod;
}else{
return firstMod - secondMod;
}
}
else if(direction == TurnDirection::NEGATIVE){
if(firstMod - secondMod > 0.0){
return -2*M_PI + firstMod - secondMod;
}else{
return firstMod - secondMod;
}
}
}
/* firstVec - secondVec */
inline VecX angleError(VecX firstVec, VecX secondVec, TurnDirection directionMatter = TurnDirection::NOMATTER){
if(firstVec.rows() != secondVec.rows()){
std::cout << "[ERROR] angleError, the sizes of firstVec and secondVec are different!" << std::endl;
}
VecX result = firstVec;
for(int i(0); i<firstVec.rows(); ++i){
result(i) = angleError(firstVec(i), secondVec(i), directionMatter);
}
return result;
}
inline bool vectorEqual(VecX v1, VecX v2, double tolerance){
if(v1.rows() != v2.rows()){
std::cout << "[WARNING] vectorEqual, the size of two vectors is not equal, v1 is "
<< v1.rows() << ", v2 is " << v2.rows() << std::endl;
return false;
}
for(int i(0); i<v1.rows(); ++i){
if(fabs(v1(i)-v2(i))>tolerance){
return false;
}
}
return true;
}
inline bool inInterval(double value, double limValue1, double limValue2, bool canEqual1 = false, bool canEqual2 = false){
double lowLim, highLim;
bool lowEqual, highEqual;
if(limValue1 >= limValue2){
highLim = limValue1;
highEqual = canEqual1;
lowLim = limValue2;
lowEqual = canEqual2;
}else{
lowLim = limValue1;
lowEqual = canEqual1;
highLim = limValue2;
highEqual = canEqual2;
}
if((value > highLim) || (value < lowLim)){
return false;
}
if((value == highLim) && !highEqual){
return false;
}
if((value == lowLim) && !lowEqual){
return false;
}
return true;
}
inline double saturation(const double a, double limValue1, double limValue2){
double lowLim, highLim;
if(limValue1 >= limValue2){
lowLim = limValue2;
highLim= limValue1;
}else{
lowLim = limValue1;
highLim= limValue2;
}
if(a < lowLim){
return lowLim;
}
else if(a > highLim){
return highLim;
}
else{
return a;
}
}
inline double saturation(const double a, Vec2 limits){
return saturation(a, limits(0), limits(1));
}
template<typename T0, typename T1>
inline T0 killZeroOffset(T0 a, const T1 limit){
if((a > -limit) && (a < limit)){
a = 0;
}
return a;
}
template<typename T0, typename T1, typename T2>
inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){
return (value-minLim)*(max-min)/(maxLim-minLim) + min;
}
// x: [0, 1], windowRatio: (0, 0.5)
template<typename T>
inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){
if((x < 0)||(x > xRange)){
// printf("[ERROR] The x of function windowFunc should between [0, xRange]\n");
std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl;
}
if((windowRatio <= 0)||(windowRatio >= 0.5)){
// printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n");
std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl;
}
if(x/xRange < windowRatio){
return x * yRange / (xRange * windowRatio);
}
else if(x/xRange > 1 - windowRatio){
return yRange * (xRange - x)/(xRange * windowRatio);
}
else{
return yRange;
}
}
template<typename T1, typename T2>
inline void updateAverage(T1 &exp, T2 newValue, double n){
if(exp.rows()!=newValue.rows()){
std::cout << "The size of updateAverage is error" << std::endl;
exit(-1);
}
if(fabs(n - 1) < 0.001){
exp = newValue;
}else{
exp = exp + (newValue - exp)/n;
}
}
template<typename T1, typename T2, typename T3>
inline void updateCovariance(T1 &cov, T2 expPast, T3 newValue, double n){
if( (cov.rows()!=cov.cols()) || (cov.rows() != expPast.rows()) || (expPast.rows()!=newValue.rows())){
std::cout << "The size of updateCovariance is error" << std::endl;
exit(-1);
}
if(fabs(n - 1) < 0.1){
cov.setZero();
}else{
cov = cov*(n-1)/n + (newValue-expPast)*(newValue-expPast).transpose()*(n-1)/(n*n);
}
}
template<typename T1, typename T2, typename T3>
inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){
// The order matters!!! covariance first!!!
updateCovariance(cov, exp, newValue, n);
updateAverage(exp, newValue, n);
}
/* rotate matrix about x axis */
inline RotMat rotX(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << 1, 0, 0, 0, c, -s, 0, s, c;
return R;
}
/* rotate matrix about y axis */
inline RotMat rotY(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, 0, s, 0, 1, 0, -s, 0, c;
return R;
}
/* rotate matrix about z axis */
inline RotMat rotZ(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, -s, 0, s, c, 0, 0, 0, 1;
return R;
}
inline RotMat so3ToRotMat(const Vec3& _rot){
RotMat R;
double theta = _rot.norm();
if (fabs(theta) <1e-6)
R = RotMat::Identity();
else{
Vec3 u_axis(_rot/theta);
double cos_theta = cos(theta);
R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose());
}
return R;
}
/* row pitch yaw to rotate matrix */
inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) {
RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row);
return m;
}
inline RotMat quatToRotMat(const Quat& q) {
double e0 = q(0);
double e1 = q(1);
double e2 = q(2);
double e3 = q(3);
RotMat R;
R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
1 - 2 * (e1 * e1 + e2 * e2);
return R;
}
inline Vec3 rotMatToRPY(const Mat3& R) {
Vec3 rpy;
rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2));
rpy(1) = asin(-R(2,0));
rpy(2) = atan2(R(1,0),R(0,0));
return rpy;
}
/* rotate matrix to exponential coordinate(omega*theta) */
inline Vec3 rotMatToExp(const RotMat& rm){
double cosValue = rm.trace()/2.0-1/2.0;
if(cosValue > 1.0f){
cosValue = 1.0f;
}else if(cosValue < -1.0f){
cosValue = -1.0f;
}
double angle = acos(cosValue);
Vec3 exp;
if (fabs(angle) < 1e-5){
exp=Vec3(0,0,0);
}
else if (fabs(angle - M_PI) < 1e-5){
exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0)));
}
else{
exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1));
}
return exp;
}
/* add 1 at the end of Vec3 */
inline Vec4 homoVec(Vec3 v3, double end = 1.0){
Vec4 v4;
v4.block(0, 0, 3, 1) = v3;
v4(3) = end;
return v4;
}
/* remove 1 at the end of Vec4 */
inline Vec3 noHomoVec(Vec4 v4){
Vec3 v3;
v3 = v4.block(0, 0, 3, 1);
return v3;
}
/* build Homogeneous Matrix by p and m */
inline HomoMat homoMatrix(Vec3 p, Mat3 m){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = m;
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by p and w */
inline HomoMat homoMatrix(Vec3 p, Vec3 w){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = so3ToRotMat(w);
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by x axis, y axis and p */
inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){
HomoMat homoM;
homoM.setZero();
Vec3 xN = x.normalized();
Vec3 yN = y.normalized();
homoM.block(0, 0, 3, 1) = xN;
homoM.block(0, 1, 3, 1) = yN;
homoM.block(0, 2, 3, 1) = skew(xN) * yN;
homoM.topRightCorner(3, 1) = p;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix of rotate joint */
inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){
HomoMat homoM;
Mat3 eye3 = Mat3::Identity();
Mat3 rotateM = so3ToRotMat(w);
homoM.setZero();
homoM.topLeftCorner(3, 3) = rotateM;
homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q;
// homoM.topRightCorner(3, 1) = q;
homoM(3, 3) = 1;
return homoM;
}
/* build Homogeneous Matrix by posture */
inline HomoMat homoMatrixPosture(Vec6 posture){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2));
homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5);
homoM(3, 3) = 1;
return homoM;
}
/* inverse matrix of homogeneous matrix */
inline HomoMat homoMatrixInverse(HomoMat homoM){
HomoMat homoInv;
homoInv.setZero();
homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose();
homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1);
homoInv(3, 3) = 1;
return homoInv;
}
/* get position of Homogeneous Matrix */
inline Vec3 getHomoPosition(HomoMat mat){
return mat.block(0, 3, 3, 1);
}
/* get rotate matrix of Homogeneous Matrix */
inline RotMat getHomoRotMat(HomoMat mat){
return mat.block(0, 0, 3, 3);
}
/* convert homogeneous matrix to posture vector */
inline Vec6 homoToPosture(HomoMat mat){
Vec6 posture;
posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat));
posture.block(3,0,3,1) = getHomoPosition(mat);
return posture;
}
/* convert posture vector matrix to homogeneous */
inline HomoMat postureToHomo(Vec6 posture){
HomoMat homoM;
homoM.setZero();
homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2));
homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5);
homoM(3, 3) = 1;
return homoM;
}
// Calculate average value and covariance
class AvgCov{
public:
AvgCov(unsigned int size, std::string name, bool avgOnly=false, unsigned int showPeriod=1000, unsigned int waitCount=5000, double zoomFactor=10000)
:_size(size), _showPeriod(showPeriod), _waitCount(waitCount), _zoomFactor(zoomFactor), _valueName(name), _avgOnly(avgOnly) {
_exp.resize(size);
_cov.resize(size, size);
_defaultWeight.resize(size, size);
_defaultWeight.setIdentity();
_measureCount = 0;
}
void measure(VecX newValue){
++_measureCount;
if(_measureCount > _waitCount){
updateAvgCov(_cov, _exp, newValue, _measureCount-_waitCount);
if(_measureCount % _showPeriod == 0){
// if(_measureCount < _waitCount + 5){
std::cout << "******" << _valueName << " measured count: " << _measureCount-_waitCount << "******" << std::endl;
// std::cout << _zoomFactor << " Times newValue of " << _valueName << std::endl << (_zoomFactor*newValue).transpose() << std::endl;
std::cout << _zoomFactor << " Times Average of " << _valueName << std::endl << (_zoomFactor*_exp).transpose() << std::endl;
if(!_avgOnly){
std::cout << _zoomFactor << " Times Covariance of " << _valueName << std::endl << _zoomFactor*_cov << std::endl;
}
}
}
}
private:
VecX _exp;
MatX _cov;
MatX _defaultWeight;
bool _avgOnly;
unsigned int _size;
unsigned int _measureCount;
unsigned int _showPeriod;
unsigned int _waitCount;
double _zoomFactor;
std::string _valueName;
};
#endif // MATHTOOLS_H

99
include/common/math/mathTypes.h Executable file
View File

@ -0,0 +1,99 @@
#ifndef MATHTYPES_H
#define MATHTYPES_H
#include <eigen3/Eigen/Dense>
/************************/
/******** Vector ********/
/************************/
// 2x1 Vector
using Vec2 = typename Eigen::Matrix<double, 2, 1>;
// 3x1 Vector
using Vec3 = typename Eigen::Matrix<double, 3, 1>;
// 4x1 Vector
using Vec4 = typename Eigen::Matrix<double, 4, 1>;
// 6x1 Vector
using Vec6 = typename Eigen::Matrix<double, 6, 1>;
// Quaternion
using Quat = typename Eigen::Matrix<double, 4, 1>;
// 4x1 Integer Vector
using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
// 12x1 Vector
using Vec12 = typename Eigen::Matrix<double, 12, 1>;
// 18x1 Vector
using Vec18 = typename Eigen::Matrix<double, 18, 1>;
// Dynamic Length Vector
using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
/************************/
/******** Matrix ********/
/************************/
// Rotation Matrix
using RotMat = typename Eigen::Matrix<double, 3, 3>;
// Homogenous Matrix
using HomoMat = typename Eigen::Matrix<double, 4, 4>;
// 2x2 Matrix
using Mat2 = typename Eigen::Matrix<double, 2, 2>;
// 3x3 Matrix
using Mat3 = typename Eigen::Matrix<double, 3, 3>;
// 3x4 Matrix, each column is a 3x1 vector
using Vec34 = typename Eigen::Matrix<double, 3, 4>;
// 6x6 Matrix
using Mat6 = typename Eigen::Matrix<double, 6, 6>;
// 12x12 Matrix
using Mat12 = typename Eigen::Matrix<double, 12, 12>;
// 3x3 Identity Matrix
#define I3 Eigen::MatrixXd::Identity(3, 3)
// 6x6 Identity Matrix
#define I6 Eigen::MatrixXd::Identity(6, 6)
// 12x12 Identity Matrix
#define I12 Eigen::MatrixXd::Identity(12, 12)
// 18x18 Identity Matrix
#define I18 Eigen::MatrixXd::Identity(18, 18)
// Dynamic Size Matrix
using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
/************************/
/****** Functions *******/
/************************/
inline Vec34 vec12ToVec34(Vec12 vec12){
Vec34 vec34;
for(int i(0); i < 4; ++i){
vec34.col(i) = vec12.segment(3*i, 3);
}
return vec34;
}
inline Vec12 vec34ToVec12(Vec34 vec34){
Vec12 vec12;
for(int i(0); i < 4; ++i){
vec12.segment(3*i, 3) = vec34.col(i);
}
return vec12;
}
template<typename T>
inline VecX stdVecToEigenVec(T stdVec){
VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size());
return eigenVec;
}
#endif // MATHTYPES_H

242
include/common/utilities/CSVTool.h Executable file
View File

@ -0,0 +1,242 @@
#ifndef CSVTOOL_H
#define CSVTOOL_H
/*
personal tool for .csv reading and modifying.
only suitable for small .csv file.
for large .csv, try <https://github.com/ben-strasser/fast-cpp-csv-parser> (read only)
*/
#include <string>
#include <vector>
#include <map>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include "common/utilities/typeTrans.h"
enum class FileType{
READ_WRITE,
CLEAR_DUMP
};
class CSVLine{
public:
// CSVLine(std::string lineTemp, std::streampos filePos);
CSVLine(std::string lineTemp);
CSVLine(std::string label, std::vector<double> values);
~CSVLine(){}
// void updateFilePos(std::streampos filePos){_filePos = filePos;}
void getValues(std::vector<double> &values);
void changeValue(std::vector<double> values);
void writeAtEnd(std::fstream &ioStream);
std::string getLabel(){return _label;}
private:
// std::streampos _filePos;
std::string _label;
std::vector<double> _values;
};
/*
FileType::READ_WRITE : must already exist such fileName
FileType::CLEAR_DUMP : if do not exist such file, will create one
*/
class CSVTool{
public:
CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6);
~CSVTool(){_ioStream.close();}
bool getLine(std::string label, std::vector<double> &values);
template<typename... Args>
bool getLineDirect(std::string label, Args&... values);
void modifyLine(std::string label, std::vector<double> &values, bool addNew);
template<typename... Args>
void modifyLineDirect(std::string label, bool addNew, Args&... values);
void readFile();
void saveFile();
private:
std::string _fileName;
std::fstream _ioStream;
int _precision;
std::string _lineTemp;
std::map<std::string, size_t> _labels;
std::vector<CSVLine*> _lines;
};
/*************************/
/* CSVLine */
/*************************/
// CSVLine::CSVLine(std::string lineTemp, std::streampos filePos)
// :_filePos(filePos){
// // std::cout << lineTemp << std::endl;
// }
inline CSVLine::CSVLine(std::string lineTemp){
// delete all spaces
lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end());
std::stringstream ss(lineTemp);
std::string stringTemp;
std::getline(ss, _label, ',');
while(std::getline(ss, stringTemp, ',')){
_values.push_back(stod(stringTemp));
}
// std::cout << "**********" << std::endl;
// std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl;
// for(int i(0); i<_values.size(); ++i){
// std::cout << _values.at(i) << ",,, ";
// }
// std::cout << std::endl;
}
inline CSVLine::CSVLine(std::string label, std::vector<double> values)
:_label(label), _values(values){
}
inline void CSVLine::changeValue(std::vector<double> values){
if(values.size() != _values.size()){
std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl;
}
_values = values;
}
inline void CSVLine::getValues(std::vector<double> &values){
values = _values;
}
inline void CSVLine::writeAtEnd(std::fstream &ioStream){
ioStream << _label << ", ";
for(int i(0); i<_values.size(); ++i){
ioStream << _values.at(i) << ", ";
}
ioStream << std::endl;
}
/*************************/
/* CSVTool */
/*************************/
inline CSVTool::CSVTool(std::string fileName, FileType type, int precision)
: _fileName(fileName), _precision(precision){
if(type == FileType::READ_WRITE){
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
if(!_ioStream.is_open()){
std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl;
exit(-1);
}
readFile();
}
else if(type == FileType::CLEAR_DUMP){
_ioStream.open(_fileName, std::fstream::out);
}
}
inline void CSVTool::readFile(){
if(!_ioStream.is_open()){
// _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl;
}
_lines.clear();
_labels.clear();
_ioStream.seekg(0, std::fstream::beg);
size_t lineNum = 0;
while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){
_lines.push_back( new CSVLine(_lineTemp) );
if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){
_labels.insert(std::pair<std::string, size_t>(_lines.at(lineNum)->getLabel(), lineNum));
++lineNum;
}else{
std::cout << "[ERROR] CSVTool::readFile, the label "
<< _lines.at(lineNum)->getLabel() << " is repeated" << std::endl;
exit(-1);
}
}
}
inline bool CSVTool::getLine(std::string label, std::vector<double> &values){
if(_labels.count(label) == 0){
std::cout << "[ERROR] No such label: " << label << std::endl;
return false;
}else{
_lines.at(_labels[label])->getValues(values);
return true;
}
}
template<typename... Args>
inline bool CSVTool::getLineDirect(std::string label, Args&... values){
std::vector<double> vec;
if(getLine(label, vec)){
typeTrans::extractVector(vec, values...);
return true;
}else{
return false;
}
}
template<typename... Args>
inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){
std::vector<double> vec;
typeTrans::combineToVector(vec, values...);
// std::cout << "CSVTool::modifyLineDirect------" << std::endl;
// std::cout << "label: " << label << std::endl;
// std::cout << "vec: ";
// for(int i(0); i<vec.size(); ++i){
// std::cout << vec.at(i) << ", ";
// }std::cout << std::endl;
modifyLine(label, vec, addNew);
}
inline void CSVTool::saveFile(){
_ioStream.close();
_ioStream.open(_fileName, std::fstream::out);
_ioStream << std::fixed << std::setprecision(_precision);
for(int i(0); i<_lines.size(); ++i){
_lines.at(i)->writeAtEnd(_ioStream);
}
_ioStream.close();
_ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out);
}
inline void CSVTool::modifyLine(std::string label, std::vector<double> &values, bool addNew =false){
if(_labels.count(label) == 0){
if(addNew){
_labels.insert(std::pair<std::string, size_t>(label, _labels.size()));
_lines.push_back(new CSVLine(label, values));
}else{
std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl;
exit(-1);
}
}else{
_lines.at(_labels[label])->changeValue(values);
}
}
#endif // CSVTOOL_H

29
include/common/utilities/LPFilter.h Executable file
View File

@ -0,0 +1,29 @@
#ifndef LOWPASSFILTER
#define LOWPASSFILTER
#include <vector>
#include <Eigen/Dense>
/*
Low pass filter
*/
class LPFilter{
public:
LPFilter(double samplePeriod, double cutFrequency, size_t valueCount);
~LPFilter(){};
void clear();
void addValue(double &newValue);
void addValue(std::vector<double> &vec);
template <typename T>
void addValue(Eigen::MatrixBase<T> &eigenVec);
// double getValue();
private:
size_t _valueCount;
double _weight;
std::vector<double> _pastValue;
bool _start;
};
#endif // LOWPASSFILTER

261
include/common/utilities/PyPlot.h Normal file
View File

@ -0,0 +1,261 @@
#ifndef PYPLOT_H
#define PYPLOT_H
#include <map>
#include <thread>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <iostream>
#include "common/utilities/matplotlibcpp.h"
#include "unitree_arm_sdk/timer.h"
namespace plt = matplotlibcpp;
// enum class PlotType{
// STATIC,
// ANIME
// };
struct Curve{
std::vector<double> x;
std::vector<double> y;
void printXY(double xRough, int pointNum){
for(int i(0); i<x.size(); ++i){
if(xRough < x[i]){
for(int j(0); j < pointNum; ++j){
std::cout << " X: " << x[i+j] << ", Y: " << y[i+j] << std::endl;
}
break;
}
}
}
};
struct Plot{
std::vector<Curve*> curves;
std::vector<std::string> labels;
std::string plotName;
int curveCount;
std::map<std::string, int> curveName2ID;
Plot(std::string name, int count, std::vector<std::string> labelVec)
:plotName(name), curveCount(count), labels(labelVec){
for(int i(0); i < count; ++i){
curveName2ID.insert(std::pair<std::string, int>(labels[i], i));
curves.push_back(new Curve());
}
}
~Plot(){
for(int i(0); i < curveCount; ++i){
delete curves[i];
}
}
double getX(long long startT){
return (double)(getSystemTime() - startT) * 1e-6;
}
void printXY(std::string curveName, double xRough, int pointNum){
std::cout << "[DEBUG] Plot: " << plotName << ", Curve: " << curveName << std::endl;
curves[curveName2ID[curveName]]->printXY(xRough, pointNum);
}
};
class PyPlot{
public:
PyPlot();
~PyPlot();
void addPlot(std::string plotName, int curveCount, std::vector<std::string> labelVec);
void addPlot(std::string plotName, int curveCount);
void showPlot(std::string plotName);
void showPlot(std::vector<std::string> plotNameVec);
void showPlotAll();
void printXY(std::string plotName, std::string curveName, double xRough, int pointNum = 1);
void addFrame(std::string plotName, double value);
void addFrame(std::string plotName, double x, double value);
template <typename T>
void addFrame(std::string plotName, T* valueArray);
template <typename T>
void addFrame(std::string plotName, double x, T* valueArray);
template <typename T>
void addFrame(std::string plotName, const Eigen::MatrixBase<T> &vec);
template <typename T>
void addFrame(std::string plotName, double x, const Eigen::MatrixBase<T> &vec);
template <typename T>
void addFrame(std::string plotName, const std::vector<T> &vec);
template <typename T>
void addFrame(std::string plotName, double x, const std::vector<T> &vec);
private:
void _checkStart();
int _plotCount = 0;
std::map<std::string, int> _plotName2ID;
std::vector< Plot* > _plots;
// std::thread _plotThread;
long long _pointNum;
Plot* _getPlotPtr(std::string plotName);
bool start;
long long startT;
};
inline PyPlot::PyPlot(){
start = false;
// _plotThread = std::thread(&PyPlot::_plotThreadFunc, this);
}
inline PyPlot::~PyPlot(){
for(int i(0); i < _plotCount; ++i){
delete _plots[i];
}
}
inline void PyPlot::_checkStart(){
if(!start){
start = true;
startT = getSystemTime();
}
}
inline void PyPlot::printXY(std::string plotName, std::string curveName, double xRough, int pointNum){
_plots[_plotName2ID[plotName]]->printXY(curveName, xRough, pointNum);
}
inline void PyPlot::addPlot(std::string plotName, int curveCount, std::vector<std::string> labelVec){
if(_plotName2ID.count(plotName) == 0){
_plotName2ID.insert(std::pair<std::string, int>(plotName, _plotCount));
++_plotCount;
_plots.push_back( new Plot(plotName, curveCount, labelVec) );
}else{
std::cout << "[ERROR] Already has same Plot: " << plotName << std::endl;
exit(-1);
}
}
inline void PyPlot::addPlot(std::string plotName, int curveCount){
std::vector<std::string> label;
for(int i(0); i < curveCount; ++i){
label.push_back(std::to_string(i+1));
}
addPlot(plotName, curveCount, label);
}
inline void PyPlot::showPlot(std::string plotName){
Plot* plot = _getPlotPtr(plotName);
plt::figure();
plt::title(plot->plotName);
for(int i(0); i < plot->curveCount; ++i){
plt::named_plot(plot->labels[i], plot->curves[i]->x, plot->curves[i]->y);
}
plt::legend();
plt::show();
}
inline void PyPlot::showPlot(std::vector<std::string> plotNameVec){
for(std::vector<std::string>::iterator itName = plotNameVec.begin(); itName != plotNameVec.end(); ++itName){
plt::figure();
Plot* plot = _plots[_plotName2ID[*itName]];
plt::title(plot->plotName);
for(int i(0); i < plot->curveCount; ++i){
plt::named_plot(plot->labels[i], plot->curves[i]->x, plot->curves[i]->y);
}
plt::legend();
}
plt::show();
}
inline void PyPlot::showPlotAll(){
for(int i(0); i < _plotCount; ++i){
plt::figure();
Plot* plot = _plots[i];
plt::title(plot->plotName);
for(int j(0); j < plot->curveCount; ++j){
plt::named_plot(plot->labels[j], plot->curves[j]->x, plot->curves[j]->y);
}
plt::legend();
}
plt::show();
}
inline Plot* PyPlot::_getPlotPtr(std::string plotName){
if(_plotName2ID.count(plotName) == 0){
std::cout << "[ERROR] Plot " << plotName << " does not exist" << std::endl;
exit(-1);
}else{
return _plots[_plotName2ID[plotName]];
}
}
inline void PyPlot::addFrame(std::string plotName, double value){
_checkStart();
Plot* plot = _getPlotPtr(plotName);
addFrame(plotName, plot->getX(startT), value);
}
inline void PyPlot::addFrame(std::string plotName, double x, double value){
Plot* plot = _getPlotPtr(plotName);
plot->curves[0]->x.push_back(x);
plot->curves[0]->y.push_back(value);
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, T* valueArray){
_checkStart();
Plot* plot = _getPlotPtr(plotName);
addFrame(plotName, plot->getX(startT), valueArray);
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, double x, T* valueArray){
Plot* plot = _getPlotPtr(plotName);
for(int i(0); i < plot->curveCount; ++i){
plot->curves[i]->x.push_back(x);
plot->curves[i]->y.push_back(valueArray[i]);
}
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, const Eigen::MatrixBase<T> &vec){
_checkStart();
Plot* plot = _getPlotPtr(plotName);
addFrame(plotName, plot->getX(startT), vec);
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, double x, const Eigen::MatrixBase<T> &vec){
Plot* plot = _getPlotPtr(plotName);
for(int i(0); i < plot->curveCount; ++i){
plot->curves[i]->x.push_back(x);
plot->curves[i]->y.push_back(vec(i));
}
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, const std::vector<T> &vec){
_checkStart();
Plot* plot = _getPlotPtr(plotName);
addFrame(plotName, plot->getX(startT), vec);
}
template <typename T>
inline void PyPlot::addFrame(std::string plotName, double x, const std::vector<T> &vec){
Plot* plot = _getPlotPtr(plotName);
for(int i(0); i < plot->curveCount; ++i){
plot->curves[i]->x.push_back(x);
plot->curves[i]->y.push_back(vec[i]);
}
}
#endif // PYPLOT_H

2557
include/common/utilities/matplotlibcpp.h Executable file
View File

File diff suppressed because it is too large Load Diff

90
include/common/utilities/typeTrans.h Executable file
View File

@ -0,0 +1,90 @@
#ifndef TYPETRANS_H
#define TYPETRANS_H
#include <iostream>
#include <vector>
#include <Eigen/Dense>
/* Tools */
// const std::string convert(const long l, const size_t n){
// long val=l;
// std::stringstream ss;
// do
// {
// ss<<(val%n);
// val/=n;
// }while(val!=0);
// std::string s=ss.str();
// std::reverse(s.begin(),s.end());
// return s;
// }
namespace typeTrans{
inline void addValue(std::vector<double> &vec, double value){
vec.push_back(value);
}
inline double getValue(std::vector<double> &vec, double value){
value = vec.at(0);
std::vector<double>::iterator begin = vec.begin();
vec.erase(begin);
return value;
}
inline void addValue(std::vector<double> &vec, Eigen::MatrixXd value){
for(int i(0); i<value.rows(); ++i){
for(int j(0); j<value.cols(); ++j){
vec.push_back(value(i, j));
}
}
}
inline Eigen::MatrixXd getValue(std::vector<double> &vec, Eigen::MatrixXd value){
std::vector<double>::iterator begin = vec.begin();
std::vector<double>::iterator end = begin;
for(int i(0); i<value.rows(); ++i){
for(int j(0); j<value.cols(); ++j){
value(i, j) = *end;
++end;
}
}
vec.erase(begin, end);
return value;
}
/* combine different type variables to vector */
template<typename T>
inline void combineToVector(std::vector<double> &vec, T value){
addValue(vec, value);
}
template<typename T, typename... Args>
inline void combineToVector(std::vector<double> &vec, const T t, const Args... rest){
combineToVector(vec, t);
combineToVector(vec, rest...);
}
/* extract different type variables from vector */
template<typename T>
inline void extractVector(std::vector<double> &vec, T &value){
value = getValue(vec, value);
}
template<typename T, typename... Args>
inline void extractVector(std::vector<double> &vec, T &t, Args&... rest){
extractVector(vec, t);
extractVector(vec, rest...);
}
}
#endif // TYPETRANS_H

84
include/control/CtrlComponents.h Executable file
View File

@ -0,0 +1,84 @@
#ifndef CTRLCOMPONENTS_H
#define CTRLCOMPONENTS_H
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "interface/IOInterface.h"
#include "interface/IOROS.h"
#include "model/ArmDynKineModel.h"
#include "model/ArmReal.h"
#include "common/utilities/CSVTool.h"
#include <string>
#include <iostream>
#ifdef COMPILE_DEBUG
#include "common/utilities/PyPlot.h"
#endif // COMPILE_DEBUG
struct CtrlComponents{
public:
CtrlComponents(){}
~CtrlComponents(){
delete lowCmd;
delete lowState;
delete ioInter;
delete armModel;
delete stateCSV;
#ifdef COMPILE_DEBUG
delete plot;
#endif // COMPILE_DEBUG
}
int dof;
std::string armConfigPath;
LowlevelCmd *lowCmd;
LowlevelState *lowState;
IOInterface *ioInter;
ArmDynKineModel *armModel;
CSVTool *stateCSV;
#ifdef COMPILE_DEBUG
PyPlot *plot;
#endif // COMPILE_DEBUG
double dt;
bool *running;
void sendRecv(){
ioInter->sendRecv(lowCmd, lowState);
}
void geneObj(){
lowCmd = new LowlevelCmd();
lowState = new LowlevelState(dt);
#ifdef NO_GRIPPER
// armModel = new Z1DynKineModel();
armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0),
0.0, Vec3(0.0, 0.0, 0.0), Vec3(0.0, 0.0, 0.0).asDiagonal());
#endif
#ifdef AG95
armModel = new Z1DynKineModel(Vec3(0.21, 0, 0),
1.107, Vec3(0.092, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal());
#endif
#ifdef MASS3KG
armModel = new Z1DynKineModel(Vec3(0.05, 0, 0),
3.1, Vec3(0.05, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal());
#endif
#ifdef UNITREE_GRIPPER
// 安装面高出6mm
// armModel = new Z1DynKineModel(Vec3(0.141, 0, -0.005),
// 0.712, Vec3(0.0488, 0.0, 0.002), Vec3(0.000688, 0.000993, 0.000812).asDiagonal());
armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0),
0.80225, Vec3(0.0037, 0.0014, -0.0003), Vec3(0.00057593, 0.00099960, 0.00106337).asDiagonal());
#endif
}
private:
};
#endif // CTRLCOMPONENTS_H

35
include/interface/IOInterface.h Executable file
View File

@ -0,0 +1,35 @@
#ifndef IOINTERFACE_H
#define IOINTERFACE_H
#include "message/LowlevelCmd.h"
#include "message/LowlevelState.h"
#include "unitree_arm_sdk/cmdPanel.h"
#include "model/ArmReal.h"
#include "unitree_arm_sdk/keyboard.h"
#include <string>
class IOInterface{
public:
IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){}
virtual ~IOInterface(){delete _cmdPanel;};
virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
// void zeroCmdPanel(){_cmdPanel->setZero();}
std::string getString(std::string slogan){return _cmdPanel->getString(slogan);}
std::vector<double> stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);}
std::vector<std::vector<double> > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);}
virtual bool calibration(){};
#ifdef CTRL_BY_SDK
SendCmd getSendCmd(){return _cmdPanel->getSendCmd();};
void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);};
#endif
protected:
CmdPanel *_cmdPanel;
};
#endif //IOINTERFACE_H

50
include/interface/IOROS.h Executable file
View File

@ -0,0 +1,50 @@
#ifdef COMPILE_WITH_SIMULATION
#ifndef IOROS_H
#define IOROS_H
#include <ros/ros.h>
#include "interface/IOInterface.h"
#include "unitree_arm_sdk/keyboard.h"
#include "message/unitree_legged_msgs/MotorCmd.h"
#include "message/unitree_legged_msgs/MotorState.h"
class IOROS : public IOInterface{
public:
IOROS(CmdPanel *cmdPanel);
~IOROS();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
// void setGripper(double position, double force){};
// void getGripper(double &position, double &force){};
private:
ros::NodeHandle _nm;
#ifndef UNITREE_GRIPPER
ros::Subscriber _servo_sub[6];
ros::Publisher _servo_pub[6];
unitree_legged_msgs::MotorState _joint_state[6];
unitree_legged_msgs::MotorCmd _joint_cmd[6];
#endif
#ifdef UNITREE_GRIPPER
ros::Subscriber _servo_sub[7];
ros::Publisher _servo_pub[7];
unitree_legged_msgs::MotorState _joint_state[7];
unitree_legged_msgs::MotorCmd _joint_cmd[7];
#endif
void _sendCmd(const LowlevelCmd *cmd);
void _recvState(LowlevelState *state);
void _initRecv();
void _initSend();
void _joint00Callback(const unitree_legged_msgs::MotorState& msg);
void _joint01Callback(const unitree_legged_msgs::MotorState& msg);
void _joint02Callback(const unitree_legged_msgs::MotorState& msg);
void _joint03Callback(const unitree_legged_msgs::MotorState& msg);
void _joint04Callback(const unitree_legged_msgs::MotorState& msg);
void _joint05Callback(const unitree_legged_msgs::MotorState& msg);
void _gripperCallback(const unitree_legged_msgs::MotorState& msg);
};
#endif // IOROS_H
#endif // COMPILE_WITH_SIMULATION

55
include/interface/IOUDP.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef IOUDP_H
#define IOUDP_H
#include "interface/IOInterface.h"
#include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
// #pragma pack(1)
// struct JointCmd{
// _Float32 T;
// _Float32 W;
// _Float32 Pos;
// _Float32 K_P;
// _Float32 K_W;
// };
// struct JointState{
// _Float32 T;
// _Float32 W;
// _Float32 Acc;
// _Float32 Pos;
// };
// union SendCmd{
// uint8_t checkCmd;
// JointCmd jointCmd[7];
// };
// union RecvState{
// uint8_t singleState;
// uint8_t selfCheck[10];
// JointState jointState[7];
// uint8_t errorCheck[16];
// };
// #pragma pack()
class IOUDP : public IOInterface{
public:
IOUDP(CmdPanel *cmdPanel);
~IOUDP();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
// void setGripper(int position, int force);
// void getGripper(int &position, int &force);
bool calibration();
private:
IOPort *_ioPort;
UDPSendCmd _cmd;
UDPRecvState _state;
size_t _motorNum;
size_t _jointNum;
};
#endif // IOUDP_H

61
include/message/LowlevelCmd.h Executable file
View File

@ -0,0 +1,61 @@
#ifndef LOWLEVELCMD_H
#define LOWLEVELCMD_H
#include "common/math/mathTypes.h"
#include "common/math/mathTools.h"
#include <vector>
#include <iostream>
struct LowlevelCmd{
private:
size_t _dof = 6;
public:
std::vector<double> q;
std::vector<double> dq;
std::vector<double> tau;
std::vector<double> kp;
std::vector<double> kd;
LowlevelCmd(){
#ifndef UNITREE_GRIPPER
q.resize(_dof);
dq.resize(_dof);
tau.resize(_dof);
kp.resize(_dof);
kd.resize(_dof);
#endif
#ifdef UNITREE_GRIPPER
q.resize(_dof+1);
dq.resize(_dof+1);
tau.resize(_dof+1);
kp.resize(_dof+1);
kd.resize(_dof+1);
#endif
}
~LowlevelCmd(){}
void setZeroDq();
void setZeroTau();
void setZeroKp();
void setZeroKd();
void setControlGain();
void setControlGain(std::vector<float> KP, std::vector<float> KW);
void setQ(std::vector<double> qInput);
void setQ(VecX qInput);
void setQd(VecX qDInput);
void setTau(VecX tauInput);
void setPassive();
void setGripperGain();
void setGripperGain(float KP, float KW);
void setGripperZeroGain();
void setGripperQ(double qInput);
void setGripperQd(double qdInput);
void setGripperTau(double tauInput);
Vec6 getQ();
Vec6 getQd();
};
#endif //LOWLEVELCMD_H

167
include/message/LowlevelState.h Executable file
View File

@ -0,0 +1,167 @@
#ifndef LOWLEVELSTATE_HPP
#define LOWLEVELSTATE_HPP
#include <iostream>
#include <vector>
#include "common/math/mathTypes.h"
#include "common/math/mathTools.h"
#include "common/enumClass.h"
#include "common/utilities/LPFilter.h"
#include "message/UserValue.h"
// struct MotorState
// {
// unsigned int mode;
// float q;
// float dq;
// float ddq;
// float tauEst;
// MotorState(){
// q = 0;
// dq = 0;
// ddq = 0;
// tauEst = 0;
// }
// };
struct LowlevelState{
private:
size_t _dof = 6;
public:
LowlevelState(double dt){
#ifndef UNITREE_GRIPPER
qFilter = new LPFilter(dt, 3.0, _dof);
dqFilter = new LPFilter(dt, 3.0, _dof);
tauFilter = new LPFilter(dt, 5.0, _dof);
q.resize(_dof);
dq.resize(_dof);
ddq.resize(_dof);
tau.resize(_dof);
qFiltered.resize(_dof);
dqFiltered.resize(_dof);
tauFiltered.resize(_dof);
#endif
#ifdef UNITREE_GRIPPER
qFilter = new LPFilter(dt, 3.0, _dof+1);
dqFilter = new LPFilter(dt, 3.0, _dof+1);
tauFilter = new LPFilter(dt, 5.0, _dof+1);
q.resize(_dof+1);
dq.resize(_dof+1);
ddq.resize(_dof+1);
tau.resize(_dof+1);
qFiltered.resize(_dof+1);
dqFiltered.resize(_dof+1);
tauFiltered.resize(_dof+1);
#endif
}
~LowlevelState(){
delete qFilter;
delete dqFilter;
delete tauFilter;
}
// MotorState motorState[12];
std::vector<double> q;
std::vector<double> dq;
std::vector<double> ddq;
std::vector<double> tau;
std::vector<double> qFiltered;
std::vector<double> dqFiltered;
std::vector<double> tauFiltered;
// ArmFSMStateName userCmd;
UserValue userValue;
LPFilter *qFilter;
LPFilter *dqFilter;
LPFilter *tauFilter;
void runFilter(){
qFiltered = q;
qFilter->addValue(qFiltered);
dqFiltered = dq;
dqFilter->addValue(dqFiltered);
tauFiltered = tau;
tauFilter->addValue(tauFiltered);
}
Vec6 getQ(){
Vec6 qReturn;
for(int i(0); i < 6; ++i){
qReturn(i) = q.at(i);
}
return qReturn;
}
Vec6 getQd(){
Vec6 qdReturn;
for(int i(0); i < 6; ++i){
qdReturn(i) = dq.at(i);
}
return qdReturn;
}
Vec6 getQdd(){
Vec6 qddReturn;
for(int i(0); i < 6; ++i){
qddReturn(i) = ddq.at(i);
}
return qddReturn;
}
Vec6 getTau(){
Vec6 tauReturn;
for(int i(0); i < 6; ++i){
tauReturn(i) = tau.at(i);
}
return tauReturn;
}
Vec6 getQFiltered(){
Vec6 qReturn;
for(int i(0); i < 6; ++i){
qReturn(i) = qFiltered.at(i);
}
return qReturn;
}
Vec6 getQdFiltered(){
Vec6 dqReturn;
for(int i(0); i < 6; ++i){
dqReturn(i) = dqFiltered.at(i);
}
return dqReturn;
}
Vec6 getTauFiltered(){
Vec6 tauReturn;
for(int i(0); i < 6; ++i){
tauReturn(i) = tauFiltered.at(i);
}
return tauReturn;
}
double getGripperQ(){
return q.at(q.size()-1);
}
double getGripperQd(){
return dq.at(q.size()-1);
}
double getGripperTau(){
return tau.at(tau.size()-1);
}
double getGripperTauFiltered(){
return tauFiltered.at(tau.size()-1);
}
};
#endif //LOWLEVELSTATE_HPP

56
include/message/UserValue.h Normal file
View File

@ -0,0 +1,56 @@
#ifndef USERVALUE_H
#define USERVALUE_H
#include "common/math/mathTypes.h"
#include "common/utilities/typeTrans.h"
#include <vector>
template<typename T>
std::vector<T> cutVector(std::vector<T> vec, size_t startID, size_t length){
std::vector<T> result;
result.assign(vec.begin()+startID, vec.begin()+startID+length);
return result;
}
struct UserValue{
Vec6 moveAxis;
double gripperPos;
void setData(std::vector<double> rawData){
#ifdef CTRL_BY_SDK
if(rawData.size() != 7){
std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
}
gripperPos = rawData.at(6);
rawData = cutVector(rawData, 0, 6);
moveAxis = typeTrans::getValue(rawData, moveAxis);
#endif
#ifdef CTRL_BY_KEYBOARD
if(rawData.size() != 7){
std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
}
gripperPos = rawData.at(6);
rawData = cutVector(rawData, 0, 6);
moveAxis = typeTrans::getValue(rawData, moveAxis);
#endif
#ifdef CTRL_BY_JOYSTICK
if(rawData.size() != 7){
std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
}
gripperPos = rawData.at(6);
rawData = cutVector(rawData, 0, 6);
moveAxis = typeTrans::getValue(rawData, moveAxis);
#endif
}
UserValue(){
setZero();
}
void setZero(){
moveAxis.setZero();
gripperPos = 0;
// gripperTau = 0;
}
};
#endif

215
include/message/unitree_legged_msgs/Cartesian.h Normal file
View File

@ -0,0 +1,215 @@
// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct Cartesian_
{
typedef Cartesian_<ContainerAllocator> Type;
Cartesian_()
: x(0.0)
, y(0.0)
, z(0.0) {
}
Cartesian_(const ContainerAllocator& _alloc)
: x(0.0)
, y(0.0)
, z(0.0) {
(void)_alloc;
}
typedef float _x_type;
_x_type x;
typedef float _y_type;
_y_type y;
typedef float _z_type;
_z_type z;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const> ConstPtr;
}; // struct Cartesian_
typedef ::unitree_legged_msgs::Cartesian_<std::allocator<void> > Cartesian;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian > CartesianPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian const> CartesianConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.z == rhs.z;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "cc153912f1453b708d221682bc23d9ac";
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xcc153912f1453b70ULL;
static const uint64_t static_value2 = 0x8d221682bc23d9acULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/Cartesian";
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
static const char* value()
{
return "float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.x);
stream.next(m.y);
stream.next(m.z);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct Cartesian_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>& v)
{
s << indent << "x: ";
Printer<float>::stream(s, indent + " ", v.x);
s << indent << "y: ";
Printer<float>::stream(s, indent + " ", v.y);
s << indent << "z: ";
Printer<float>::stream(s, indent + " ", v.z);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H

403
include/message/unitree_legged_msgs/HighCmd.h Normal file
View File

@ -0,0 +1,403 @@
// Generated by gencpp from file unitree_legged_msgs/HighCmd.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/LED.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct HighCmd_
{
typedef HighCmd_<ContainerAllocator> Type;
HighCmd_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, footRaiseHeight(0.0)
, yaw(0.0)
, pitch(0.0)
, roll(0.0)
, led()
, wirelessRemote()
, AppRemote()
, reserve(0)
, crc(0) {
wirelessRemote.assign(0);
AppRemote.assign(0);
}
HighCmd_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, footRaiseHeight(0.0)
, yaw(0.0)
, pitch(0.0)
, roll(0.0)
, led()
, wirelessRemote()
, AppRemote()
, reserve(0)
, crc(0) {
(void)_alloc;
led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
wirelessRemote.assign(0);
AppRemote.assign(0);
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef uint8_t _mode_type;
_mode_type mode;
typedef float _forwardSpeed_type;
_forwardSpeed_type forwardSpeed;
typedef float _sideSpeed_type;
_sideSpeed_type sideSpeed;
typedef float _rotateSpeed_type;
_rotateSpeed_type rotateSpeed;
typedef float _bodyHeight_type;
_bodyHeight_type bodyHeight;
typedef float _footRaiseHeight_type;
_footRaiseHeight_type footRaiseHeight;
typedef float _yaw_type;
_yaw_type yaw;
typedef float _pitch_type;
_pitch_type pitch;
typedef float _roll_type;
_roll_type roll;
typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4> _led_type;
_led_type led;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef boost::array<uint8_t, 40> _AppRemote_type;
_AppRemote_type AppRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef int32_t _crc_type;
_crc_type crc;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const> ConstPtr;
}; // struct HighCmd_
typedef ::unitree_legged_msgs::HighCmd_<std::allocator<void> > HighCmd;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd > HighCmdPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd const> HighCmdConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.mode == rhs.mode &&
lhs.forwardSpeed == rhs.forwardSpeed &&
lhs.sideSpeed == rhs.sideSpeed &&
lhs.rotateSpeed == rhs.rotateSpeed &&
lhs.bodyHeight == rhs.bodyHeight &&
lhs.footRaiseHeight == rhs.footRaiseHeight &&
lhs.yaw == rhs.yaw &&
lhs.pitch == rhs.pitch &&
lhs.roll == rhs.roll &&
lhs.led == rhs.led &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.AppRemote == rhs.AppRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "1a655499a3f64905db59ceed65ca774a";
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x1a655499a3f64905ULL;
static const uint64_t static_value2 = 0xdb59ceed65ca774aULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/HighCmd";
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"uint8 mode\n"
"float32 forwardSpeed\n"
"float32 sideSpeed\n"
"float32 rotateSpeed \n"
"float32 bodyHeight\n"
"float32 footRaiseHeight\n"
"float32 yaw\n"
"float32 pitch\n"
"float32 roll\n"
"LED[4] led\n"
"uint8[40] wirelessRemote\n"
"uint8[40] AppRemote # Old version Aliengo does not have\n"
"uint32 reserve # Old version Aliengo does not have\n"
"int32 crc\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/LED\n"
"uint8 r\n"
"uint8 g\n"
"uint8 b\n"
;
}
static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.mode);
stream.next(m.forwardSpeed);
stream.next(m.sideSpeed);
stream.next(m.rotateSpeed);
stream.next(m.bodyHeight);
stream.next(m.footRaiseHeight);
stream.next(m.yaw);
stream.next(m.pitch);
stream.next(m.roll);
stream.next(m.led);
stream.next(m.wirelessRemote);
stream.next(m.AppRemote);
stream.next(m.reserve);
stream.next(m.crc);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct HighCmd_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "forwardSpeed: ";
Printer<float>::stream(s, indent + " ", v.forwardSpeed);
s << indent << "sideSpeed: ";
Printer<float>::stream(s, indent + " ", v.sideSpeed);
s << indent << "rotateSpeed: ";
Printer<float>::stream(s, indent + " ", v.rotateSpeed);
s << indent << "bodyHeight: ";
Printer<float>::stream(s, indent + " ", v.bodyHeight);
s << indent << "footRaiseHeight: ";
Printer<float>::stream(s, indent + " ", v.footRaiseHeight);
s << indent << "yaw: ";
Printer<float>::stream(s, indent + " ", v.yaw);
s << indent << "pitch: ";
Printer<float>::stream(s, indent + " ", v.pitch);
s << indent << "roll: ";
Printer<float>::stream(s, indent + " ", v.roll);
s << indent << "led[]" << std::endl;
for (size_t i = 0; i < v.led.size(); ++i)
{
s << indent << " led[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + " ", v.led[i]);
}
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "AppRemote[]" << std::endl;
for (size_t i = 0; i < v.AppRemote.size(); ++i)
{
s << indent << " AppRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.AppRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<int32_t>::stream(s, indent + " ", v.crc);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H

497
include/message/unitree_legged_msgs/HighState.h Normal file
View File

@ -0,0 +1,497 @@
// Generated by gencpp from file unitree_legged_msgs/HighState.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/IMU.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct HighState_
{
typedef HighState_<ContainerAllocator> Type;
HighState_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, imu()
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, updownSpeed(0.0)
, forwardPosition(0.0)
, sidePosition(0.0)
, footPosition2Body()
, footSpeed2Body()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForce()
, jointP() {
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
jointP.assign(0.0);
}
HighState_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, mode(0)
, imu(_alloc)
, forwardSpeed(0.0)
, sideSpeed(0.0)
, rotateSpeed(0.0)
, bodyHeight(0.0)
, updownSpeed(0.0)
, forwardPosition(0.0)
, sidePosition(0.0)
, footPosition2Body()
, footSpeed2Body()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForce()
, jointP() {
(void)_alloc;
footPosition2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
jointP.assign(0.0);
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef uint8_t _mode_type;
_mode_type mode;
typedef ::unitree_legged_msgs::IMU_<ContainerAllocator> _imu_type;
_imu_type imu;
typedef float _forwardSpeed_type;
_forwardSpeed_type forwardSpeed;
typedef float _sideSpeed_type;
_sideSpeed_type sideSpeed;
typedef float _rotateSpeed_type;
_rotateSpeed_type rotateSpeed;
typedef float _bodyHeight_type;
_bodyHeight_type bodyHeight;
typedef float _updownSpeed_type;
_updownSpeed_type updownSpeed;
typedef float _forwardPosition_type;
_forwardPosition_type forwardPosition;
typedef float _sidePosition_type;
_sidePosition_type sidePosition;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _footPosition2Body_type;
_footPosition2Body_type footPosition2Body;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _footSpeed2Body_type;
_footSpeed2Body_type footSpeed2Body;
typedef boost::array<int16_t, 4> _footForce_type;
_footForce_type footForce;
typedef boost::array<int16_t, 4> _footForceEst_type;
_footForceEst_type footForceEst;
typedef uint32_t _tick_type;
_tick_type tick;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForce_type;
_eeForce_type eeForce;
typedef boost::array<float, 12> _jointP_type;
_jointP_type jointP;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> const> ConstPtr;
}; // struct HighState_
typedef ::unitree_legged_msgs::HighState_<std::allocator<void> > HighState;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState > HighStatePtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::HighState const> HighStateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.mode == rhs.mode &&
lhs.imu == rhs.imu &&
lhs.forwardSpeed == rhs.forwardSpeed &&
lhs.sideSpeed == rhs.sideSpeed &&
lhs.rotateSpeed == rhs.rotateSpeed &&
lhs.bodyHeight == rhs.bodyHeight &&
lhs.updownSpeed == rhs.updownSpeed &&
lhs.forwardPosition == rhs.forwardPosition &&
lhs.sidePosition == rhs.sidePosition &&
lhs.footPosition2Body == rhs.footPosition2Body &&
lhs.footSpeed2Body == rhs.footSpeed2Body &&
lhs.footForce == rhs.footForce &&
lhs.footForceEst == rhs.footForceEst &&
lhs.tick == rhs.tick &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.eeForce == rhs.eeForce &&
lhs.jointP == rhs.jointP;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "a12e8b22df896c82203810ec6d521dad";
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xa12e8b22df896c82ULL;
static const uint64_t static_value2 = 0x203810ec6d521dadULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/HighState";
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"uint8 mode\n"
"IMU imu\n"
"float32 forwardSpeed\n"
"float32 sideSpeed\n"
"float32 rotateSpeed\n"
"float32 bodyHeight\n"
"float32 updownSpeed\n"
"float32 forwardPosition # (will be float type next version) # Old version Aliengo is different\n"
"float32 sidePosition # (will be float type next version) # Old version Aliengo is different\n"
"Cartesian[4] footPosition2Body\n"
"Cartesian[4] footSpeed2Body\n"
"int16[4] footForce # Old version Aliengo is different\n"
"int16[4] footForceEst # Old version Aliengo does not have\n"
"uint32 tick \n"
"uint8[40] wirelessRemote\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"# Under are not defined in SDK yet. # Old version Aliengo does not have\n"
"Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.\n"
"float32[12] jointP # for visualization\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/IMU\n"
"float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.mode);
stream.next(m.imu);
stream.next(m.forwardSpeed);
stream.next(m.sideSpeed);
stream.next(m.rotateSpeed);
stream.next(m.bodyHeight);
stream.next(m.updownSpeed);
stream.next(m.forwardPosition);
stream.next(m.sidePosition);
stream.next(m.footPosition2Body);
stream.next(m.footSpeed2Body);
stream.next(m.footForce);
stream.next(m.footForceEst);
stream.next(m.tick);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.eeForce);
stream.next(m.jointP);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct HighState_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "imu: ";
s << std::endl;
Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + " ", v.imu);
s << indent << "forwardSpeed: ";
Printer<float>::stream(s, indent + " ", v.forwardSpeed);
s << indent << "sideSpeed: ";
Printer<float>::stream(s, indent + " ", v.sideSpeed);
s << indent << "rotateSpeed: ";
Printer<float>::stream(s, indent + " ", v.rotateSpeed);
s << indent << "bodyHeight: ";
Printer<float>::stream(s, indent + " ", v.bodyHeight);
s << indent << "updownSpeed: ";
Printer<float>::stream(s, indent + " ", v.updownSpeed);
s << indent << "forwardPosition: ";
Printer<float>::stream(s, indent + " ", v.forwardPosition);
s << indent << "sidePosition: ";
Printer<float>::stream(s, indent + " ", v.sidePosition);
s << indent << "footPosition2Body[]" << std::endl;
for (size_t i = 0; i < v.footPosition2Body.size(); ++i)
{
s << indent << " footPosition2Body[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.footPosition2Body[i]);
}
s << indent << "footSpeed2Body[]" << std::endl;
for (size_t i = 0; i < v.footSpeed2Body.size(); ++i)
{
s << indent << " footSpeed2Body[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.footSpeed2Body[i]);
}
s << indent << "footForce[]" << std::endl;
for (size_t i = 0; i < v.footForce.size(); ++i)
{
s << indent << " footForce[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForce[i]);
}
s << indent << "footForceEst[]" << std::endl;
for (size_t i = 0; i < v.footForceEst.size(); ++i)
{
s << indent << " footForceEst[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForceEst[i]);
}
s << indent << "tick: ";
Printer<uint32_t>::stream(s, indent + " ", v.tick);
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "eeForce[]" << std::endl;
for (size_t i = 0; i < v.eeForce.size(); ++i)
{
s << indent << " eeForce[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForce[i]);
}
s << indent << "jointP[]" << std::endl;
for (size_t i = 0; i < v.jointP.size(); ++i)
{
s << indent << " jointP[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.jointP[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H

247
include/message/unitree_legged_msgs/IMU.h Normal file
View File

@ -0,0 +1,247 @@
// Generated by gencpp from file unitree_legged_msgs/IMU.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
#define UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct IMU_
{
typedef IMU_<ContainerAllocator> Type;
IMU_()
: quaternion()
, gyroscope()
, accelerometer()
, temperature(0) {
quaternion.assign(0.0);
gyroscope.assign(0.0);
accelerometer.assign(0.0);
}
IMU_(const ContainerAllocator& _alloc)
: quaternion()
, gyroscope()
, accelerometer()
, temperature(0) {
(void)_alloc;
quaternion.assign(0.0);
gyroscope.assign(0.0);
accelerometer.assign(0.0);
}
typedef boost::array<float, 4> _quaternion_type;
_quaternion_type quaternion;
typedef boost::array<float, 3> _gyroscope_type;
_gyroscope_type gyroscope;
typedef boost::array<float, 3> _accelerometer_type;
_accelerometer_type accelerometer;
typedef int8_t _temperature_type;
_temperature_type temperature;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> const> ConstPtr;
}; // struct IMU_
typedef ::unitree_legged_msgs::IMU_<std::allocator<void> > IMU;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU > IMUPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::IMU const> IMUConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
{
return lhs.quaternion == rhs.quaternion &&
lhs.gyroscope == rhs.gyroscope &&
lhs.accelerometer == rhs.accelerometer &&
lhs.temperature == rhs.temperature;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "dd4bb4e42aa2f15aa1fb1b6a3c3752cb";
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xdd4bb4e42aa2f15aULL;
static const uint64_t static_value2 = 0xa1fb1b6a3c3752cbULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/IMU";
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
static const char* value()
{
return "float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
;
}
static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.quaternion);
stream.next(m.gyroscope);
stream.next(m.accelerometer);
stream.next(m.temperature);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct IMU_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_<ContainerAllocator>& v)
{
s << indent << "quaternion[]" << std::endl;
for (size_t i = 0; i < v.quaternion.size(); ++i)
{
s << indent << " quaternion[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.quaternion[i]);
}
s << indent << "gyroscope[]" << std::endl;
for (size_t i = 0; i < v.gyroscope.size(); ++i)
{
s << indent << " gyroscope[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.gyroscope[i]);
}
s << indent << "accelerometer[]" << std::endl;
for (size_t i = 0; i < v.accelerometer.size(); ++i)
{
s << indent << " accelerometer[" << i << "]: ";
Printer<float>::stream(s, indent + " ", v.accelerometer[i]);
}
s << indent << "temperature: ";
Printer<int8_t>::stream(s, indent + " ", v.temperature);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H

215
include/message/unitree_legged_msgs/LED.h Normal file
View File

@ -0,0 +1,215 @@
// Generated by gencpp from file unitree_legged_msgs/LED.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LED_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LED_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LED_
{
typedef LED_<ContainerAllocator> Type;
LED_()
: r(0)
, g(0)
, b(0) {
}
LED_(const ContainerAllocator& _alloc)
: r(0)
, g(0)
, b(0) {
(void)_alloc;
}
typedef uint8_t _r_type;
_r_type r;
typedef uint8_t _g_type;
_g_type g;
typedef uint8_t _b_type;
_b_type b;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> const> ConstPtr;
}; // struct LED_
typedef ::unitree_legged_msgs::LED_<std::allocator<void> > LED;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED > LEDPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LED const> LEDConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
{
return lhs.r == rhs.r &&
lhs.g == rhs.g &&
lhs.b == rhs.b;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "353891e354491c51aabe32df673fb446";
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x353891e354491c51ULL;
static const uint64_t static_value2 = 0xaabe32df673fb446ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LED";
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 r\n"
"uint8 g\n"
"uint8 b\n"
;
}
static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.r);
stream.next(m.g);
stream.next(m.b);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LED_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_<ContainerAllocator>& v)
{
s << indent << "r: ";
Printer<uint8_t>::stream(s, indent + " ", v.r);
s << indent << "g: ";
Printer<uint8_t>::stream(s, indent + " ", v.g);
s << indent << "b: ";
Printer<uint8_t>::stream(s, indent + " ", v.b);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H

348
include/message/unitree_legged_msgs/LowCmd.h Normal file
View File

@ -0,0 +1,348 @@
// Generated by gencpp from file unitree_legged_msgs/LowCmd.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/LED.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LowCmd_
{
typedef LowCmd_<ContainerAllocator> Type;
LowCmd_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, motorCmd()
, led()
, wirelessRemote()
, reserve(0)
, crc(0)
, ff() {
wirelessRemote.assign(0);
}
LowCmd_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, motorCmd()
, led()
, wirelessRemote()
, reserve(0)
, crc(0)
, ff() {
(void)_alloc;
motorCmd.assign( ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> (_alloc));
led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
wirelessRemote.assign(0);
ff.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef boost::array< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> , 20> _motorCmd_type;
_motorCmd_type motorCmd;
typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4> _led_type;
_led_type led;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _ff_type;
_ff_type ff;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const> ConstPtr;
}; // struct LowCmd_
typedef ::unitree_legged_msgs::LowCmd_<std::allocator<void> > LowCmd;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd > LowCmdPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd const> LowCmdConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.motorCmd == rhs.motorCmd &&
lhs.led == rhs.led &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.ff == rhs.ff;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "357432b2562edd0a8e89b9c9f5fc4821";
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x357432b2562edd0aULL;
static const uint64_t static_value2 = 0x8e89b9c9f5fc4821ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LowCmd";
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag \n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"MotorCmd[20] motorCmd\n"
"LED[4] led\n"
"uint8[40] wirelessRemote\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"Cartesian[4] ff # will delete # Old version Aliengo does not have\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/MotorCmd\n"
"uint8 mode # motor target mode\n"
"float32 q # motor target position\n"
"float32 dq # motor target velocity\n"
"float32 tau # motor target torque\n"
"float32 Kp # motor spring stiffness coefficient\n"
"float32 Kd # motor damper coefficient\n"
"uint32[3] reserve # motor target torque\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/LED\n"
"uint8 r\n"
"uint8 g\n"
"uint8 b\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.motorCmd);
stream.next(m.led);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.ff);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LowCmd_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "motorCmd[]" << std::endl;
for (size_t i = 0; i < v.motorCmd.size(); ++i)
{
s << indent << " motorCmd[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, indent + " ", v.motorCmd[i]);
}
s << indent << "led[]" << std::endl;
for (size_t i = 0; i < v.led.size(); ++i)
{
s << indent << " led[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + " ", v.led[i]);
}
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "ff[]" << std::endl;
for (size_t i = 0; i < v.ff.size(); ++i)
{
s << indent << " ff[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.ff[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H

448
include/message/unitree_legged_msgs/LowState.h Normal file
View File

@ -0,0 +1,448 @@
// Generated by gencpp from file unitree_legged_msgs/LowState.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
#define UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
#include <unitree_legged_msgs/IMU.h>
#include <unitree_legged_msgs/MotorState.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
#include <unitree_legged_msgs/Cartesian.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct LowState_
{
typedef LowState_<ContainerAllocator> Type;
LowState_()
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, imu()
, motorState()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForceRaw()
, eeForce()
, position()
, velocity()
, velocity_w() {
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
}
LowState_(const ContainerAllocator& _alloc)
: levelFlag(0)
, commVersion(0)
, robotID(0)
, SN(0)
, bandWidth(0)
, imu(_alloc)
, motorState()
, footForce()
, footForceEst()
, tick(0)
, wirelessRemote()
, reserve(0)
, crc(0)
, eeForceRaw()
, eeForce()
, position(_alloc)
, velocity(_alloc)
, velocity_w(_alloc) {
(void)_alloc;
motorState.assign( ::unitree_legged_msgs::MotorState_<ContainerAllocator> (_alloc));
footForce.assign(0);
footForceEst.assign(0);
wirelessRemote.assign(0);
eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
}
typedef uint8_t _levelFlag_type;
_levelFlag_type levelFlag;
typedef uint16_t _commVersion_type;
_commVersion_type commVersion;
typedef uint16_t _robotID_type;
_robotID_type robotID;
typedef uint32_t _SN_type;
_SN_type SN;
typedef uint8_t _bandWidth_type;
_bandWidth_type bandWidth;
typedef ::unitree_legged_msgs::IMU_<ContainerAllocator> _imu_type;
_imu_type imu;
typedef boost::array< ::unitree_legged_msgs::MotorState_<ContainerAllocator> , 20> _motorState_type;
_motorState_type motorState;
typedef boost::array<int16_t, 4> _footForce_type;
_footForce_type footForce;
typedef boost::array<int16_t, 4> _footForceEst_type;
_footForceEst_type footForceEst;
typedef uint32_t _tick_type;
_tick_type tick;
typedef boost::array<uint8_t, 40> _wirelessRemote_type;
_wirelessRemote_type wirelessRemote;
typedef uint32_t _reserve_type;
_reserve_type reserve;
typedef uint32_t _crc_type;
_crc_type crc;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForceRaw_type;
_eeForceRaw_type eeForceRaw;
typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4> _eeForce_type;
_eeForce_type eeForce;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _position_type;
_position_type position;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef ::unitree_legged_msgs::Cartesian_<ContainerAllocator> _velocity_w_type;
_velocity_w_type velocity_w;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> const> ConstPtr;
}; // struct LowState_
typedef ::unitree_legged_msgs::LowState_<std::allocator<void> > LowState;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState > LowStatePtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::LowState const> LowStateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
{
return lhs.levelFlag == rhs.levelFlag &&
lhs.commVersion == rhs.commVersion &&
lhs.robotID == rhs.robotID &&
lhs.SN == rhs.SN &&
lhs.bandWidth == rhs.bandWidth &&
lhs.imu == rhs.imu &&
lhs.motorState == rhs.motorState &&
lhs.footForce == rhs.footForce &&
lhs.footForceEst == rhs.footForceEst &&
lhs.tick == rhs.tick &&
lhs.wirelessRemote == rhs.wirelessRemote &&
lhs.reserve == rhs.reserve &&
lhs.crc == rhs.crc &&
lhs.eeForceRaw == rhs.eeForceRaw &&
lhs.eeForce == rhs.eeForce &&
lhs.position == rhs.position &&
lhs.velocity == rhs.velocity &&
lhs.velocity_w == rhs.velocity_w;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "cef9d4f6b5a89bd6330896affb1bca88";
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xcef9d4f6b5a89bd6ULL;
static const uint64_t static_value2 = 0x330896affb1bca88ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/LowState";
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 levelFlag\n"
"uint16 commVersion # Old version Aliengo does not have\n"
"uint16 robotID # Old version Aliengo does not have\n"
"uint32 SN # Old version Aliengo does not have\n"
"uint8 bandWidth # Old version Aliengo does not have\n"
"IMU imu\n"
"MotorState[20] motorState\n"
"int16[4] footForce # force sensors # Old version Aliengo is different\n"
"int16[4] footForceEst # force sensors # Old version Aliengo does not have\n"
"uint32 tick # reference real-time from motion controller (unit: us)\n"
"uint8[40] wirelessRemote # wireless commands\n"
"uint32 reserve # Old version Aliengo does not have\n"
"uint32 crc\n"
"\n"
"# Old version Aliengo does not have:\n"
"Cartesian[4] eeForceRaw\n"
"Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization \n"
"Cartesian position # will delete\n"
"Cartesian velocity # will delete\n"
"Cartesian velocity_w # will delete\n"
"\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/IMU\n"
"float32[4] quaternion\n"
"float32[3] gyroscope\n"
"float32[3] accelerometer\n"
"int8 temperature\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/MotorState\n"
"uint8 mode # motor current mode \n"
"float32 q # motor current position（rad）\n"
"float32 dq # motor current speed（rad/s）\n"
"float32 ddq # motor current speed（rad/s）\n"
"float32 tauEst # current estimated output torque（N*m）\n"
"float32 q_raw # motor current position（rad）\n"
"float32 dq_raw # motor current speed（rad/s）\n"
"float32 ddq_raw # motor current speed（rad/s）\n"
"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n"
"uint32[2] reserve\n"
"================================================================================\n"
"MSG: unitree_legged_msgs/Cartesian\n"
"float32 x\n"
"float32 y\n"
"float32 z\n"
;
}
static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.levelFlag);
stream.next(m.commVersion);
stream.next(m.robotID);
stream.next(m.SN);
stream.next(m.bandWidth);
stream.next(m.imu);
stream.next(m.motorState);
stream.next(m.footForce);
stream.next(m.footForceEst);
stream.next(m.tick);
stream.next(m.wirelessRemote);
stream.next(m.reserve);
stream.next(m.crc);
stream.next(m.eeForceRaw);
stream.next(m.eeForce);
stream.next(m.position);
stream.next(m.velocity);
stream.next(m.velocity_w);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct LowState_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_<ContainerAllocator>& v)
{
s << indent << "levelFlag: ";
Printer<uint8_t>::stream(s, indent + " ", v.levelFlag);
s << indent << "commVersion: ";
Printer<uint16_t>::stream(s, indent + " ", v.commVersion);
s << indent << "robotID: ";
Printer<uint16_t>::stream(s, indent + " ", v.robotID);
s << indent << "SN: ";
Printer<uint32_t>::stream(s, indent + " ", v.SN);
s << indent << "bandWidth: ";
Printer<uint8_t>::stream(s, indent + " ", v.bandWidth);
s << indent << "imu: ";
s << std::endl;
Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + " ", v.imu);
s << indent << "motorState[]" << std::endl;
for (size_t i = 0; i < v.motorState.size(); ++i)
{
s << indent << " motorState[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, indent + " ", v.motorState[i]);
}
s << indent << "footForce[]" << std::endl;
for (size_t i = 0; i < v.footForce.size(); ++i)
{
s << indent << " footForce[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForce[i]);
}
s << indent << "footForceEst[]" << std::endl;
for (size_t i = 0; i < v.footForceEst.size(); ++i)
{
s << indent << " footForceEst[" << i << "]: ";
Printer<int16_t>::stream(s, indent + " ", v.footForceEst[i]);
}
s << indent << "tick: ";
Printer<uint32_t>::stream(s, indent + " ", v.tick);
s << indent << "wirelessRemote[]" << std::endl;
for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
{
s << indent << " wirelessRemote[" << i << "]: ";
Printer<uint8_t>::stream(s, indent + " ", v.wirelessRemote[i]);
}
s << indent << "reserve: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve);
s << indent << "crc: ";
Printer<uint32_t>::stream(s, indent + " ", v.crc);
s << indent << "eeForceRaw[]" << std::endl;
for (size_t i = 0; i < v.eeForceRaw.size(); ++i)
{
s << indent << " eeForceRaw[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForceRaw[i]);
}
s << indent << "eeForce[]" << std::endl;
for (size_t i = 0; i < v.eeForce.size(); ++i)
{
s << indent << " eeForce[" << i << "]: ";
s << std::endl;
s << indent;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.eeForce[i]);
}
s << indent << "position: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.position);
s << indent << "velocity: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.velocity);
s << indent << "velocity_w: ";
s << std::endl;
Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + " ", v.velocity_w);
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H

261
include/message/unitree_legged_msgs/MotorCmd.h Normal file
View File

@ -0,0 +1,261 @@
// Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct MotorCmd_
{
typedef MotorCmd_<ContainerAllocator> Type;
MotorCmd_()
: mode(0)
, q(0.0)
, dq(0.0)
, tau(0.0)
, Kp(0.0)
, Kd(0.0)
, reserve() {
reserve.assign(0);
}
MotorCmd_(const ContainerAllocator& _alloc)
: mode(0)
, q(0.0)
, dq(0.0)
, tau(0.0)
, Kp(0.0)
, Kd(0.0)
, reserve() {
(void)_alloc;
reserve.assign(0);
}
typedef uint8_t _mode_type;
_mode_type mode;
typedef float _q_type;
_q_type q;
typedef float _dq_type;
_dq_type dq;
typedef float _tau_type;
_tau_type tau;
typedef float _Kp_type;
_Kp_type Kp;
typedef float _Kd_type;
_Kd_type Kd;
typedef boost::array<uint32_t, 3> _reserve_type;
_reserve_type reserve;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const> ConstPtr;
}; // struct MotorCmd_
typedef ::unitree_legged_msgs::MotorCmd_<std::allocator<void> > MotorCmd;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
{
return lhs.mode == rhs.mode &&
lhs.q == rhs.q &&
lhs.dq == rhs.dq &&
lhs.tau == rhs.tau &&
lhs.Kp == rhs.Kp &&
lhs.Kd == rhs.Kd &&
lhs.reserve == rhs.reserve;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
{
static const char* value()
{
return "bbb3b7d91319c3a1b99055f0149ba221";
}
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL;
static const uint64_t static_value2 = 0xb99055f0149ba221ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/MotorCmd";
}
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 mode # motor target mode\n"
"float32 q # motor target position\n"
"float32 dq # motor target velocity\n"
"float32 tau # motor target torque\n"
"float32 Kp # motor spring stiffness coefficient\n"
"float32 Kd # motor damper coefficient\n"
"uint32[3] reserve # motor target torque\n"
;
}
static const char* value(const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.mode);
stream.next(m.q);
stream.next(m.dq);
stream.next(m.tau);
stream.next(m.Kp);
stream.next(m.Kd);
stream.next(m.reserve);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct MotorCmd_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_<ContainerAllocator>& v)
{
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "q: ";
Printer<float>::stream(s, indent + " ", v.q);
s << indent << "dq: ";
Printer<float>::stream(s, indent + " ", v.dq);
s << indent << "tau: ";
Printer<float>::stream(s, indent + " ", v.tau);
s << indent << "Kp: ";
Printer<float>::stream(s, indent + " ", v.Kp);
s << indent << "Kd: ";
Printer<float>::stream(s, indent + " ", v.Kd);
s << indent << "reserve[]" << std::endl;
for (size_t i = 0; i < v.reserve.size(); ++i)
{
s << indent << " reserve[" << i << "]: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H

291
include/message/unitree_legged_msgs/MotorState.h Normal file
View File

@ -0,0 +1,291 @@
// Generated by gencpp from file unitree_legged_msgs/MotorState.msg
// DO NOT EDIT!
#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H
#include <string>
#include <vector>
#include <map>
#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>
namespace unitree_legged_msgs
{
template <class ContainerAllocator>
struct MotorState_
{
typedef MotorState_<ContainerAllocator> Type;
MotorState_()
: mode(0)
, q(0.0)
, dq(0.0)
, ddq(0.0)
, tauEst(0.0)
, q_raw(0.0)
, dq_raw(0.0)
, ddq_raw(0.0)
, temperature(0)
, reserve() {
reserve.assign(0);
}
MotorState_(const ContainerAllocator& _alloc)
: mode(0)
, q(0.0)
, dq(0.0)
, ddq(0.0)
, tauEst(0.0)
, q_raw(0.0)
, dq_raw(0.0)
, ddq_raw(0.0)
, temperature(0)
, reserve() {
(void)_alloc;
reserve.assign(0);
}
typedef uint8_t _mode_type;
_mode_type mode;
typedef float _q_type;
_q_type q;
typedef float _dq_type;
_dq_type dq;
typedef float _ddq_type;
_ddq_type ddq;
typedef float _tauEst_type;
_tauEst_type tauEst;
typedef float _q_raw_type;
_q_raw_type q_raw;
typedef float _dq_raw_type;
_dq_raw_type dq_raw;
typedef float _ddq_raw_type;
_ddq_raw_type ddq_raw;
typedef int8_t _temperature_type;
_temperature_type temperature;
typedef boost::array<uint32_t, 2> _reserve_type;
_reserve_type reserve;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> > Ptr;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const> ConstPtr;
}; // struct MotorState_
typedef ::unitree_legged_msgs::MotorState_<std::allocator<void> > MotorState;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr;
typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr;
// constants requiring out of line definition
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, "", v);
return s;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
{
return lhs.mode == rhs.mode &&
lhs.q == rhs.q &&
lhs.dq == rhs.dq &&
lhs.ddq == rhs.ddq &&
lhs.tauEst == rhs.tauEst &&
lhs.q_raw == rhs.q_raw &&
lhs.dq_raw == rhs.dq_raw &&
lhs.ddq_raw == rhs.ddq_raw &&
lhs.temperature == rhs.temperature &&
lhs.reserve == rhs.reserve;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::unitree_legged_msgs::MotorState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::MotorState_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace unitree_legged_msgs
namespace ros
{
namespace message_traits
{
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsFixedSize< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
: TrueType
{ };
template <class ContainerAllocator>
struct IsMessage< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
: TrueType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
: FalseType
{ };
template <class ContainerAllocator>
struct HasHeader< ::unitree_legged_msgs::MotorState_<ContainerAllocator> const>
: FalseType
{ };
template<class ContainerAllocator>
struct MD5Sum< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
{
static const char* value()
{
return "94c55ee3b7852be2bd437b20ce12a254";
}
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL;
static const uint64_t static_value2 = 0xbd437b20ce12a254ULL;
};
template<class ContainerAllocator>
struct DataType< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
{
static const char* value()
{
return "unitree_legged_msgs/MotorState";
}
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
};
template<class ContainerAllocator>
struct Definition< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
{
static const char* value()
{
return "uint8 mode # motor current mode \n"
"float32 q # motor current position（rad）\n"
"float32 dq # motor current speed（rad/s）\n"
"float32 ddq # motor current speed（rad/s）\n"
"float32 tauEst # current estimated output torque（N*m）\n"
"float32 q_raw # motor current position（rad）\n"
"float32 dq_raw # motor current speed（rad/s）\n"
"float32 ddq_raw # motor current speed（rad/s）\n"
"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n"
"uint32[2] reserve\n"
;
}
static const char* value(const ::unitree_legged_msgs::MotorState_<ContainerAllocator>&) { return value(); }
};
} // namespace message_traits
} // namespace ros
namespace ros
{
namespace serialization
{
template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
{
template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
{
stream.next(m.mode);
stream.next(m.q);
stream.next(m.dq);
stream.next(m.ddq);
stream.next(m.tauEst);
stream.next(m.q_raw);
stream.next(m.dq_raw);
stream.next(m.ddq_raw);
stream.next(m.temperature);
stream.next(m.reserve);
}
ROS_DECLARE_ALLINONE_SERIALIZER
}; // struct MotorState_
} // namespace serialization
} // namespace ros
namespace ros
{
namespace message_operations
{
template<class ContainerAllocator>
struct Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >
{
template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_<ContainerAllocator>& v)
{
s << indent << "mode: ";
Printer<uint8_t>::stream(s, indent + " ", v.mode);
s << indent << "q: ";
Printer<float>::stream(s, indent + " ", v.q);
s << indent << "dq: ";
Printer<float>::stream(s, indent + " ", v.dq);
s << indent << "ddq: ";
Printer<float>::stream(s, indent + " ", v.ddq);
s << indent << "tauEst: ";
Printer<float>::stream(s, indent + " ", v.tauEst);
s << indent << "q_raw: ";
Printer<float>::stream(s, indent + " ", v.q_raw);
s << indent << "dq_raw: ";
Printer<float>::stream(s, indent + " ", v.dq_raw);
s << indent << "ddq_raw: ";
Printer<float>::stream(s, indent + " ", v.ddq_raw);
s << indent << "temperature: ";
Printer<int8_t>::stream(s, indent + " ", v.temperature);
s << indent << "reserve[]" << std::endl;
for (size_t i = 0; i < v.reserve.size(); ++i)
{
s << indent << " reserve[" << i << "]: ";
Printer<uint32_t>::stream(s, indent + " ", v.reserve[i]);
}
}
};
} // namespace message_operations
} // namespace ros
#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H

114
include/model/ArmDynKineModel.h Executable file
View File

@ -0,0 +1,114 @@
#ifndef ARMDYNCKINEMODEL_H
#define ARMDYNCKINEMODEL_H
#include <rbdl/rbdl.h>
#include <vector>
#include "common/math/mathTypes.h"
#include "quadProgpp/include/QuadProg++.hh"
/* Kinematics only suitable for 6 DOF arm */
class ArmDynKineModel{
public:
ArmDynKineModel(int dof, Vec3 endPosLocal,
double endEffectorMass,
Vec3 endEffectorCom, Mat3 endEffectorInertia);
virtual ~ArmDynKineModel();
Vec6 forDerivativeKinematice(Vec6 q, Vec6 qd);
Vec6 invDerivativeKinematice(Vec6 q, Vec6 twistSpatial);
Vec6 invDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 endForce);
int getDOF(){return _dof;}
std::vector<double> getJointQMax(){return _jointQMax;}
std::vector<double> getJointQMin(){return _jointQMin;}
std::vector<double> getJointSpeedMax(){return _jointSpeedMax;}
void solveQP(Vec6 twist, Vec6 pastAngle, Vec6 &result, double dt, bool setJ0QdZero, bool setQdZero);
bool invKinematics(HomoMat gDes, Vec6 pastAngle, Vec6 &result, bool checkInWorkSpace = false);
HomoMat forwardKinematics(Vec6 q, int index=6); // index from 0 to 5, used for fourth version
bool _useIKSolver = false;
protected:
void _dofCheck();
void _buildModel();
int _dof;
RigidBodyDynamics::Model *_model;
std::vector<unsigned int> _linkID;
std::vector<RigidBodyDynamics::Body> _body;
std::vector<double> _linkMass;
std::vector<Vec3> _linkCom;
std::vector<RigidBodyDynamics::Math::Matrix3d> _linkInertia;
double _endEffectorMass;
Vec3 _endEffectorCom;
Mat3 _endEffectorInertia;
std::vector<RigidBodyDynamics::Joint> _joint;
std::vector<Vec3> _jointAxis;
std::vector<Vec3> _jointPos;
std::vector<double> _jointQMax;
std::vector<double> _jointQMin;
std::vector<double> _jointSpeedMax;
Vec3 _endPosGlobal, _endPosLocal; // based on mount pos
Vec3 _endMountPosGlobal, _endMountPosLocal;
std::vector<Vec6> _theta;
/* inverse derivative kinematics */
RigidBodyDynamics::Math::MatrixNd _Jacobian;
/* inverse dynamics */
std::vector<RigidBodyDynamics::Math::SpatialVector> _endForceVec;
/* inverse kinematics */
bool _checkAngle(Vec6 angle); // check all angles once
Vec6 JointQMax;
Vec6 JointQMin;
Vec6 JointQdMax;
Vec6 JointQdMin;
double theta0[2], theta1[4], theta2[4], theta3[4], theta4[4], theta5[4];
double theta2Bias;
std::vector<Vec3> _linkPosLocal;
std::vector<double> _disJoint;
double _disP13;
Vec3 _vecP45;
Vec3 _vecP14;
Vec3 _vecP13;
Vec3 _vecP34;
Vec3 _joint4Axis;
Vec3 _joint5Axis;
Vec3 _plane324X, _plane324Z;
double _vec34ProjX, _vec34ProjZ;
Vec3 _plane1234Y, _plane1234_X;
Vec3 _vecP04ProjXY;
double _angle213, _angle123, _angle132;
double _angle_x13;
HomoMat _T46;
/* quadprog */
quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x;
Mat6 _G;
Vec6 _g0;
VecX _ci0;
MatX _CI;
};
class Z1DynKineModel : public ArmDynKineModel{
public:
Z1DynKineModel(Vec3 endPosLocal = Vec3::Zero(),
double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(),
Mat3 endEffectorInertia = Mat3::Zero());
~Z1DynKineModel(){}
};
#endif // ARMDYNCKINEMODEL_H

82
include/model/ArmReal.h Executable file
View File

@ -0,0 +1,82 @@
#ifndef ARM_H
#define ARM_H
#include <vector>
#include "common/math/mathTypes.h"
#include "model/Joint.h"
#include "model/Motor.h"
#include "unitree_arm_sdk/udp.h"
class ArmReal{
public:
ArmReal(int dof, int motorNum, IOPort *ioPort);
virtual ~ArmReal();
void setJointKp(std::vector<double> jointKp);
void setJointKd(std::vector<double> jointKd);
void setJointQ(std::vector<double> jointQ);
void setJointDq(std::vector<double> jointDq);
void setJointTau(std::vector<double> jointTau);
void getJointQ(std::vector<double> &jointQState);
void getJointDq(std::vector<double> &jointDqState);
void getJointDDq(std::vector<double> &jointDDqState);
void getJointTau(std::vector<double> &jointTauState);
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorTau(std::vector<double> &motorTau);
void armCalibration();
void armCalibration(std::vector<double> motorAngleBias);
bool armComm();
size_t getDof(){return _dof;}
size_t getMotorNum(){return _motorNum;}
// int getMotorNum(){return _motorNum;}
protected:
void _init();
void _getCmd(std::vector<MOTOR_send> &cmd);
void _setState(std::vector<MOTOR_recv> &motorState);
void _getMotorQBias();
// void _setComm();
void _setCaliBias();
int _motorNum;
int _dof;
int _commReturn;
bool _commYN, _motorCommYN;
std::vector<Joint*> _joints;
std::vector<double> _jointCaliAngle;
std::vector<double> _motorAngleBias;
std::vector<MOTOR_send> _motorCmd;
std::vector<MOTOR_recv> _motorState;
/* serial or UDP Communication */
IOPort *_ioPort;
};
class z1Real : public ArmReal{
public:
z1Real(IOPort *ioPort);
~z1Real(){}
};
class TigerHeadReal : public ArmReal{
public:
TigerHeadReal(IOPort *ioPort);
~TigerHeadReal(){}
};
class DogHeadReal : public ArmReal{
public:
DogHeadReal(IOPort *ioPort);
~DogHeadReal(){}
};
class TestSerial : public ArmReal{
public:
TestSerial(IOPort *ioPort);
~TestSerial(){}
};
#endif // ARM_H

55
include/model/Joint.h Executable file
View File

@ -0,0 +1,55 @@
#ifndef JOINT_H
#define JOINT_H
#include <vector>
#include "model/Motor.h"
#include "common/enumClass.h"
class Joint{
public:
Joint(){}
virtual ~Joint(){}
void setJointKp (double jointKp);
void setJointKd (double jointKd);
void setJointQ (double jointQ);
void setJointDq (double jointDq);
void setJointTau(double jointTau);
double getJointQ();
double getJointDq();
double getJointDDq();
double getJointTau();
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorDDq(std::vector<double> &motorDDq);
void getMotorTau(std::vector<double> &motorTau);
void getCmd(std::vector<MOTOR_send> &cmd);
void setState(std::vector<MOTOR_recv> &state);
void setCaliBias(double cali, std::vector<double> bias);
protected:
JointMotorType _motorType;
int _motorNum;
double _jointInitAngle;
std::vector<Motor*> _motor;
};
class Jointz1 : public Joint{
public:
Jointz1(int motorID, double direction);
Jointz1(int motorID0, int motorID1, double direction0, double direction1);
Jointz1(int motorID, int motorMsgOrder, double direction);
Jointz1(int motorID0, int motorID1, int motorMsgOrder0, int motorMsgOrder1, double direction0, double direction1);
~Jointz1(){}
};
class JointTigerHead : public Joint{
public:
JointTigerHead(int motorID, double direction);
JointTigerHead(int motorID, int motorMsgOrder, double direction);
~JointTigerHead(){}
};
#endif // JOINT_H

56
include/model/Motor.h Executable file
View File

@ -0,0 +1,56 @@
#ifndef MOTOR_H
#define MOTOR_H
#include <vector>
#include "common/enumClass.h"
#include "unitree_arm_sdk/common/arm_motor_common.h"
class Motor{
public:
Motor(int id, int motorMsgOrder, double reductionRatio, MotorMountType type, double direction);
virtual ~Motor(){}
void setMotorKp (double jointKp);
void setMotorKd (double jointKd);
void setMotorQ (double jointQ);
void setMotorDq (double jointDq);
void setMotorTau(double jointTau);
double getJointQ(){return _qJoint;}
double getJointDq(){return _dqJoint;}
double getJointDDq(){return _ddqJoint;}
double getJointTau(){return _tauJoint;}
void getMotorQ(std::vector<double> &motorQ);
void getMotorDq(std::vector<double> &motorDq);
void getMotorDDq(std::vector<double> &motorDDq);
void getMotorTau(std::vector<double> &motorTau);
void setQBias(std::vector<double> qBias);
void getCmd(std::vector<MOTOR_send> &cmd);
void setState(std::vector<MOTOR_recv> &state);
protected:
int _id;
int _motorMsgOrder;
double _kp, _kd, _q, _dq, _tau;
double _qJoint, _dqJoint, _ddqJoint, _tauJoint;
double _qMotor, _dqMotor, _ddqMotor, _tauMotor;
double _reductionRatio;
double _directon;
double _qBias;
MotorMountType _type;
};
class Motorz1 : public Motor{
public:
Motorz1(int id, int motorMsgOrder, MotorMountType type, double direction);
~Motorz1(){}
};
class MotorGo1 : public Motor{
public:
MotorGo1(int id, int motorMsgOrder, MotorMountType type, double direction);
~MotorGo1(){}
};
#endif // MOTOR_H

45
include/trajectory/CartesionSpaceTraj.h Executable file
View File

@ -0,0 +1,45 @@
#ifndef CartesionSpaceTraj_H
#define CartesionSpaceTraj_H
#include <vector>
#include <trajectory/Trajectory.h>
#include <string>
#include "control/CtrlComponents.h"
class CartesionSpaceTraj : public Trajectory{
public:
CartesionSpaceTraj(CtrlComponents *ctrlComp);
CartesionSpaceTraj(ArmDynKineModel *armModel);
CartesionSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
~CartesionSpaceTraj(){}
void setCartesionTraj(Vec6 startP, Vec6 endP, double oriSpeed, double posSpeed);
void setCartesionTraj(Vec6 startP, Vec6 middleP, double middleS, Vec6 endP, double oriSpeed, double posSpeed);
bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
void setCartesionTrajMoveC(Vec6 startP, Vec6 middleP, Vec6 endP, double oriSpeed, double posSpeed);
bool getCartesionCmdMoveC(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
double _radius;
private:
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
bool _checkPostureValid(Vec6 endPosture, int pointOrder);
void _generateA345();
double _pathTimeTemp; // path total time
double _s, _sDot; // [0, 1]
double _a3, _a4, _a5; // parameters of path
int _trajOrder; // The order of trajectory curve
std::vector<Vec6> _curveParam;
/* MoveC instruct */
Vec3 _center;
double _theta;
Vec3 _axis_x;
Vec3 _axis_y;
Vec3 _axis_z;
};
#endif // JOINTSPACETRAJ_H

32
include/trajectory/EndCircleTraj.h Executable file
View File

@ -0,0 +1,32 @@
#ifndef ENDCIRCLETRAJ_H
#define ENDCIRCLETRAJ_H
#include "model/ArmDynKineModel.h"
#include "trajectory/EndHomoTraj.h"
class EndCircleTraj: public EndHomoTraj{
public:
EndCircleTraj(CtrlComponents *ctrlComp);
EndCircleTraj(ArmDynKineModel *armModel);
~EndCircleTraj(){}
void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit,
Vec3 axisDirection, double maxSpeed, double angle,
bool keepOrientation = true);
void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit,
Vec3 axisDirection, double maxSpeed, double angle,
bool keepOrientation = true);
private:
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
Vec3 _center;
double _radius;
HomoMat _centerHomo;
HomoMat _initHomoToCenter;
RotMat _initOri;
double _maxSpeed, _goalAngle, _speed, _angle;
Vec3 _omegaAxis;
bool _keepOrientation;
};
#endif // ENDCIRCLETRAJ_H

28
include/trajectory/EndHomoTraj.h Executable file
View File

@ -0,0 +1,28 @@
#ifndef ENDHOMOTRAJ_H
#define ENDHOMOTRAJ_H
#include "model/ArmDynKineModel.h"
#include "trajectory/Trajectory.h"
#include "trajectory/SCurve.h"
class EndHomoTraj : public Trajectory{
public:
EndHomoTraj(CtrlComponents *ctrlComp);
EndHomoTraj(ArmDynKineModel *armModel);
virtual ~EndHomoTraj();
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
protected:
virtual bool _getEndTraj(HomoMat &homo, Vec6 &twist) = 0;
HomoMat _cmdHomo;
Vec6 _cmdTwist;
Vec6 _qPast;
// double _currentTime;
SCurve *_sCurve;
};
#endif // ENDHOMOTRAJ_H

30
include/trajectory/EndLineTraj.h Executable file
View File

@ -0,0 +1,30 @@
#ifndef ENDLINETRAJ_H
#define ENDLINETRAJ_H
#include "trajectory/EndHomoTraj.h"
#include "trajectory/SCurve.h"
class EndLineTraj : public EndHomoTraj{
public:
EndLineTraj(CtrlComponents *ctrlComp);
EndLineTraj(ArmDynKineModel *armModel);
~EndLineTraj(){}
void setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
private:
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
// SCurve _sCurves[2]; // 0: position, 1: orientation
Vec3 _movingAxis, _turningAxis;
double _movingDistance, _turningAngle;
Vec3 _currentDistance, _currentAngle;
Vec3 _currentVelocity, _currentOmega;
SCurve _posCurve;
SCurve _oriCurve;
};
#endif // ENDLINETRAJ_H

45
include/trajectory/JointSpaceTraj.h Executable file
View File

@ -0,0 +1,45 @@
#ifndef JOINTSPACETRAJ_H
#define JOINTSPACETRAJ_H
#include <vector>
#include <trajectory/Trajectory.h>
#include <string>
#include "control/CtrlComponents.h"
class JointSpaceTraj : public Trajectory{
public:
JointSpaceTraj(CtrlComponents *ctrlComp);
JointSpaceTraj(ArmDynKineModel *armModel);
JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
~JointSpaceTraj(){}
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
void setJointTraj(Vec6 startQ, Vec6 endQ, double speed);
void setJointTraj(Vec6 startQ, std::string endName, double speed);
void setJointTraj(std::string startName, std::string endName, double speed);
void setJointTraj(Vec6 startQ, Vec6 middleQ, double middleS, Vec6 endQ, double speed);
void setJointTraj(std::string startName, std::string middleName, double middleS, std::string endName, double speed);
void setJointTraj(std::vector<Vec6> stateQ, std::vector<double> stateS, double pathTime);
void setJointTraj(std::vector<std::string> stateName, std::vector<double> stateS, double pathTime);
private:
void _checkAngleValid(const Vec6 &q, int pointOrder);
bool _checkJointAngleValid(const double &q, int jointOrder);
void _generateA345();
int _jointNum;
double _pathTimeTemp; // path total time
double _s, _sDot; // [0, 1]
double _a3, _a4, _a5; // parameters of path
// double _speedFactor; // [0, 1]
std::vector<double> _jointMaxQ;
std::vector<double> _jointMinQ;
std::vector<double> _jointMaxSpeed;
int _trajOrder; // The order of trajectory curve
std::vector<Vec6> _curveParam;
};
#endif // JOINTSPACETRAJ_H

50
include/trajectory/SCurve.h Executable file
View File

@ -0,0 +1,50 @@
#ifndef SCURVE_H
#define SCURVE_H
/* 归一化后的s曲线 */
#include <math.h>
#include <iostream>
class SCurve{
public:
SCurve(){}
~SCurve(){}
void setSCurve(double deltaQ, double dQMax, double ddQMax, double dddQMax);
void restart();
/*
考虑到归一化，在实际使用时，物理量为：
加速度 = deltaQ * getDDs();
*/
double getDDs();
double getDDs(double t);
/*
考虑到归一化，在实际使用时，物理量为：
速度 = deltaQ * getDs();
*/
double getDs();
double getDs(double t);
/*
考虑到归一化，在实际使用时，物理量为：
位置 = deltaQ * gets();
*/
double gets();
double gets(double t);
double getT();
private:
int _getSegment(double t);
void _setFunc();
double _runTime();
bool _started = false;
double _startTime;
double _J, _aMax, _vMax;
double _T[7]; // period
double _t[7]; // moment
double _v0, _v1; // ds at _t[0], _t[1]
double _s0, _s1, _s2, _s3, _s4, _s5, _s6; // s at _t[0], _t[1]
};
#endif // SCURVE_H

24
include/trajectory/StopForTime.h Executable file
View File

@ -0,0 +1,24 @@
#ifndef STOPFORTIME_H
#define STOPFORTIME_H
#include "trajectory/Trajectory.h"
class StopForTime : public Trajectory{
public:
StopForTime(CtrlComponents *ctrlComp);
StopForTime(ArmDynKineModel *armModel);
StopForTime(ArmDynKineModel *armModel, CSVTool *csvState);
~StopForTime(){}
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
void setStop(Vec6 stopQ, double stopTime);
void setStop(std::string stateName, double stopTime);
private:
// Vec6 _stopQ;
// double _stopTime;
};
#endif // STOPFORTIME_H

100
include/trajectory/Trajectory.h Executable file
View File

@ -0,0 +1,100 @@
#ifndef TRAJECTORY_H
#define TRAJECTORY_H
#include "common/math/mathTypes.h"
#include "control/CtrlComponents.h"
#include "unitree_arm_sdk/timer.h"
class Trajectory{
public:
Trajectory(CtrlComponents *ctrlComp){
_ctrlComp = ctrlComp;
_armModel = ctrlComp->armModel;
_csvState = ctrlComp->stateCSV;
_dof = _armModel->getDOF();
_hasKinematic = true;
}
Trajectory(ArmDynKineModel *armModel){
_ctrlComp = nullptr;
_armModel = armModel;
_csvState = nullptr;
_dof = armModel->getDOF();
if(_dof == 6){
_hasKinematic = true;
}
}
Trajectory(ArmDynKineModel *armModel, CSVTool *csvState):Trajectory(armModel){
_csvState = csvState;
}
virtual ~Trajectory(){}
virtual bool getJointCmd(Vec6 &q, Vec6 &qd){};
virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){};
virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){};
void restart(){
_pathStarted = false;
_reached = false;
}
void setGripper(double startQ, double endQ){
_gripperStartQ = startQ;
_gripperEndQ = endQ;
}
bool correctYN(){return _settingCorrect;}
Vec6 getStartQ(){return _startQ;}
Vec6 getEndQ(){return _endQ;}
HomoMat getStartHomo(){
if(_hasKinematic){
return _startHomo;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
HomoMat getEndHomo(){
if(_hasKinematic){
return _endHomo;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
double getPathTime(){return _pathTime;}
protected:
CtrlComponents *_ctrlComp;
ArmDynKineModel *_armModel;
CSVTool *_csvState;
bool _pathStarted = false;
bool _reached = false;
bool _paused = false;
bool _settingCorrect = true;
double _startTime;
double _currentTime;
double _pathTime;
double _tCost;
Vec6 _startQ, _endQ;
HomoMat _startHomo, _endHomo;
double _gripperStartQ;
double _gripperEndQ;
size_t _dof;
bool _hasKinematic;
void _runTime(){
_currentTime = getTimeSecond();
if(!_pathStarted){
_pathStarted = true;
_startTime = _currentTime;
_tCost = 0;
}
_tCost = _currentTime - _startTime;
_reached = (_tCost>_pathTime) ? true : false;
_tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
}
};
#endif // TRAJECTORY_H

37
include/trajectory/TrajectoryManager.h Executable file
View File

@ -0,0 +1,37 @@
#ifndef TRAJECTORY_MANAGER_H
#define TRAJECTORY_MANAGER_H
#include <vector>
#include "control/CtrlComponents.h"
#include "trajectory/JointSpaceTraj.h"
#include "trajectory/EndLineTraj.h"
#include "trajectory/EndCircleTraj.h"
#include "trajectory/StopForTime.h"
class TrajectoryManager{
public:
TrajectoryManager(CtrlComponents *ctrlComp);
TrajectoryManager(ArmDynKineModel *armModel);
TrajectoryManager(ArmDynKineModel *armModel, CSVTool *csvState);
~TrajectoryManager(){}
bool getJointCmd(Vec6 &q, Vec6 &qd);
bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
void addTrajectory(Trajectory* traj);
void setLoop(double backSpeed = 0.7);
void restartTraj();
void emptyTraj();
Vec6 getStartQ();
Vec6 getEndQ();
HomoMat getEndHomo();
// bool checkTrajectoryContinuous();
private:
CtrlComponents *_ctrlComp;
JointSpaceTraj *_trajBack;
std::vector<Trajectory*> _trajVec;
int _trajID = 0;
double _jointErr = 0.05;
bool _trajCorrect = true;
bool _loop = false;
};
#endif // TRAJECTORY_MANAGER_H

BIN
lib/libUnitree_motor_SDK_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libUnitree_motor_SDK_Linux64_EPOLL.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_JOYSTICK_ROS_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_JOYSTICK_UDP_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_KEYBOARD_ROS_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_KEYBOARD_UDP_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_SDK_ROS_Linux64.so Executable file
View File

Binary file not shown.

BIN
lib/libZ1_SDK_UDP_Linux64.so Executable file
View File

Binary file not shown.

249
main.cpp Executable file
View File

@ -0,0 +1,249 @@
#include <iostream>
#include <unistd.h>
#include <csignal>
#include <sched.h>
#include <iomanip>
#include "control/CtrlComponents.h"
#include "model/ArmReal.h"
#include "model/ArmDynKineModel.h"
#include "interface/IOUDP.h"
#include "FSM/State_BackToStart.h"
#include "FSM/State_Cartesian.h"
#include "FSM/State_MoveJ.h"
#include "FSM/State_MoveL.h"
#include "FSM/State_MoveC.h"
#include "FSM/State_Dance.h"
#include "FSM/State_JointSpace.h"
#include "FSM/State_Passive.h"
#include "FSM/State_SaveState.h"
#include "FSM/State_Teach.h"
#include "FSM/State_TeachRepeat.h"
#include "FSM/State_ToState.h"
#include "FSM/State_Trajectory.h"
#include "FSM/State_Calibration.h"
#include "FSM/State_LowCmd.h"
#include "FSM/FiniteStateMachine.h"
#include "unitree_arm_sdk/unitree_arm_sdk.h"
bool running = true;
// over watch the ctrl+c command
void ShutDown(int sig){
std::cout << "[STATE] stop the controller" << std::endl;
running = false;
}
void setProcessScheduler(){
pid_t pid = getpid();
sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
if (sched_setscheduler(pid, SCHED_FIFO, &param) == -1) {
std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
}
}
int main(int argc, char **argv){
/* set real-time process */
setProcessScheduler();
/* set the print format */
std::cout << std::fixed << std::setprecision(3);
EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
std::vector<KeyAction*> events;
CmdPanel *cmdPanel;
#ifdef CTRL_BY_SDK
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD));
events.push_back(new ValueAction("q", "a", 1.0));
events.push_back(new ValueAction("w", "s", 1.0));
events.push_back(new ValueAction("e", "d", 1.0));
events.push_back(new ValueAction("r", "f", 1.0));
events.push_back(new ValueAction("t", "g", 1.0));
events.push_back(new ValueAction("y", "h", 1.0));
events.push_back(new ValueAction("down", "up", 1.0));
cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction);
#endif
#ifdef CTRL_BY_KEYBOARD
events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
events.push_back(new ValueAction("q", "a", 1.0));
events.push_back(new ValueAction("w", "s", 1.0));
events.push_back(new ValueAction("e", "d", 1.0));
events.push_back(new ValueAction("r", "f", 1.0));
events.push_back(new ValueAction("t", "g", 1.0));
events.push_back(new ValueAction("y", "h", 1.0));
events.push_back(new ValueAction("down", "up", 1.0));
cmdPanel = new Keyboard(events, emptyAction);
#endif
#ifdef CTRL_BY_JOYSTICK
events.push_back(new StateAction("r12_passive", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("r2_backToState", (int)ArmFSMStateName::BACKTOSTART));
events.push_back(new StateAction("r1_cartesian", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("select", (int)ArmFSMStateName::NEXT));
events.push_back(new ValueAction("ly_forTranY", "ly_invTranY", 1.0));
events.push_back(new ValueAction("lx_forTranX", "lx_invTranX", 1.0));
events.push_back(new ValueAction("up_forTranZ", "down_invTranZ", 1.0));
events.push_back(new ValueAction("ry_forRotY", "ry_invRotY", 1.0));
events.push_back(new ValueAction("rx_forRotX", "rx_invRotX", 1.0));
events.push_back(new ValueAction("Y_forRotZ", "A_invRotZ", 1.0));
events.push_back(new ValueAction("right_gClose", "left_gOpen", 1.0));
// events.push_back(new ValueAction("X_velDown", "B_velUp", 1.0));
cmdPanel = new UnitreeJoystick(events, emptyAction);
#endif
#ifdef RUN_ROS
ros::init(argc, argv, "z1_controller");
#endif // RUN_ROS
CtrlComponents *ctrlComp = new CtrlComponents();
ctrlComp->dof = 6;
ctrlComp->armConfigPath = "../config/";
ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
ctrlComp->running = &running;
#ifdef UDP
ctrlComp->ioInter = new IOUDP(cmdPanel);
ctrlComp->dt = 0.004;
#endif
#ifdef ROS
ctrlComp->ioInter = new IOROS(cmdPanel);
ctrlComp->dt = 0.004;
#endif
#ifdef COMPILE_DEBUG
std::cout << "add plot" << std::endl;
ctrlComp->plot = new PyPlot();
/* for general debugging */
// ctrlComp->plot->addPlot("qCmd", 6);
// ctrlComp->plot->addPlot("qdCmd", 6);
// ctrlComp->plot->addPlot("tauCmd", 6);
// ctrlComp->plot->addPlot("qReal", 6);
// ctrlComp->plot->addPlot("qdReal", 6);
// ctrlComp->plot->addPlot("realTau", 6);
// ctrlComp->plot->addPlot("errorTau", 6);
// ctrlComp->plot->addPlot("error", 1);
// ctrlComp->plot->addPlot("costTime", 1);
/* for MoveJ, MoveL, MoveC*/
// ctrlComp->plot->addPlot("moveLPostureDes", 6);
// ctrlComp->plot->addPlot("moveLTwistDes", 6);
// ctrlComp->plot->addPlot("moveLPostureAct", 6);
// ctrlComp->plot->addPlot("moveLTwistAct", 6);
// ctrlComp->plot->addPlot("moveLPostureError", 6);
// ctrlComp->plot->addPlot("moveLTwistError", 6);
// /* for cartesian */
// ctrlComp->plot->addPlot("cartesianPostureDes", 6);
// ctrlComp->plot->addPlot("cartesianTwistDes", 6);
// ctrlComp->plot->addPlot("cartesianPostureAct", 6);
// ctrlComp->plot->addPlot("cartesianTwistAct", 6);
// ctrlComp->plot->addPlot("cartesianPostureError", 6);
// ctrlComp->plot->addPlot("cartesianTwistError", 6);
/* for trajectory */
// plot->addPlot("s", 1);
// plot->addPlot("qPath", 6);
// plot->addPlot("qdPath", 6);
// plot->addPlot("qdPathDiff", 6);
// ctrlComp->plot->addPlot("qState", 6);
// ctrlComp->plot->addPlot("qError", 6);
/* for gripper */
// ctrlComp->plot->addPlot("gripper q error", 1);
// ctrlComp->plot->addPlot("gripper qd error", 1);
// ctrlComp->plot->addPlot("gripper tau", 1);
#endif // COMPILE_DEBUG
ctrlComp->geneObj();
std::vector<BaseState*> states;
states.push_back(new State_Passive(ctrlComp)); // First state in states is the begining state for FSM
states.push_back(new State_BackToStart(ctrlComp));
states.push_back(new State_JointSpace(ctrlComp));
states.push_back(new State_Cartesian(ctrlComp));
states.push_back(new State_MoveJ(ctrlComp));
states.push_back(new State_MoveL(ctrlComp));
states.push_back(new State_MoveC(ctrlComp));
states.push_back(new State_LowCmd(ctrlComp));
states.push_back(new State_SaveState(ctrlComp));
states.push_back(new State_Teach(ctrlComp));
states.push_back(new State_TeachRepeat(ctrlComp));
states.push_back(new State_ToState(ctrlComp));
states.push_back(new State_Trajectory(ctrlComp));
states.push_back(new State_Calibration(ctrlComp));
std::vector<TrajectoryManager*> _trajMag;
std::vector<JointSpaceTraj*> _jointTraj;
/* start to forward */
_trajMag.push_back(new TrajectoryManager(ctrlComp));
_jointTraj.push_back(new JointSpaceTraj(ctrlComp));
_jointTraj.at(_jointTraj.size()-1)->setJointTraj("startFlat", "forward", 0.5);
_trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1));
/* forward to start */
_trajMag.push_back(new TrajectoryManager(ctrlComp));
_jointTraj.push_back(new JointSpaceTraj(ctrlComp));
_jointTraj.at(_jointTraj.size()-1)->setJointTraj("forward", "startFlat", 0.5);
_trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1));
states.push_back(new State_Dance(ctrlComp, _trajMag.at(0), ArmFSMStateName::DANCE00, ArmFSMStateName::DANCE01, "dance 00"));
states.push_back(new State_Dance(ctrlComp, _trajMag.at(1), ArmFSMStateName::DANCE01, ArmFSMStateName::TRAJECTORY, "dance 01"));
// #endif
FiniteStateMachine fsm(states, cmdPanel, 0, ctrlComp->dt);
signal(SIGINT, ShutDown);
// std::vector<double> values;
while(running){
// values = cmdPanel->getValues();
// std::cout << "value: " << values.at(0) << std::endl;
usleep(100000);
}
#ifdef COMPILE_DEBUG
std::cout << "show plot" << std::endl;
ctrlComp->plot->showPlotAll();
#endif // COMPILE_DEBUG
delete ctrlComp;
return 0;
}

25
package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package format="2">
<name>z1_controller</name>
<version>0.0.0</version>
<description>The z1_controller package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<build_export_depend>roscp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>urdf</build_export_depend>
<build_export_depend>xacro</build_export_depend>
<exec_depend>roscp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
</package>

BIN
thirdparty/eigen-3.3.9.zip vendored Normal file
View File

Binary file not shown.

2532
thirdparty/quadProgpp/include/Array.hh vendored Executable file
View File

File diff suppressed because it is too large Load Diff

77
thirdparty/quadProgpp/include/QuadProg++.hh vendored Executable file
View File

@ -0,0 +1,77 @@
/*
File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
for the solution of a (convex) Quadratic Programming problem
by means of an active-set dual method.
The problem is in the form:
min 0.5 * x G x + g0 x
s.t.
CE^T x + ce0 = 0
CI^T x + ci0 >= 0
The matrix and vectors dimensions are as follows:
G: n * n
g0: n
CE: n * p
ce0: p
CI: n * m
ci0: m
x: n
The function will return the cost of the solution written in the x vector or
std::numeric_limits::infinity() if the problem is infeasible. In the latter case
the value of the x vector is not correct.
References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
Notes:
1. pay attention in setting up the vectors ce0 and ci0.
If the constraints of your problem are specified in the form
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
2. The matrices have column dimension equal to MATRIX_DIM,
a constant set to 20 in this file (by means of a #define macro).
If the matrices are bigger than 20 x 20 the limit could be
increased by means of a -DMATRIX_DIM=n on the compiler command line.
3. The matrix G is modified within the function since it is used to compute
the G = L^T L cholesky factorization for further computations inside the function.
If you need the original matrix G you should make a copy of it and pass the copy
to the function.
Author: Luca Di Gaspero
DIEGM - University of Udine, Italy
luca.digaspero@uniud.it
http://www.diegm.uniud.it/digaspero/
The author will be grateful if the researchers using this software will
acknowledge the contribution of this function in their research papers.
Copyright (c) 2007-2016 Luca Di Gaspero
This software may be modified and distributed under the terms
of the MIT license. See the LICENSE file for details.
*/
#ifndef _QUADPROGPP
#define _QUADPROGPP
#include "quadProgpp/include/Array.hh"
#include <eigen3/Eigen/Dense>
namespace quadprogpp {
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
const Matrix<double>& CE, const Vector<double>& ce0,
const Matrix<double>& CI, const Vector<double>& ci0,
Vector<double>& x);
} // namespace quadprogpp
#endif // #define _QUADPROGPP

33
thirdparty/quadProgpp/src/Array.cc vendored Executable file
View File

@ -0,0 +1,33 @@
// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
// This file is part of QuadProg++:
// Copyright (C) 2006--2009 Luca Di Gaspero.
//
// This software may be modified and distributed under the terms
// of the MIT license. See the LICENSE file for details.
#include "quadProgpp/include/Array.hh"
/**
Index utilities
*/
namespace quadprogpp {
std::set<unsigned int> seq(unsigned int s, unsigned int e)
{
std::set<unsigned int> tmp;
for (unsigned int i = s; i <= e; i++)
tmp.insert(i);
return tmp;
}
std::set<unsigned int> singleton(unsigned int i)
{
std::set<unsigned int> tmp;
tmp.insert(i);
return tmp;
}
} // namespace quadprogpp

791
thirdparty/quadProgpp/src/QuadProg++.cc vendored Executable file
View File

@ -0,0 +1,791 @@
/*
File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
Author: Luca Di Gaspero
DIEGM - University of Udine, Italy
luca.digaspero@uniud.it
http://www.diegm.uniud.it/digaspero/
This software may be modified and distributed under the terms
of the MIT license. See the LICENSE file for details.
*/
#include <iostream>
#include <algorithm>
#include <cmath>
#include <limits>
#include <sstream>
#include <stdexcept>
#include "quadProgpp/include/QuadProg++.hh"
//#define TRACE_SOLVER
namespace quadprogpp {
// Utility functions for updating some data needed by the solution method
void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np);
void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq);
void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq);
bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& rnorm);
void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l);
// Utility functions for computing the Cholesky decomposition and solving
// linear systems
void cholesky_decomposition(Matrix<double>& A);
void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b);
void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b);
void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y);
// Utility functions for computing the scalar product and the euclidean
// distance between two numbers
double scalar_product(const Vector<double>& x, const Vector<double>& y);
double distance(double a, double b);
// Utility functions for printing vectors and matrices
void print_matrix(const char* name, const Matrix<double>& A, int n = -1, int m = -1);
template<typename T>
void print_vector(const char* name, const Vector<T>& v, int n = -1);
// The Solving function, implementing the Goldfarb-Idnani method
double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
const Matrix<double>& CE, const Vector<double>& ce0,
const Matrix<double>& CI, const Vector<double>& ci0,
Vector<double>& x)
{
std::ostringstream msg;
unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
if (G.nrows() != n)
{
msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")";
throw std::logic_error(msg.str());
}
if (CE.nrows() != n)
{
msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if (ce0.size() != p)
{
msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")";
throw std::logic_error(msg.str());
}
if (CI.nrows() != n)
{
msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if (ci0.size() != m)
{
msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")";
throw std::logic_error(msg.str());
}
x.resize(n);
register unsigned int i, j, k, l; /* indices */
int ip; // this is the index of the constraint to be added to the active set
Matrix<double> R(n, n), J(n, n);
Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
double inf;
if (std::numeric_limits<double>::has_infinity)
inf = std::numeric_limits<double>::infinity();
else
inf = 1.0E300;
double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
* and the full step length t2 */
Vector<int> A(m + p), A_old(m + p), iai(m + p);
unsigned int iq, iter = 0;
Vector<bool> iaexcl(m + p);
/* p is the number of equality constraints */
/* m is the number of inequality constraints */
#ifdef TRACE_SOLVER
std::cout << std::endl << "Starting solve_quadprog" << std::endl;
print_matrix("G", G);
print_vector("g0", g0);
print_matrix("CE", CE);
print_vector("ce0", ce0);
print_matrix("CI", CI);
print_vector("ci0", ci0);
#endif
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = 0.0;
for (i = 0; i < n; i++)
{
c1 += G[i][i];
}
/* decompose the matrix G in the form L^T L */
cholesky_decomposition(G);
#ifdef TRACE_SOLVER
print_matrix("G", G);
#endif
/* initialize the matrix R */
for (i = 0; i < n; i++)
{
d[i] = 0.0;
for (j = 0; j < n; j++)
R[i][j] = 0.0;
}
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
c2 = 0.0;
for (i = 0; i < n; i++)
{
d[i] = 1.0;
forward_elimination(G, z, d);
for (j = 0; j < n; j++)
J[i][j] = z[j];
c2 += z[i];
d[i] = 0.0;
}
#ifdef TRACE_SOLVER
print_matrix("J", J);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
cholesky_solve(G, x, g0);
for (i = 0; i < n; i++)
x[i] = -x[i];
/* and compute the current solution value */
f_value = 0.5 * scalar_product(g0, x);
#ifdef TRACE_SOLVER
std::cout << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < p; i++)
{
for (j = 0; j < n; j++)
np[j] = CE[j][i];
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, n, iq);
print_vector("z", z);
print_vector("r", r, iq);
print_vector("d", d);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
/* set x = x + t2 * z */
for (k = 0; k < n; k++)
x[k] += t2 * z[k];
/* set u = u+ */
u[iq] = t2;
for (k = 0; k < iq; k++)
u[k] -= t2 * r[k];
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
A[i] = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// Equality constraints are linearly dependent
throw std::runtime_error("Constraints are linearly dependent");
return f_value;
}
}
/* set iai = K \ A */
for (i = 0; i < m; i++)
iai[i] = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x);
#endif
/* step 1: choose a violated constraint */
for (i = p; i < iq; i++)
{
ip = A[i];
iai[ip] = -1;
}
/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < m; i++)
{
iaexcl[i] = true;
sum = 0.0;
for (j = 0; j < n; j++)
sum += CI[j][i] * x[j];
sum += ci0[i];
s[i] = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
return f_value;
}
/* save old values for u and A */
for (i = 0; i < iq; i++)
{
u_old[i] = u[i];
A_old[i] = A[i];
}
/* and for x */
for (i = 0; i < n; i++)
x_old[i] = x[i];
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < m; i++)
{
if (s[i] < ss && iai[i] != -1 && iaexcl[i])
{
ss = s[i];
ip = i;
}
}
if (ss >= 0.0)
{
return f_value;
}
/* set np = n[ip] */
for (i = 0; i < n; i++)
np[i] = CI[i][ip];
/* set u = [u 0]^T */
u[iq] = 0.0;
/* add ip to the active set A */
A[iq] = ip;
#ifdef TRACE_SOLVER
std::cout << "Trying with constraint " << ip << std::endl;
print_vector("np", np);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
print_vector("z", z);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d);
print_vector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+[x] / r */
for (k = p; k < iq; k++)
{
if (r[k] > 0.0)
{
if (u[k] / r[k] < t1)
{
t1 = u[k] / r[k];
l = A[k];
}
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
{
t2 = -s[ip] / scalar_product(z, np);
if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies
t2 = inf;
}
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
return inf;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1] and drop constraint l from the active set A */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
std::cout << " in dual space: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("z", z);
print_vector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
/* set x = x + t * z */
for (k = 0; k < n; k++)
x[k] += t * z[k];
/* update the solution value */
f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
/* u = u + t * [-r 1] */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
#ifdef TRACE_SOLVER
std::cout << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_vector("A", A, iq + 1);
#endif
if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
{
#ifdef TRACE_SOLVER
std::cout << "Full step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, n, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
for (i = 0; i < m; i++)
iai[i] = i;
for (i = p; i < iq; i++)
{
A[i] = A_old[i];
u[i] = u_old[i];
iai[A[i]] = -1;
}
for (i = 0; i < n; i++)
x[i] = x_old[i];
goto l2; /* go to step 2 */
}
else
iai[ip] = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cout << "Partial step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* drop constraint l */
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
#endif
/* update s[ip] = CI * x + ci0 */
sum = 0.0;
for (k = 0; k < n; k++)
sum += CI[k][ip] * x[k];
s[ip] = sum + ci0[ip];
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
goto l2a;
}
inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np)
{
register int i, j, n = d.size();
register double sum;
/* compute d = H^T * np */
for (i = 0; i < n; i++)
{
sum = 0.0;
for (j = 0; j < n; j++)
sum += J[j][i] * np[j];
d[i] = sum;
}
}
inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq)
{
register int i, j, n = z.size();
/* setting of z = H * d */
for (i = 0; i < n; i++)
{
z[i] = 0.0;
for (j = iq; j < n; j++)
z[i] += J[i][j] * d[j];
}
}
inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq)
{
register int i, j;
register double sum;
/* setting of r = R^-1 d */
for (i = iq - 1; i >= 0; i--)
{
sum = 0.0;
for (j = i + 1; j < iq; j++)
sum += R[i][j] * r[j];
r[i] = (d[i] - sum) / R[i][i];
}
}
bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& R_norm)
{
unsigned int n = d.size();
#ifdef TRACE_SOLVER
std::cout << "Add constraint " << iq << '/';
#endif
register unsigned int i, j, k;
double cc, ss, h, t1, t2, xny;
/* we have to find the Givens rotation which will reduce the element
d[j] to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for (j = n - 1; j >= iq + 1; j--)
{
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc = d[j - 1];
ss = d[j];
h = distance(cc, ss);
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
continue;
d[j] = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0)
{
cc = -cc;
ss = -ss;
d[j - 1] = -h;
}
else
d[j - 1] = h;
xny = ss / (1.0 + cc);
for (k = 0; k < n; k++)
{
t1 = J[k][j - 1];
t2 = J[k][j];
J[k][j - 1] = t1 * cc + t2 * ss;
J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
}
}
/* update the number of constraints added*/
iq++;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
for (i = 0; i < iq; i++)
R[i][iq - 1] = d[i];
#ifdef TRACE_SOLVER
std::cout << iq << std::endl;
print_matrix("R", R, iq, iq);
print_matrix("J", J);
print_vector("d", d, iq);
#endif
if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm)
{
// problem degenerate
return false;
}
R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
return true;
}
void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l)
{
#ifdef TRACE_SOLVER
std::cout << "Delete constraint " << l << ' ' << iq;
#endif
register unsigned int i, j, k, qq = 0; // just to prevent warnings from smart compilers
double cc, ss, h, xny, t1, t2;
bool found = false;
/* Find the index qq for active constraint l to be removed */
for (i = p; i < iq; i++)
if (A[i] == l)
{
qq = i;
found = true;
break;
}
if(!found)
{
std::ostringstream os;
os << "Attempt to delete non existing constraint, constraint: " << l;
throw std::invalid_argument(os.str());
}
/* remove the constraint from the active set and the duals */
for (i = qq; i < iq - 1; i++)
{
A[i] = A[i + 1];
u[i] = u[i + 1];
for (j = 0; j < n; j++)
R[j][i] = R[j][i + 1];
}
A[iq - 1] = A[iq];
u[iq - 1] = u[iq];
A[iq] = 0;
u[iq] = 0.0;
for (j = 0; j < iq; j++)
R[j][iq - 1] = 0.0;
/* constraint has been fully removed */
iq--;
#ifdef TRACE_SOLVER
std::cout << '/' << iq << std::endl;
#endif
if (iq == 0)
return;
for (j = qq; j < iq; j++)
{
cc = R[j][j];
ss = R[j + 1][j];
h = distance(cc, ss);
if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
continue;
cc = cc / h;
ss = ss / h;
R[j + 1][j] = 0.0;
if (cc < 0.0)
{
R[j][j] = -h;
cc = -cc;
ss = -ss;
}
else
R[j][j] = h;
xny = ss / (1.0 + cc);
for (k = j + 1; k < iq; k++)
{
t1 = R[j][k];
t2 = R[j + 1][k];
R[j][k] = t1 * cc + t2 * ss;
R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
}
for (k = 0; k < n; k++)
{
t1 = J[k][j];
t2 = J[k][j + 1];
J[k][j] = t1 * cc + t2 * ss;
J[k][j + 1] = xny * (J[k][j] + t1) - t2;
}
}
}
inline double distance(double a, double b)
{
register double a1, b1, t;
a1 = fabs(a);
b1 = fabs(b);
if (a1 > b1)
{
t = (b1 / a1);
return a1 * sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
t = (a1 / b1);
return b1 * sqrt(1.0 + t * t);
}
return a1 * sqrt(2.0);
}
inline double scalar_product(const Vector<double>& x, const Vector<double>& y)
{
register int i, n = x.size();
register double sum;
sum = 0.0;
for (i = 0; i < n; i++)
sum += x[i] * y[i];
return sum;
}
void cholesky_decomposition(Matrix<double>& A)
{
register int i, j, k, n = A.nrows();
register double sum;
for (i = 0; i < n; i++)
{
for (j = i; j < n; j++)
{
sum = A[i][j];
for (k = i - 1; k >= 0; k--)
sum -= A[i][k]*A[j][k];
if (i == j)
{
if (sum <= 0.0)
{
std::ostringstream os;
// raise error
print_matrix("A", A);
os << "Error in cholesky decomposition, sum: " << sum;
throw std::logic_error(os.str());
exit(-1);
}
A[i][i] = sqrt(sum);
}
else
A[j][i] = sum / A[i][i];
}
for (k = i + 1; k < n; k++)
A[i][k] = A[k][i];
}
}
void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b)
{
int n = L.nrows();
Vector<double> y(n);
/* Solve L * y = b */
forward_elimination(L, y, b);
/* Solve L^T * x = y */
backward_elimination(L, x, y);
}
inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b)
{
register int i, j, n = L.nrows();
y[0] = b[0] / L[0][0];
for (i = 1; i < n; i++)
{
y[i] = b[i];
for (j = 0; j < i; j++)
y[i] -= L[i][j] * y[j];
y[i] = y[i] / L[i][i];
}
}
inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y)
{
register int i, j, n = U.nrows();
x[n - 1] = y[n - 1] / U[n - 1][n - 1];
for (i = n - 2; i >= 0; i--)
{
x[i] = y[i];
for (j = i + 1; j < n; j++)
x[i] -= U[i][j] * x[j];
x[i] = x[i] / U[i][i];
}
}
void print_matrix(const char* name, const Matrix<double>& A, int n, int m)
{
std::ostringstream s;
std::string t;
if (n == -1)
n = A.nrows();
if (m == -1)
m = A.ncols();
s << name << ": " << std::endl;
for (int i = 0; i < n; i++)
{
s << " ";
for (int j = 0; j < m; j++)
s << A[i][j] << ", ";
s << std::endl;
}
t = s.str();
t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
std::cout << t << std::endl;
}
template<typename T>
void print_vector(const char* name, const Vector<T>& v, int n)
{
std::ostringstream s;
std::string t;
if (n == -1)
n = v.size();
s << name << ": " << std::endl << " ";
for (int i = 0; i < n; i++)
{
s << v[i] << ", ";
}
t = s.str();
t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
std::cout << t << std::endl;
}
} // namespace quadprogpp

BIN
thirdparty/rbdl-2.6.0.zip vendored Normal file
View File

Binary file not shown.