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:
Agnel Wang 2022-09-13 19:53:15 +08:00
parent e64a878cdf
commit a5eaa3dae4
55 changed files with 6798 additions and 3670 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
CMakeLists.txt
View File

@ -1,19 +1,15 @@
cmake_minimum_required(VERSION 3.0)
project(z1_controller)
project(z1_controller LANGUAGES CXX)
add_definitions(-std=c++14)
set(PACKAGE_NAME z1_controller)
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
# ----------------------options----------------------
set(COMMUNICATION UDP) # UDP
# set(COMMUNICATION ROS) # ROS
# ----------------------configurations----------------------
# COMMUNICATION
if(${COMMUNICATION} STREQUAL "UDP")
add_definitions(-DUDP)
set(SIMULATION OFF)
@ -26,6 +22,7 @@ else()
message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
endif()
# SIM OR REAL
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()
@ -40,34 +37,8 @@ if(SIMULATION)
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()
# ARM_MODEL
add_definitions(-DARM_Z1)
if(SIMULATION)
find_package(catkin REQUIRED COMPONENTS
@ -110,35 +81,9 @@ include_directories(
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
# ----------------------add executable----------------------
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)
target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${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}/)
# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)

224
README.md
View File

@ -1,223 +1 @@
# 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
```
see [unitree-z1-docs](http://dev-z1.unitree.com/)

26
config.xml Normal file
View File

@ -0,0 +1,26 @@
<configurartion>
<control set="1">
<option1>keyboard</option1>
<option2>sdk</option2>
<option3>joystick</option3> <!-- default aliengo_joystick -->
</control>
<gripper set="2">
<option1 name="none"/>
<option2 name="Unitree_gripper"/>
<option3 name="user_gripper">
<endPosLocalX>0.0</endPosLocalX>
<endPosLocalY>0.0</endPosLocalY>
<endPosLocalZ>0.0</endPosLocalZ>
<mass>0.0</mass>
<comX>0.0</comX>
<comY>0.0</comY>
<comZ>0.0</comZ>
<Ixx>0.0</Ixx>
<Iyy>0.0</Iyy>
<Izz>0.0</Izz>
</option3>
</gripper>
<IP>192.168.123.110</IP>
</configurartion>

57
config/savedArmStates.csv
View File

@ -1,59 +1,14 @@
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,
forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000,
start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 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,
mohe1, 0.966000, 1.420800, -1.412412, -1.141151, 0.997200, 1.675206, -0.560087, -0.444472, 0.699105, 0.049195, 0.666938, -0.742508, 0.062250, 0.235148, 0.491423, 0.501125, 0.712304, 0.622679, 0.000000, 0.000000, 0.000000, 1.000000,
mohe2, -1.057200, 2.424000, -2.682793, 0.986449, -1.342800, -1.709994, -0.765519, -0.400523, 0.503550, 0.166634, -0.625604, 0.646204, -0.437081, -0.515483, -0.150335, -0.649616, -0.745251, 0.381296, 0.000000, 0.000000, 0.000000, 1.000000,
5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.000065, 0.000206, 0.784707, -0.614016, -0.084965, 0.592966, 0.610421, 0.789294, -0.066347, 0.461215, 0.107801, 0.000198, 0.994173, 0.198126, 0.000000, 0.000000, 0.000000, 1.000000,
5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.000065, -0.000378, 0.871097, 0.475482, -0.122910, 0.664711, -0.470757, 0.879726, 0.066863, -0.359274, 0.139919, -0.000383, 0.990163, 0.133116, 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.389527 0.002468 0.999923 0.012173 0.000358 0.000269 0.026329 -0.012234 0.999578 0.394252 0.402549 0.000000 0.000000 0.000000 1.000000
2 start -0.000336 0.001634 0.008171 0.000000 0.064640 0.000248 0.000230 0.997230 0.997805 0.000106 0.000104 0.074376 0.066225 0.085128 0.048696 -0.000087 1.000000 -0.000255 -0.000252 0.000008 0.000011 -0.074376 -0.066225 0.000248 0.000246 0.997230 0.997805 0.133277 0.148729 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
3 startFlat 0.000000 0.000000 -0.000012 -0.074351 0.000000 0.000006 0.997236 -0.000000 -0.074294 0.086594 0.086594 0.000000 1.000000 -0.000006 0.000000 0.000000 0.074294 0.000006 0.997236 0.176788 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
4 teachstart -0.858000 1.951000 -0.825000 0.000000 -1.154000 0.000000 0.653696 0.756536 -0.018308 0.260474 0.260474 -0.756240 0.653952 0.021180 -0.301334 -0.301334 0.027996 0.000000 0.999608 0.183326 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
5 fulltest00 2.316194 0.013239 -0.001959 -0.000047 -0.278982 0.221496 -0.654115 -0.677458 0.336427 -0.022776 -0.022776 0.708636 -0.704403 -0.040645 0.024673 0.024673 0.264516 0.211818 0.940832 0.174303 0.174303 0.000000 0.000000 0.000000 1.000000
6 fulltest01 0.000194 1.621239 -1.455959 2.470727 1.492796 0.419496 0.205219 0.489994 0.847225 0.332273 0.332273 0.619814 -0.735004 0.274956 0.059687 0.059687 0.757440 0.468696 -0.454542 0.521459 0.521459 0.000000 0.000000 0.000000 1.000000
7 fulltest02 -2.336194 3.117239 -3.099593 -2.478727 -1.516796 0.419496 0.415340 -0.220546 0.882526 -0.404129 -0.404129 -0.454627 0.789988 0.411380 -0.505948 -0.505948 -0.787913 -0.572083 0.227847 0.075097 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
8 001 0.000000 0.922204 -1.215402 0.293198 0.000000 0.000000 1.000000 0.000000 -0.000000 0.200000 0.200000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.500000 0.500000 0.000000 0.000000 0.000000 1.000000
9 002 -1.035000 0.922204 -1.017402 -0.285802 0.000000 0.000000 0.473918 0.859862 -0.189839 0.103971 0.103971 -0.798204 0.510526 0.319739 -0.175114 -0.175114 0.371849 0.000000 0.928293 0.541400 0.541400 0.000000 0.000000 0.000000 1.000000
10 003 0.954000 0.910204 -0.984402 -0.192802 0.000000 0.000000 0.557929 -0.815736 -0.152611 0.121384 0.121384 0.786832 0.578425 -0.215223 0.171185 0.171185 0.263839 0.000000 0.964567 0.510707 0.510707 0.000000 0.000000 0.000000 1.000000
11 starFlat mohe1 -0.003451 0.966000 0.000501 1.420800 0.009250 -1.412412 -0.064964 -1.141151 0.000525 0.997200 0.000590 1.675206 0.998472 -0.560087 0.002895 -0.444472 -0.055186 0.699105 0.085395 0.049195 -0.002921 0.666938 0.999996 -0.742508 -0.000400 0.062250 -0.000217 0.235148 0.055184 0.491423 0.000560 0.501125 0.998476 0.712304 0.161033 0.622679 0.000000 0.000000 0.000000 1.000000
12 yy mohe2 0.000000 -1.057200 0.922204 2.424000 -1.215402 -2.682793 0.293198 0.986449 0.000000 -1.342800 0.000000 -1.709994 1.000000 -0.765519 0.000000 -0.400523 -0.000000 0.503550 0.200000 0.166634 0.000000 -0.625604 1.000000 0.646204 0.000000 -0.437081 0.000000 -0.515483 0.000000 -0.150335 0.000000 -0.649616 1.000000 -0.745251 0.500000 0.381296 0.000000 0.000000 0.000000 1.000000
13 5kg1 0.661051 3.010641 -2.885593 -0.233059 0.000065 0.000206 0.784707 -0.614016 -0.084965 0.592966 0.610421 0.789294 -0.066347 0.461215 0.107801 0.000198 0.994173 0.198126 0.000000 0.000000 0.000000 1.000000
14 5kg2 -0.495523 3.122645 -2.856166 -0.406859 0.000065 -0.000378 0.871097 0.475482 -0.122910 0.664711 -0.470757 0.879726 0.066863 -0.359274 0.139919 -0.000383 0.990163 0.133116 0.000000 0.000000 0.000000 1.000000

2
include/FSM/BaseState.h
View File

@ -2,6 +2,7 @@
#define BASESTATE_H
#include <string>
#include "common/enumClass.h"
class BaseState{
public:
@ -25,6 +26,7 @@ public:
protected:
int _stateNameEnum;
std::string _stateNameString;
Control _ctrl;
};
#endif

9
include/FSM/FSMState.h
View File

@ -24,7 +24,7 @@ public:
virtual void run() = 0;
virtual void exit() = 0;
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
protected:
void _armCtrl();
void _tauDynForward();
@ -33,20 +33,21 @@ protected:
bool _collisionTest();
void _jointPosProtect();
void _jointSpeedProtect();
#ifdef UNITREE_GRIPPER
void _stateUpdate();
void _gripperCmd();
void _gripperCtrl();
double _gripperPos;
double _gripperW;
double _gripperTau;
double _gripperPosStep;
double _gripperPosStep;//keyboard
double _gripperTauStep;
double _gripperLinearFriction;
double _gripperCoulombFriction;
static double _gripperCoulombDirection;
double _gripperFriction;
#endif
CtrlComponents *_ctrlComp;
ArmFSMStateName _nextStateName;

2
include/FSM/FiniteStateMachine.h
View File

@ -5,6 +5,7 @@
#include "FSM/BaseState.h"
#include "unitree_arm_sdk/cmdPanel.h"
#include "unitree_arm_sdk/loop.h"
#include "unitree_arm_sdk/keyboard.h"
enum class FSMRunMode{
NORMAL,
@ -25,7 +26,6 @@ public:
private:
void _run();
static void* _staticRun(void* obj);
std::vector<BaseState*> _states;
FSMRunMode _mode;

1
include/FSM/State_BackToStart.h
View File

@ -8,6 +8,7 @@ public:
State_BackToStart(CtrlComponents *ctrlComp);
private:
void _setTraj();
void _setTrajSDK(){}
};
#endif // STATE_BACKTOSTART_H

2
include/FSM/State_Cartesian.h
View File

@ -12,13 +12,13 @@ public:
void exit();
int checkChange(int cmd);
private:
void quadprogArea();
double _posSpeed;
double _oriSpeed;
Vec3 _omega;
Vec3 _velocity;
Vec6 _twist;
HomoMat _endHomoGoal, _endHomoGoalPast;
Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
HomoMat _endHomoFeedback;
Vec6 _Pdes;
Vec6 _Pfd;

1
include/FSM/State_Dance.h
View File

@ -14,7 +14,6 @@ public:
int checkChange(int cmd);
private:
bool _freeBottom = false;
// ArmFSMStateName _nextState;
void _setTraj(){}
};

1
include/FSM/State_MoveL.h
View File

@ -31,5 +31,4 @@ private:
CartesionSpaceTraj *_cartesionTraj;
bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
};
#endif // CARTESIAN_H

20
include/FSM/State_SetTraj.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef STATE_SETTRAJ_H
#define STATE_SETTRAJ_H
#include "FSM/State_Trajectory.h"
class State_SetTraj: public State_Trajectory{
public:
State_SetTraj(CtrlComponents *ctrlComp,
ArmFSMStateName stateEnum = ArmFSMStateName::SETTRAJ,
std::string stateString = "setTraj");
~State_SetTraj();
void enter();
void run();
void exit();
int checkChange(int cmd);
private:
Vec6 _posture[2];
};
#endif

1
include/FSM/State_TeachRepeat.h
View File

@ -14,6 +14,7 @@ public:
void exit();
int checkChange(int cmd);
private:
bool _setCorrectly;
JointSpaceTraj *_toStartTraj;
bool _reachedStart = false;
bool _finishedRepeat = false;

1
include/FSM/State_ToState.h
View File

@ -13,6 +13,7 @@ public:
void exit();
int checkChange(int cmd);
private:
bool _setCorrectly;
double _costTime;
HomoMat _goalHomo;
JointSpaceTraj *_jointTraj;

9
include/FSM/State_Trajectory.h
View File

@ -13,11 +13,12 @@ public:
~State_Trajectory();
void enter();
void run();
virtual void exit();
virtual int checkChange(int cmd);
void exit();
int checkChange(int cmd);
protected:
// EndHomoTraj *_traj;
virtual void _setTraj();
virtual void _setTrajSdk();
TrajectoryManager *_traj;
HomoMat _goalHomo;
Vec6 _goalTwist;
@ -31,6 +32,10 @@ protected:
std::vector<EndLineTraj*> _lineTraj;
long long startTime;
static std::vector<TrajCmd> _trajCmd;
Vec6 _posture[2];
static bool _isTrajFSM;
};
#endif // CARTESIANPATH_H

42
include/common/enumClass.h
View File

@ -1,41 +1,6 @@
#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,
@ -62,6 +27,7 @@ enum class ArmFSMStateName{
TEACH,
TEACHREPEAT,
CALIBRATION,
SETTRAJ,
DANCE00,
DANCE01,
DANCE02,
@ -94,4 +60,10 @@ enum class HeadType{
DOG
};
enum class Control{
_KEYBOARD,
_SDK,
_JOYSTICK,
};
#endif // ENUMCLASS_H

20
include/common/math/mathTools.h
View File

@ -2,7 +2,7 @@
#define MATHTOOLS_H
#include <iostream>
// #include "common/mathTypes.h"
#include "common/math/mathTypes.h"
// template<typename T1, typename T2>
// inline T1 max(const T1 a, const T2 b){
@ -60,14 +60,7 @@ inline double angleError(double first, double second, TurnDirection direction =
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(direction == TurnDirection::POSITIVE){
if(firstMod - secondMod < 0.0){
return 2*M_PI + firstMod - secondMod;
}else{
@ -80,6 +73,12 @@ inline double angleError(double first, double second, TurnDirection direction =
}else{
return firstMod - secondMod;
}
}else{//no matter
if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){
return secondMod - firstMod;
}else{
return firstMod - secondMod;
}
}
}
@ -478,5 +477,4 @@ private:
double _zoomFactor;
std::string _valueName;
};
#endif // MATHTOOLS_H
#endif

685
include/common/math/robotics.h Normal file
View File

@ -0,0 +1,685 @@
#pragma once
#include <vector>
#include "common/math/mathTools.h"
namespace mr {
/*
* Function: Find if the value is negligible enough to consider 0
* Inputs: value to be checked as a double
* Returns: Boolean of true-ignore or false-can't ignore
*/
bool NearZero(const double);
/*
* Function: Calculate the 6x6 matrix [adV] of the given 6-vector
* Input: Eigen::VectorXd (6x1)
* Output: Eigen::MatrixXd (6x6)
* Note: Can be used to calculate the Lie bracket [V1, V2] = [adV1]V2
*/
Eigen::MatrixXd ad(Eigen::VectorXd);
/*
* Function: Returns a normalized version of the input vector
* Input: Eigen::MatrixXd
* Output: Eigen::MatrixXd
* Note: MatrixXd is used instead of VectorXd for the case of row vectors
* Requires a copy
* Useful because of the MatrixXd casting
*/
Eigen::MatrixXd Normalize(Eigen::MatrixXd);
/*
* Function: Returns the skew symmetric matrix representation of an angular velocity vector
* Input: Eigen::Vector3d 3x1 angular velocity vector
* Returns: Eigen::MatrixXd 3x3 skew symmetric matrix
*/
Eigen::Matrix3d VecToso3(const Eigen::Vector3d&);
/*
* Function: Returns angular velocity vector represented by the skew symmetric matrix
* Inputs: Eigen::MatrixXd 3x3 skew symmetric matrix
* Returns: Eigen::Vector3d 3x1 angular velocity
*/
Eigen::Vector3d so3ToVec(const Eigen::MatrixXd&);
/*
* Function: Tranlates an exponential rotation into it's individual components
* Inputs: Exponential rotation (rotation matrix in terms of a rotation axis
* and the angle of rotation)
* Returns: The axis and angle of rotation as [x, y, z, theta]
*/
Eigen::Vector4d AxisAng3(const Eigen::Vector3d&);
/*
* Function: Translates an exponential rotation into a rotation matrix
* Inputs: exponenential representation of a rotation
* Returns: Rotation matrix
*/
Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d&);
/* Function: Computes the matrix logarithm of a rotation matrix
* Inputs: Rotation matrix
* Returns: matrix logarithm of a rotation
*/
Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d&);
/*
* Function: Combines a rotation matrix and position vector into a single
* Special Euclidian Group (SE3) homogeneous transformation matrix
* Inputs: Rotation Matrix (R), Position Vector (p)
* Returns: Matrix of T = [ [R, p],
* [0, 1] ]
*/
Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d&, const Eigen::Vector3d&);
/*
* Function: Separates the rotation matrix and position vector from
* the transfomation matrix representation
* Inputs: Homogeneous transformation matrix
* Returns: std::vector of [rotation matrix, position vector]
*/
std::vector<Eigen::MatrixXd> TransToRp(const Eigen::MatrixXd&);
/*
* Function: Translates a spatial velocity vector into a transformation matrix
* Inputs: Spatial velocity vector [angular velocity, linear velocity]
* Returns: Transformation matrix
*/
Eigen::MatrixXd VecTose3(const Eigen::VectorXd&);
/* Function: Translates a transformation matrix into a spatial velocity vector
* Inputs: Transformation matrix
* Returns: Spatial velocity vector [angular velocity, linear velocity]
*/
Eigen::VectorXd se3ToVec(const Eigen::MatrixXd&);
/*
* Function: Provides the adjoint representation of a transformation matrix
* Used to change the frame of reference for spatial velocity vectors
* Inputs: 4x4 Transformation matrix SE(3)
* Returns: 6x6 Adjoint Representation of the matrix
*/
Eigen::MatrixXd Adjoint(const Eigen::MatrixXd&);
/*
* Function: Rotation expanded for screw axis
* Inputs: se3 matrix representation of exponential coordinates (transformation matrix)
* Returns: 6x6 Matrix representing the rotation
*/
Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&);
/*
* Function: Computes the matrix logarithm of a homogeneous transformation matrix
* Inputs: R: Transformation matrix in SE3
* Returns: The matrix logarithm of R
*/
Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&);
/*
* Function: Compute end effector frame (used for current spatial position calculation)
* Inputs: Home configuration (position and orientation) of end-effector
* The joint screw axes in the space frame when the manipulator
* is at the home position
* A list of joint coordinates.
* Returns: Transfomation matrix representing the end-effector frame when the joints are
* at the specified coordinates
* Notes: FK means Forward Kinematics
*/
Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&);
/*
* Function: Compute end effector frame (used for current body position calculation)
* Inputs: Home configuration (position and orientation) of end-effector
* The joint screw axes in the body frame when the manipulator
* is at the home position
* A list of joint coordinates.
* Returns: Transfomation matrix representing the end-effector frame when the joints are
* at the specified coordinates
* Notes: FK means Forward Kinematics
*/
Eigen::MatrixXd FKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&);
/*
* Function: Gives the space Jacobian
* Inputs: Screw axis in home position, joint configuration
* Returns: 6xn Spatial Jacobian
*/
Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&);
/*
* Function: Gives the body Jacobian
* Inputs: Screw axis in BODY position, joint configuration
* Returns: 6xn Bobdy Jacobian
*/
Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&);
/*
* Inverts a homogeneous transformation matrix
* Inputs: A homogeneous transformation Matrix T
* Returns: The inverse of T
*/
Eigen::MatrixXd TransInv(const Eigen::MatrixXd&);
/*
* Inverts a rotation matrix
* Inputs: A rotation matrix R
* Returns: The inverse of R
*/
Eigen::MatrixXd RotInv(const Eigen::MatrixXd&);
/*
* Takes a parametric description of a screw axis and converts it to a
* normalized screw axis
* Inputs:
* q: A point lying on the screw axis
* s: A unit vector in the direction of the screw axis
* h: The pitch of the screw axis
* Returns: A normalized screw axis described by the inputs
*/
Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h);
/*
* Function: Translates a 6-vector of exponential coordinates into screw
* axis-angle form
* Inputs:
* expc6: A 6-vector of exponential coordinates for rigid-body motion
S*theta
* Returns: The corresponding normalized screw axis S; The distance theta traveled
* along/about S in form [S, theta]
* Note: Is it better to return std::map<S, theta>?
*/
Eigen::VectorXd AxisAng6(const Eigen::VectorXd&);
/*
* Function: Returns projection of one matrix into SO(3)
* Inputs:
* M: A matrix near SO(3) to project to SO(3)
* Returns: The closest matrix R that is in SO(3)
* Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition
* (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review).
* This function is only appropriate for matrices close to SO(3).
*/
Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd&);
/*
* Function: Returns projection of one matrix into SE(3)
* Inputs:
* M: A 4x4 matrix near SE(3) to project to SE(3)
* Returns: The closest matrix T that is in SE(3)
* Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition
* (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review).
* This function is only appropriate for matrices close to SE(3).
*/
Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd&);
/*
* Function: Returns the Frobenius norm to describe the distance of M from the SO(3) manifold
* Inputs:
* M: A 3x3 matrix
* Outputs:
* the distance from mat to the SO(3) manifold using the following
* method:
* If det(M) <= 0, return a large number.
* If det(M) > 0, return norm(M^T*M - I).
*/
double DistanceToSO3(const Eigen::Matrix3d&);
/*
* Function: Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold
* Inputs:
* T: A 4x4 matrix
* Outputs:
* the distance from T to the SE(3) manifold using the following
* method:
* Compute the determinant of matR, the top 3x3 submatrix of T.
* If det(matR) <= 0, return a large number.
* If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T*matR,
* and set the first three entries of the fourth column of mat to zero. Then
* return norm(T - I).
*/
double DistanceToSE3(const Eigen::Matrix4d&);
/*
* Function: Returns true if M is close to or on the manifold SO(3)
* Inputs:
* M: A 3x3 matrix
* Outputs:
* true if M is very close to or in SO(3), false otherwise
*/
bool TestIfSO3(const Eigen::Matrix3d&);
/*
* Function: Returns true if T is close to or on the manifold SE(3)
* Inputs:
* M: A 4x4 matrix
* Outputs:
* true if T is very close to or in SE(3), false otherwise
*/
bool TestIfSE3(const Eigen::Matrix4d&);
/*
* Function: Computes inverse kinematics in the body frame for an open chain robot
* Inputs:
* Blist: The joint screw axes in the end-effector frame when the
* manipulator is at the home position, in the format of a
* matrix with axes as the columns
* M: The home configuration of the end-effector
* T: The desired end-effector configuration Tsd
* thetalist[in][out]: An initial guess and result output of joint angles that are close to
* satisfying Tsd
* emog: A small positive tolerance on the end-effector orientation
* error. The returned joint angles must give an end-effector
* orientation error less than eomg
* ev: A small positive tolerance on the end-effector linear position
* error. The returned joint angles must give an end-effector
* position error less than ev
* Outputs:
* success: A logical value where TRUE means that the function found
* a solution and FALSE means that it ran through the set
* number of maximum iterations without finding a solution
* within the tolerances eomg and ev.
* thetalist[in][out]: Joint angles that achieve T within the specified tolerances,
*/
bool IKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double);
/*
* Function: Computes inverse kinematics in the space frame for an open chain robot
* Inputs:
* Slist: The joint screw axes in the space frame when the
* manipulator is at the home position, in the format of a
* matrix with axes as the columns
* M: The home configuration of the end-effector
* T: The desired end-effector configuration Tsd
* thetalist[in][out]: An initial guess and result output of joint angles that are close to
* satisfying Tsd
* emog: A small positive tolerance on the end-effector orientation
* error. The returned joint angles must give an end-effector
* orientation error less than eomg
* ev: A small positive tolerance on the end-effector linear position
* error. The returned joint angles must give an end-effector
* position error less than ev
* Outputs:
* success: A logical value where TRUE means that the function found
* a solution and FALSE means that it ran through the set
* number of maximum iterations without finding a solution
* within the tolerances eomg and ev.
* thetalist[in][out]: Joint angles that achieve T within the specified tolerances,
*/
bool IKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double);
/*
* Function: This function uses forward-backward Newton-Euler iterations to solve the
* equation:
* taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
* + g(thetalist) + Jtr(thetalist) * Ftip
* Inputs:
* thetalist: n-vector of joint variables
* dthetalist: n-vector of joint rates
* ddthetalist: n-vector of joint accelerations
* g: Gravity vector g
* Ftip: Spatial force applied by the end-effector expressed in frame {n+1}
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* taulist: The n-vector of required joint forces/torques
*
*/
Eigen::VectorXd InverseDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&,
const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and
* ddthetalist = 0. The purpose is to calculate one important term in the dynamics equation
* Inputs:
* thetalist: n-vector of joint variables
* g: Gravity vector g
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* grav: The 3-vector showing the effect force of gravity to the dynamics
*
*/
Eigen::VectorXd GravityForces(const Eigen::VectorXd&, const Eigen::VectorXd&,
const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: This function calls InverseDynamics n times, each time passing a
* ddthetalist vector with a single element equal to one and all other
* inputs set to zero. Each call of InverseDynamics generates a single
* column, and these columns are assembled to create the inertia matrix.
*
* Inputs:
* thetalist: n-vector of joint variables
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* M: The numerical inertia matrix M(thetalist) of an n-joint serial
* chain at the given configuration thetalist.
*/
Eigen::MatrixXd MassMatrix(const Eigen::VectorXd&,
const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: This function calls InverseDynamics with g = 0, Ftip = 0, and
* ddthetalist = 0.
*
* Inputs:
* thetalist: n-vector of joint variables
* dthetalist: A list of joint rates
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* c: The vector c(thetalist,dthetalist) of Coriolis and centripetal
* terms for a given thetalist and dthetalist.
*/
Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd&, const Eigen::VectorXd&,
const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: This function calls InverseDynamics with g = 0, dthetalist = 0, and
* ddthetalist = 0.
*
* Inputs:
* thetalist: n-vector of joint variables
* Ftip: Spatial force applied by the end-effector expressed in frame {n+1}
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* JTFtip: The joint forces and torques required only to create the
* end-effector force Ftip.
*/
Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd&, const Eigen::VectorXd&,
const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: This function computes ddthetalist by solving:
* Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist)
* - g(thetalist) - Jtr(thetalist) * Ftip
* Inputs:
* thetalist: n-vector of joint variables
* dthetalist: n-vector of joint rates
* taulist: An n-vector of joint forces/torques
* g: Gravity vector g
* Ftip: Spatial force applied by the end-effector expressed in frame {n+1}
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* ddthetalist: The resulting joint accelerations
*
*/
Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&,
const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
/*
* Function: Compute the joint angles and velocities at the next timestep using
first order Euler integration
* Inputs:
* thetalist[in]: n-vector of joint variables
* dthetalist[in]: n-vector of joint rates
* ddthetalist: n-vector of joint accelerations
* dt: The timestep delta t
*
* Outputs:
* thetalist[out]: Vector of joint variables after dt from first order Euler integration
* dthetalist[out]: Vector of joint rates after dt from first order Euler integration
*/
void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double);
/*
* Function: Compute the joint forces/torques required to move the serial chain along the given
* trajectory using inverse dynamics
* Inputs:
* thetamat: An N x n matrix of robot joint variables (N: no. of trajecoty time step points; n: no. of robot joints
* dthetamat: An N x n matrix of robot joint velocities
* ddthetamat: An N x n matrix of robot joint accelerations
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
*
* Outputs:
* taumat: The N x n matrix of joint forces/torques for the specified trajectory, where each of the N rows is the vector
* of joint forces/torques at each time step
*/
Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&);
/*
* Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
* Inputs:
* thetalist: n-vector of initial joint variables
* dthetalist: n-vector of initial joint rates
* taumat: An N x n matrix of joint forces/torques, where each row is is the joint effort at any time step
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
* dt: The timestep between consecutive joint forces/torques
* intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
* Must be an integer value greater than or equal to 1
*
* Outputs: std::vector of [thetamat, dthetamat]
* thetamat: The N x n matrix of joint angles resulting from the specified joint forces/torques
* dthetamat: The N x n matrix of joint velocities
*/
std::vector<Eigen::MatrixXd> ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, double, int);
/*
* Function: Compute the joint control torques at a particular time instant
* Inputs:
* thetalist: n-vector of joint variables
* dthetalist: n-vector of joint rates
* eint: n-vector of the time-integral of joint errors
* g: Gravity vector g
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
* thetalistd: n-vector of reference joint variables
* dthetalistd: n-vector of reference joint rates
* ddthetalistd: n-vector of reference joint accelerations
* Kp: The feedback proportional gain (identical for each joint)
* Ki: The feedback integral gain (identical for each joint)
* Kd: The feedback derivative gain (identical for each joint)
*
* Outputs:
* tau_computed: The vector of joint forces/torques computed by the feedback
* linearizing controller at the current instant
*/
Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double);
/*
* Function: Compute s(t) for a cubic time scaling
* Inputs:
* Tf: Total time of the motion in seconds from rest to rest
* t: The current time t satisfying 0 < t < Tf
*
* Outputs:
* st: The path parameter corresponding to a third-order
* polynomial motion that begins and ends at zero velocity
*/
double CubicTimeScaling(double, double);
/*
* Function: Compute s(t) for a quintic time scaling
* Inputs:
* Tf: Total time of the motion in seconds from rest to rest
* t: The current time t satisfying 0 < t < Tf
*
* Outputs:
* st: The path parameter corresponding to a fifth-order
* polynomial motion that begins and ends at zero velocity
* and zero acceleration
*/
double QuinticTimeScaling(double, double);
/*
* Function: Compute a straight-line trajectory in joint space
* Inputs:
* thetastart: The initial joint variables
* thetaend: The final joint variables
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: A trajectory as an N x n matrix, where each row is an n-vector
* of joint variables at an instant in time. The first row is
* thetastart and the Nth row is thetaend . The elapsed time
* between each row is Tf / (N - 1)
*/
Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int);
/*
* Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
* the screw motion about a space screw axis
* Inputs:
* Xstart: The initial end-effector configuration
* Xend: The final end-effector configuration
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: The discretized trajectory as a list of N matrices in SE(3)
* separated in time by Tf/(N-1). The first in the list is Xstart
* and the Nth is Xend
*/
std::vector<Eigen::MatrixXd> ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
/*
* Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
* the origin of the end-effector frame following a straight line
* Inputs:
* Xstart: The initial end-effector configuration
* Xend: The final end-effector configuration
* Tf: Total time of the motion in seconds from rest to rest
* N: The number of points N > 1 (Start and stop) in the discrete
* representation of the trajectory
* method: The time-scaling method, where 3 indicates cubic (third-
* order polynomial) time scaling and 5 indicates quintic
* (fifth-order polynomial) time scaling
*
* Outputs:
* traj: The discretized trajectory as a list of N matrices in SE(3)
* separated in time by Tf/(N-1). The first in the list is Xstart
* and the Nth is Xend
* Notes:
* This function is similar to ScrewTrajectory, except the origin of the
* end-effector frame follows a straight line, decoupled from the rotational
* motion.
*/
std::vector<Eigen::MatrixXd> CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
/*
* Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
* Inputs:
* thetalist: n-vector of initial joint variables
* dthetalist: n-vector of initial joint rates
* g: Gravity vector g
* Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
* the user should input a zero matrix)
* Mlist: List of link frames {i} relative to {i-1} at the home position
* Glist: Spatial inertia matrices Gi of the links
* Slist: Screw axes Si of the joints in a space frame, in the format
* of a matrix with the screw axes as the columns.
* thetamatd: An Nxn matrix of desired joint variables from the reference trajectory
* dthetamatd: An Nxn matrix of desired joint velocities
* ddthetamatd: An Nxn matrix of desired joint accelerations
* gtilde: The gravity vector based on the model of the actual robot (actual values given above)
* Mtildelist: The link frame locations based on the model of the actual robot (actual values given above)
* Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above)
* Kp: The feedback proportional gain (identical for each joint)
* Ki: The feedback integral gain (identical for each joint)
* Kd: The feedback derivative gain (identical for each joint)
* dt: The timestep between points on the reference trajectory
* intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
* Must be an integer value greater than or equal to 1
*
* Outputs: std::vector of [taumat, thetamat]
* taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques
* corresponds to a single time instant
* thetamat: The N x n matrix of actual joint angles
*/
std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
double, double, double, double, int);
}

12
include/common/utilities/CSVTool.h
View File

@ -6,7 +6,6 @@
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>
@ -61,6 +60,7 @@ public:
void readFile();
void saveFile();
bool _hasFile;
private:
std::string _fileName;
std::fstream _ioStream;
@ -139,10 +139,13 @@ inline CSVTool::CSVTool(std::string fileName, FileType type, int precision)
if(!_ioStream.is_open()){
std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl;
exit(-1);
// exit(-1);
_hasFile = false;
}else{
readFile();
_hasFile = true;
}
readFile();
}
else if(type == FileType::CLEAR_DUMP){
_ioStream.open(_fileName, std::fstream::out);
@ -154,6 +157,7 @@ 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;
return;
}
_lines.clear();
@ -177,7 +181,7 @@ inline void CSVTool::readFile(){
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;
// std::cout << "[ERROR] No such label: " << label << std::endl;
return false;
}else{
_lines.at(_labels[label])->getValues(values);

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

@ -1,261 +0,0 @@
#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
View File

File diff suppressed because it is too large Load Diff

15
include/common/utilities/typeTrans.h
View File

@ -5,21 +5,6 @@
#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){

69
include/control/CtrlComponents.h
View File

@ -1,37 +1,20 @@
#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
#include "common/math/robotics.h"
struct CtrlComponents{
public:
CtrlComponents(){}
~CtrlComponents(){
delete lowCmd;
delete lowState;
delete ioInter;
delete armModel;
delete stateCSV;
#ifdef COMPILE_DEBUG
delete plot;
#endif // COMPILE_DEBUG
}
CtrlComponents();
~CtrlComponents();
int dof;
std::string armConfigPath;
@ -40,45 +23,25 @@ public:
IOInterface *ioInter;
ArmDynKineModel *armModel;
CSVTool *stateCSV;
#ifdef COMPILE_DEBUG
PyPlot *plot;
#endif // COMPILE_DEBUG
//config
Control ctrl;
bool _hasGripper;
std::string ctrl_IP;
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
}
void sendRecv();
void geneObj();
bool isRecord;
void writeData();
private:
Vec3 _endPosLocal;
double _endEffectorMass;
Vec3 _endEffectorCom;
Mat3 _endEffectorInertia;
};
#endif // CTRLCOMPONENTS_H

11
include/interface/IOInterface.h
View File

@ -4,9 +4,9 @@
#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>
#include "common/math/robotics.h"
class IOInterface{
public:
@ -19,17 +19,16 @@ public:
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(){};
virtual bool calibration(){return false;};
#ifdef CTRL_BY_SDK
SendCmd getSendCmd(){return _cmdPanel->getSendCmd();};
void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);};
#endif
bool isDisConnect = false;// udp disconnection
bool _isDisConnect[7];// joint(motor) disconnection
protected:
uint16_t _isDisConnectCnt[7];
CmdPanel *_cmdPanel;
};
#endif //IOINTERFACE_H

21
include/interface/IOROS.h
View File

@ -11,25 +11,23 @@
class IOROS : public IOInterface{
public:
IOROS(CmdPanel *cmdPanel);
IOROS(CmdPanel *cmdPanel, bool hasGripper);
~IOROS();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
// void setGripper(double position, double force){};
// void getGripper(double &position, double &force){};
private:
bool _hasGripper;
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
// #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
// #else
// 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
void _sendCmd(const LowlevelCmd *cmd);
void _recvState(LowlevelState *state);
void _initRecv();
@ -42,7 +40,6 @@ private:
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

34
include/interface/IOUDP.h
View File

@ -4,38 +4,9 @@
#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(CmdPanel *cmdPanel, const char* IP, bool hasGripper);
~IOUDP();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
// void setGripper(int position, int force);
@ -50,6 +21,9 @@ private:
size_t _motorNum;
size_t _jointNum;
uint8_t _singleState;
uint8_t _selfCheck[10];
};
#endif // IOUDP_H

26
include/message/LowlevelCmd.h
View File

@ -16,24 +16,11 @@ public:
std::vector<double> kp;
std::vector<double> kd;
std::vector<std::vector<double>> q_cmd_data;
std::vector<std::vector<double>> dq_cmd_data;
std::vector<std::vector<double>> tau_cmd_data;
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();
~LowlevelCmd(){}
void setZeroDq();
@ -51,10 +38,15 @@ public:
void setGripperGain(float KP, float KW);
void setGripperZeroGain();
void setGripperQ(double qInput);
double getGripperQ();
void setGripperQd(double qdInput);
double getGripperQd();
void setGripperTau(double tauInput);
double getGripperTau();
Vec6 getQ();
Vec6 getQd();
void resizeGripper();
};

154
include/message/LowlevelState.h
View File

@ -9,59 +9,12 @@
#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;
}
LowlevelState(double dt);
~LowlevelState();
// MotorState motorState[12];
std::vector<double> q;
@ -69,99 +22,36 @@ public:
std::vector<double> ddq;
std::vector<double> tau;
std::vector<float> temperature;
std::vector<uint8_t> errorstate;
std::vector<double> qFiltered;
std::vector<double> dqFiltered;
std::vector<double> tauFiltered;
std::vector<std::vector<double>> q_state_data;
std::vector<std::vector<double>> dq_state_data;
std::vector<std::vector<double>> tau_state_data;
// 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);
}
void resizeGripper(double dt);
void runFilter();
Vec6 getQ();
Vec6 getQd();
Vec6 getQdd();
Vec6 getTau();
Vec6 getQFiltered();
Vec6 getQdFiltered();
Vec6 getTauFiltered();
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

18
include/message/UserValue.h
View File

@ -17,30 +17,12 @@ struct UserValue{
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(){

14
include/model/ArmDynKineModel.h
View File

@ -1,3 +1,4 @@
#ifndef ARMDYNCKINEMODEL_H
#define ARMDYNCKINEMODEL_H
@ -25,7 +26,7 @@ public:
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
HomoMat forwardKinematics(Vec6 q, int index=6, bool isExp = false); // index from 0 to 5, used for fourth version
bool _useIKSolver = false;
protected:
@ -111,4 +112,13 @@ public:
};
#endif // ARMDYNCKINEMODEL_H
class Z1PlusDynKineModel : public ArmDynKineModel{
public:
Z1PlusDynKineModel(Vec3 endPosLocal = Vec3::Zero(),
double endEffectorMass = 0.0,
Vec3 endEffectorCom = Vec3::Zero(),
Mat3 endEffectorInertia = Mat3::Zero());
~Z1PlusDynKineModel(){}
};
#endif

6
include/trajectory/EndCircleTraj.h
View File

@ -16,10 +16,16 @@ public:
void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit,
Vec3 axisDirection, double maxSpeed, double angle,
bool keepOrientation = true);
void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP,
double maxSpeed);
private:
void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
Vec3 _center;
double _radius;
Vec6 _middlePosture;
Vec6 _middleQ;
HomoMat _middleHomo;
HomoMat _centerHomo;
HomoMat _initHomoToCenter;
RotMat _initOri;

14
include/trajectory/EndHomoTraj.h
View File

@ -18,11 +18,17 @@ protected:
HomoMat _cmdHomo;
Vec6 _cmdTwist;
Vec6 _qPast;
// double _currentTime;
Vec6 _deltaPosture;
Vec3 _omgtheta;
double _theta;
Mat3 _startR, _endR, _delatR, _currentR;
Vec3 _startp, _endp, _currentp;
SCurve *_sCurve;
private:
bool checkInSingularity();
void _checkAngleValid(const Vec6 &q, int pointOrder);
bool _checkJointAngleValid(const double &q, int jointOrder);
};
#endif // ENDHOMOTRAJ_H
#endif // ENDHOMOTRAJ_H

6
include/trajectory/EndLineTraj.h
View File

@ -12,7 +12,8 @@ public:
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);
void setEndLineTraj(Vec6 startP, Vec6 endP, double maxMovingSpeed, double maxTurningSpeed);
void setEndLineTraj(std::string startName, std::string endName, double maxMovingSpeed, double maxTurningSpeed);
private:
bool _getEndTraj(HomoMat &homo, Vec6 &twist);
@ -22,9 +23,8 @@ private:
Vec3 _currentDistance, _currentAngle;
Vec3 _currentVelocity, _currentOmega;
SCurve _posCurve;
SCurve _oriCurve;
};
#endif // ENDLINETRAJ_H
#endif

8
include/trajectory/JointSpaceTraj.h
View File

@ -5,6 +5,7 @@
#include <trajectory/Trajectory.h>
#include <string>
#include "control/CtrlComponents.h"
#include "trajectory/SCurve.h"
class JointSpaceTraj : public Trajectory{
public:
@ -13,10 +14,11 @@ public:
JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
~JointSpaceTraj(){}
void setGripper(double startQ, double endQ);
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);
bool 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);
@ -33,10 +35,6 @@ private:
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;

32
include/trajectory/Trajectory.h
View File

@ -13,6 +13,9 @@ public:
_csvState = ctrlComp->stateCSV;
_dof = _armModel->getDOF();
_hasKinematic = true;
_jointMaxQ = _armModel->getJointQMax();
_jointMinQ = _armModel->getJointQMin();
_jointMaxSpeed = _armModel->getJointSpeedMax();
}
Trajectory(ArmDynKineModel *armModel){
_ctrlComp = nullptr;
@ -22,26 +25,32 @@ public:
if(_dof == 6){
_hasKinematic = true;
}
_jointMaxQ = _armModel->getJointQMax();
_jointMinQ = _armModel->getJointQMin();
_jointMaxSpeed = _armModel->getJointSpeedMax();
}
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){};
virtual bool getJointCmd(Vec6 &q, Vec6 &qd){return false;};
virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){return false;};
virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){return false;};
void restart(){
_pathStarted = false;
_reached = false;
_qPast = _startQ;
}
void setGripper(double startQ, double endQ){
virtual void setGripper(double startQ, double endQ){
_gripperStartQ = startQ;
_gripperEndQ = endQ;
}
bool correctYN(){return _settingCorrect;}
Vec6 getStartQ(){return _startQ;}
Vec6 getEndQ(){return _endQ;}
double getEndGripperQ(){return _gripperEndQ;};
double getStartGripperQ(){return _gripperStartQ;};
HomoMat getStartHomo(){
if(_hasKinematic){
return _startHomo;
@ -59,6 +68,14 @@ public:
}
}
Vec6 getEndPosture(){
if(_hasKinematic){
return _endPosture;
}else{
std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
exit(-1);
}
}
double getPathTime(){return _pathTime;}
protected:
CtrlComponents *_ctrlComp;
@ -72,6 +89,9 @@ protected:
double _currentTime;
double _pathTime;
double _tCost;
Vec6 _qPast;
Vec6 _startPosture, _endPosture;
Vec6 _startQ, _endQ;
HomoMat _startHomo, _endHomo;
@ -81,6 +101,10 @@ protected:
size_t _dof;
bool _hasKinematic;
std::vector<double> _jointMaxQ;
std::vector<double> _jointMinQ;
std::vector<double> _jointMaxSpeed;
void _runTime(){
_currentTime = getTimeSecond();

3
include/trajectory/TrajectoryManager.h
View File

@ -22,6 +22,9 @@ public:
void emptyTraj();
Vec6 getStartQ();
Vec6 getEndQ();
Vec6 getEndPosture();
double getEndGripperQ();
double getStartGripperQ();
HomoMat getEndHomo();
// bool checkTrajectoryContinuous();
private:

BIN
lib/libZ1_JOYSTICK_ROS_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_JOYSTICK_UDP_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_KEYBOARD_ROS_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_KEYBOARD_UDP_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_ROS_Linux64.so Normal file
View File

Binary file not shown.

BIN
lib/libZ1_SDK_ROS_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_SDK_UDP_Linux64.so
View File

Binary file not shown.

BIN
lib/libZ1_UDP_Linux64.so Normal file
View File

Binary file not shown.

223
main.cpp
View File

@ -4,7 +4,6 @@
#include <sched.h>
#include <iomanip>
#include "control/CtrlComponents.h"
#include "model/ArmReal.h"
#include "model/ArmDynKineModel.h"
#include "interface/IOUDP.h"
@ -24,6 +23,7 @@
#include "FSM/State_Calibration.h"
#include "FSM/State_LowCmd.h"
#include "FSM/FiniteStateMachine.h"
#include "FSM/State_SetTraj.h"
#include "unitree_arm_sdk/unitree_arm_sdk.h"
@ -35,6 +35,7 @@ void ShutDown(int sig){
running = false;
}
//set real-time program
void setProcessScheduler(){
pid_t pid = getpid();
sched_param param;
@ -53,144 +54,93 @@ int main(int argc, char **argv){
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));
CtrlComponents *ctrlComp = new CtrlComponents();
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));
if(ctrlComp->ctrl == Control::_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 StateAction("l", (int)ArmFSMStateName::SETTRAJ));
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));
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);
}else if(ctrlComp->ctrl == Control::_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));
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("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);
}else if(ctrlComp->ctrl == Control::_JOYSTICK){
events.push_back(new StateAction("r12", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));
events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART));
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));
events.push_back(new ValueAction("left_up", "left_down", 1.0));//Tran_Y
events.push_back(new ValueAction("left_left", "left_right", -1.0));//Tran_X, inverse
events.push_back(new ValueAction("up", "down", 1.0));//Tran_Z
events.push_back(new ValueAction("right_up", "right_down", 1.0));//Rot_Y
events.push_back(new ValueAction("right_left", "right_right", 1.0));//Rot_x
events.push_back(new ValueAction("Y", "A", 1.0));//Rot_Z
events.push_back(new ValueAction("right", "left", 1.0));//girpper, close-open
cmdPanel = new UnitreeJoystick(events, emptyAction);
#endif
cmdPanel = new UnitreeJoystick(events, emptyAction);
}
#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;
ctrlComp->ioInter = new IOUDP(cmdPanel, ctrlComp->ctrl_IP.c_str(), ctrlComp->_hasGripper);
#elif defined ROS
ctrlComp->ioInter = new IOROS(cmdPanel, ctrlComp->_hasGripper);
#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;
@ -208,41 +158,20 @@ std::cout << "add plot" << std::endl;
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
states.push_back(new State_SetTraj(ctrlComp));
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
ctrlComp->writeData();
#endif
delete ctrlComp;
return 0;

BIN
thirdparty/eigen-3.3.9.zip vendored
View File

Binary file not shown.

2
thirdparty/quadProgpp/include/Array.hh vendored
View File

@ -1785,7 +1785,7 @@ void svd(const Matrix<T>& A, Matrix<T>& U, Vector<T>& W, Matrix<T>& V)
for (unsigned int its = 0; its < max_its; its++)
{
flag = true;
for (l = k; l >= 0; l--) // FIXME: in NR it was l >= 1 but there subscripts start from one
for (l = k; l >= 0; l--)
{ // Test for splitting
nm = l - 1; // Note that rV[0] is always zero
if ((T)(fabs(rv1[l]) + anorm) == anorm)

1
thirdparty/quadProgpp/src/QuadProg++.cc vendored
View File

@ -338,7 +338,6 @@ l2a:/* Step 2a: determine step direction */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
return inf;
}
/* case (ii): step in dual space */

BIN
thirdparty/rbdl-2.6.0.zip vendored
View File

Binary file not shown.

305
thirdparty/tinyxml/include/tinystr.h vendored Normal file
View File

@ -0,0 +1,305 @@
/*
www.sourceforge.net/projects/tinyxml
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#ifndef TIXML_USE_STL
#ifndef TIXML_STRING_INCLUDED
#define TIXML_STRING_INCLUDED
#include <assert.h>
#include <string.h>
/* The support for explicit isn't that universal, and it isn't really
required - it is used to check that the TiXmlString class isn't incorrectly
used. Be nice to old compilers and macro it here:
*/
#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
// Microsoft visual studio, version 6 and higher.
#define TIXML_EXPLICIT explicit
#elif defined(__GNUC__) && (__GNUC__ >= 3 )
// GCC version 3 and higher.s
#define TIXML_EXPLICIT explicit
#else
#define TIXML_EXPLICIT
#endif
/*
TiXmlString is an emulation of a subset of the std::string template.
Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
Only the member functions relevant to the TinyXML project have been implemented.
The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
a string and there's no more room, we allocate a buffer twice as big as we need.
*/
class TiXmlString
{
public :
// The size type used
typedef size_t size_type;
// Error value for find primitive
static const size_type npos; // = -1;
// TiXmlString empty constructor
TiXmlString () : rep_(&nullrep_)
{
}
// TiXmlString copy constructor
TiXmlString ( const TiXmlString & copy) : rep_(0)
{
init(copy.length());
memcpy(start(), copy.data(), length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
{
init( static_cast<size_type>( strlen(copy) ));
memcpy(start(), copy, length());
}
// TiXmlString constructor, based on a string
TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
{
init(len);
memcpy(start(), str, len);
}
// TiXmlString destructor
~TiXmlString ()
{
quit();
}
TiXmlString& operator = (const char * copy)
{
return assign( copy, (size_type)strlen(copy));
}
TiXmlString& operator = (const TiXmlString & copy)
{
return assign(copy.start(), copy.length());
}
// += operator. Maps to append
TiXmlString& operator += (const char * suffix)
{
return append(suffix, static_cast<size_type>( strlen(suffix) ));
}
// += operator. Maps to append
TiXmlString& operator += (char single)
{
return append(&single, 1);
}
// += operator. Maps to append
TiXmlString& operator += (const TiXmlString & suffix)
{
return append(suffix.data(), suffix.length());
}
// Convert a TiXmlString into a null-terminated char *
const char * c_str () const { return rep_->str; }
// Convert a TiXmlString into a char * (need not be null terminated).
const char * data () const { return rep_->str; }
// Return the length of a TiXmlString
size_type length () const { return rep_->size; }
// Alias for length()
size_type size () const { return rep_->size; }
// Checks if a TiXmlString is empty
bool empty () const { return rep_->size == 0; }
// Return capacity of string
size_type capacity () const { return rep_->capacity; }
// single char extraction
const char& at (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// [] operator
char& operator [] (size_type index) const
{
assert( index < length() );
return rep_->str[ index ];
}
// find a char in a string. Return TiXmlString::npos if not found
size_type find (char lookup) const
{
return find(lookup, 0);
}
// find a char in a string from an offset. Return TiXmlString::npos if not found
size_type find (char tofind, size_type offset) const
{
if (offset >= length()) return npos;
for (const char* p = c_str() + offset; *p != '\0'; ++p)
{
if (*p == tofind) return static_cast< size_type >( p - c_str() );
}
return npos;
}
void clear ()
{
//Lee:
//The original was just too strange, though correct:
// TiXmlString().swap(*this);
//Instead use the quit & re-init:
quit();
init(0,0);
}
/* Function to reserve a big amount of data when we know we'll need it. Be aware that this
function DOES NOT clear the content of the TiXmlString if any exists.
*/
void reserve (size_type cap);
TiXmlString& assign (const char* str, size_type len);
TiXmlString& append (const char* str, size_type len);
void swap (TiXmlString& other)
{
Rep* r = rep_;
rep_ = other.rep_;
other.rep_ = r;
}
private:
void init(size_type sz) { init(sz, sz); }
void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
char* start() const { return rep_->str; }
char* finish() const { return rep_->str + rep_->size; }
struct Rep
{
size_type size, capacity;
char str[1];
};
void init(size_type sz, size_type cap)
{
if (cap)
{
// Lee: the original form:
// rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
// doesn't work in some cases of new being overloaded. Switching
// to the normal allocation, although use an 'int' for systems
// that are overly picky about structure alignment.
const size_type bytesNeeded = sizeof(Rep) + cap;
const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int );
rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
rep_->str[ rep_->size = sz ] = '\0';
rep_->capacity = cap;
}
else
{
rep_ = &nullrep_;
}
}
void quit()
{
if (rep_ != &nullrep_)
{
// The rep_ is really an array of ints. (see the allocator, above).
// Cast it back before delete, so the compiler won't incorrectly call destructors.
delete [] ( reinterpret_cast<int*>( rep_ ) );
}
}
Rep * rep_;
static Rep nullrep_;
} ;
inline bool operator == (const TiXmlString & a, const TiXmlString & b)
{
return ( a.length() == b.length() ) // optimization on some platforms
&& ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare
}
inline bool operator < (const TiXmlString & a, const TiXmlString & b)
{
return strcmp(a.c_str(), b.c_str()) < 0;
}
inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; }
inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
TiXmlString operator + (const TiXmlString & a, const char* b);
TiXmlString operator + (const char* a, const TiXmlString & b);
/*
TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
Only the operators that we need for TinyXML have been developped.
*/
class TiXmlOutStream : public TiXmlString
{
public :
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const TiXmlString & in)
{
*this += in;
return *this;
}
// TiXmlOutStream << operator.
TiXmlOutStream & operator << (const char * in)
{
*this += in;
return *this;
}
} ;
#endif // TIXML_STRING_INCLUDED
#endif // TIXML_USE_STL

1805
thirdparty/tinyxml/include/tinyxml.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

111
thirdparty/tinyxml/src/tinystr.cpp vendored Normal file
View File

@ -0,0 +1,111 @@
/*
www.sourceforge.net/projects/tinyxml
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#ifndef TIXML_USE_STL
#include "tinyxml/include/tinystr.h"
// Error value for find primitive
const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
// Null rep.
TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
void TiXmlString::reserve (size_type cap)
{
if (cap > capacity())
{
TiXmlString tmp;
tmp.init(length(), cap);
memcpy(tmp.start(), data(), length());
swap(tmp);
}
}
TiXmlString& TiXmlString::assign(const char* str, size_type len)
{
size_type cap = capacity();
if (len > cap || cap > 3*(len + 8))
{
TiXmlString tmp;
tmp.init(len);
memcpy(tmp.start(), str, len);
swap(tmp);
}
else
{
memmove(start(), str, len);
set_size(len);
}
return *this;
}
TiXmlString& TiXmlString::append(const char* str, size_type len)
{
size_type newsize = length() + len;
if (newsize > capacity())
{
reserve (newsize + capacity());
}
memmove(finish(), str, len);
set_size(newsize);
return *this;
}
TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
{
TiXmlString tmp;
tmp.reserve(a.length() + b.length());
tmp += a;
tmp += b;
return tmp;
}
TiXmlString operator + (const TiXmlString & a, const char* b)
{
TiXmlString tmp;
TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
tmp.reserve(a.length() + b_len);
tmp += a;
tmp.append(b, b_len);
return tmp;
}
TiXmlString operator + (const char* a, const TiXmlString & b)
{
TiXmlString tmp;
TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
tmp.reserve(a_len + b.length());
tmp.append(a, a_len);
tmp += b;
return tmp;
}
#endif // TIXML_USE_STL

1886
thirdparty/tinyxml/src/tinyxml.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

52
thirdparty/tinyxml/src/tinyxmlerror.cpp vendored Normal file
View File

@ -0,0 +1,52 @@
/*
www.sourceforge.net/projects/tinyxml
Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any
damages arising from the use of this software.
Permission is granted to anyone to use this software for any
purpose, including commercial applications, and to alter it and
redistribute it freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and
must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*/
#include "tinyxml/include/tinyxml.h"
// The goal of the seperate error file is to make the first
// step towards localization. tinyxml (currently) only supports
// english error messages, but the could now be translated.
//
// It also cleans up the code a bit.
//
const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
{
"No error",
"Error",
"Failed to open file",
"Error parsing Element.",
"Failed to read Element name",
"Error reading Element value.",
"Error reading Attributes.",
"Error: empty tag.",
"Error reading end tag.",
"Error parsing Unknown.",
"Error parsing Comment.",
"Error parsing Declaration.",
"Error document empty.",
"Error null (0) or unexpected EOF found in input stream.",
"Error parsing CDATA.",
"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
};

1638
thirdparty/tinyxml/src/tinyxmlparser.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff