commit e64a878cdfffd1f56be80ca67fcf17ca23ac82cb Author: David Zhai <1003429115@qq.com> Date: Wed Jul 20 11:11:38 2022 +0800 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..62eb83c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,144 @@ +cmake_minimum_required(VERSION 3.0) +project(z1_controller) +add_definitions(-std=c++14) +set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3") + +# set(GRIPPER MASS3KG) # 3kg +# set(GRIPPER NONE) # No Gripper +set(GRIPPER UNITREE_GRIPPER) # The gripper made by unitree + +set(COMMUNICATION UDP) # UDP +# set(COMMUNICATION ROS) # ROS + +# set(CTRL_PANEL KEYBOARD) # control by keyboard +set(CTRL_PANEL SDK) # control by SDK +# set(CTRL_PANEL JOYSTICK) # control by joystick + +if(${COMMUNICATION} STREQUAL "UDP") + add_definitions(-DUDP) + set(SIMULATION OFF) + set(REAL_ROBOT ON) +elseif(${COMMUNICATION} STREQUAL "ROS") + add_definitions(-DROS) + set(SIMULATION ON) + set(REAL_ROBOT OFF) +else() + message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error") +endif() + +if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT))) + message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF") +endif() + +if(REAL_ROBOT) + add_definitions(-DCOMPILE_WITH_REAL_ROBOT) +endif() + +if(SIMULATION) + add_definitions(-DCOMPILE_WITH_SIMULATION) + add_definitions(-DRUN_ROS) + find_package(gazebo REQUIRED) +endif() + +if(DEBUG) + add_definitions(-DCOMPILE_DEBUG) + find_package(Python2 COMPONENTS Interpreter Development NumPy) +endif() + + +if(${GRIPPER} STREQUAL "NONE") + add_definitions(-DNO_GRIPPER) +elseif(${GRIPPER} STREQUAL "AG95") + add_definitions(-DAG95) +elseif(${GRIPPER} STREQUAL "MASS3KG") + add_definitions(-DMASS3KG) +elseif(${GRIPPER} STREQUAL "UNITREE_GRIPPER") + add_definitions(-DUNITREE_GRIPPER) +else() + message(FATAL_ERROR "[CMake ERROR] The GRIPPER is error") +endif() + + +if(${CTRL_PANEL} STREQUAL "KEYBOARD") + add_definitions(-DCTRL_BY_KEYBOARD) +elseif(${CTRL_PANEL} STREQUAL "JOYSTICK") + add_definitions(-DCTRL_BY_JOYSTICK) +elseif(${CTRL_PANEL} STREQUAL "SDK") + add_definitions(-DCTRL_BY_SDK) +else() + message(FATAL_ERROR "[CMake ERROR] The CTRL_PANEL is error") +endif() + +if(SIMULATION) + find_package(catkin REQUIRED COMPONENTS + controller_manager + genmsg + joint_state_controller + robot_state_publisher + roscpp + gazebo_ros + std_msgs + tf + geometry_msgs + ) + + catkin_package( + CATKIN_DEPENDS + ) + + include_directories( + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ) + + link_directories( + ${GAZEBO_LIBRARY_DIRS} + ) + + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") + +endif() + +set(EIGEN_PATH /usr/include/eigen3) +include_directories( + include + thirdparty + ../z1_sdk/include + ${Boost_INCLUDE_DIR} + ${EIGEN_PATH} +) + +link_directories(lib) + +# # add_library +# file(GLOB_RECURSE ADD_SRC_LIST +# src/*/*.cpp +# src/*/*/*.cpp +# thirdparty/*/src/*.cpp +# thirdparty/*/src/*.cc +# ../z1_sdk/src/*.cpp +# ) + +# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) # SHARED:CMAKE_LIBRARY_OUTPUT_DIRECTORY STATIC:CMAKE_ARCHIVE_OUTPUT_DIRECTORY +# add_library(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 SHARED ${ADD_SRC_LIST}) +# target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${catkin_LIBRARIES} rbdl -pthread) +# if(SIMULATION) +# add_dependencies(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${${PEOJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# endif() + + +# add_executable and target_link_libraries +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build) # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY +add_executable(z1_ctrl main.cpp) +target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${CTRL_PANEL}_${COMMUNICATION}_Linux64.so) + + +# debug +if(DEBUG) + find_package(PythonLibs REQUIRED) + include_directories(${PYTHON_INCLUDE_DIRS}) + target_link_libraries(Z1_${CTRL_PANEL}_${COMMUNICATION}_Linux64 ${PYTHON_LIBRARIES}) +endif() + + +# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..4e14b59 --- /dev/null +++ b/README.md @@ -0,0 +1,223 @@ +# v3.6.1 +The z1_controller is mainly used to control the robot arm and communicate with the Z1 SDK + +## Notice +support robot: Z1(v3.6.1) +not support robot: Z1(v3.4, v3.5) + +## Dependencies +- [ROS](https://www.ros.org/)（Melodic） +- build-essential + +```bash +sudo apt install build-essential +``` + +- Boost (version 1.5.4 or higher) + +```bash +dpkg -S /usr/include/boost/version.hpp # check boost version +sudo apt install libboost-dev # install boost +``` + +- CMake (version 2.8.3 or higher) + +```bash +cmake --version # check cmake version +sudo apt install cmake # install cmake +``` + +- [Eigen](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) (version 3.3.9 or higher) + +```bash +cd eigen-3.3.9 +mkdir build && cd build +cmake .. +sudo make install +sudo ln -s /usr/local/include/eigen3 /usr/include/eigen3 +sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen +``` + +- [RBDL](https://github.com/rbdl/rbdl/releases/tag/v2.6.0) (version 2.6.0) + +```bash +cd rbdl-2.6.0 +mkdir build && cd build +cmake -D CMAKE_BUILD_TYPE=Release .. +sudo make install +sudo sh -c "echo '/usr/local/lib' >> /etc/ld.so.conf" +sudo ldconfig +``` + +## Build + +```bash +# config //z1_controller/CMakeList.txt +cd //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 //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
(right hand) | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/negative | positive
/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
/backward | right
/left | up
/down | roll
(right hand) | pitch
(right hand) | yaw
(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 //z1_controller/build + sudo ./z1_ctrl + ``` + +- Sencond, build the z1_sdk, and then open another terminal to run example. + + ``` + cd //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 +``` + diff --git a/config/savedArmStates.csv b/config/savedArmStates.csv new file mode 100755 index 0000000..63de734 --- /dev/null +++ b/config/savedArmStates.csv @@ -0,0 +1,59 @@ +forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.424746, 0.002468, 0.999923, 0.012173, 0.000358, 0.026329, -0.012234, 0.999578, 0.394252, 0.000000, 0.000000, 0.000000, 1.000000, +start, -0.000336, 0.001634, 0.008171, 0.064640, 0.000248, 0.000230, 0.997230, 0.000106, 0.074376, 0.085128, -0.000087, 1.000000, -0.000255, 0.000008, -0.074376, 0.000248, 0.997230, 0.133277, 0.000000, 0.000000, 0.000000, 1.000000, +highRiseLeft, -0.756000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, 0.403000, 0.633000, 0.661000, -0.102000, 0.476000, -0.762000, 0.440000, 0.178000, 0.782000, 0.137000, -0.608000, 0.610000, 0.000000, 0.000000, 0.000000, 1.000000, +lowPickLeft, 0.590000, 2.088000, -0.899000, 0.591000, 0.906000, -0.641000, -0.558000, -0.521000, 0.646000, 0.221000, 0.155000, 0.699000, 0.698000, 0.198000, -0.816000, 0.489000, -0.309000, 0.071000, 0.000000, 0.000000, 0.000000, 1.000000, +startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000, +videoHighRise, 0.804000, 0.500000, -1.496000, -0.942000, -0.879000, -1.004000, -0.471646, 0.768269, -0.432796, -0.179591, 0.408109, 0.625271, 0.665195, -0.100048, 0.781663, 0.137109, -0.608444, 0.610103, 0.000000, 0.000000, 0.000000, 1.000000, +videoToLeft, 2.316499, 2.223266, -1.131086, 0.590578, 0.905720, -0.640975, -0.121092, -0.577184, -0.807586, -0.278129, -0.514682, -0.659161, 0.548278, 0.239010, -0.848787, 0.482042, -0.217247, 0.058040, 0.000000, 0.000000, 0.000000, 1.000000, +video_02, -0.042000, 1.416000, -1.484000, 0.000000, -0.279000, 0.000000, 0.939568, 0.041988, -0.339778, 0.322181, -0.039485, 0.999118, 0.014279, -0.013540, 0.340078, 0.000000, 0.940397, 0.545628, 0.000000, 0.000000, 0.000000, 1.000000, +video_03, 0.918960, 2.171685, -1.471256, 0.000000, 0.427000, 0.000000, 0.260242, -0.794971, 0.547992, 0.300209, 0.341030, 0.606647, 0.718107, 0.393404, -0.903312, 0.000000, 0.428984, 0.149885, 0.000000, 0.000000, 0.000000, 1.000000, +video_01, -0.871858, 0.870714, -0.722365, 0.055117, -0.678396, -0.070888, 0.528563, 0.793043, -0.302826, 0.095419, -0.682618, 0.609129, 0.403725, -0.118699, 0.504632, -0.006679, 0.863309, 0.420252, 0.000000, 0.000000, 0.000000, 1.000000, +test, 0.368525, 2.393267, -1.148771, -0.953744, 0.368296, 0.368360, 0.704081, -0.517416, 0.486365, 0.522431, 0.657831, 0.733192, -0.172300, 0.259328, -0.267449, 0.441259, 0.856599, 0.093448, 0.000000, 0.000000, 0.000000, 1.000000, +repeatLow, 0.000000, 1.931603, -0.835792, 0.000000, 0.390233, 0.000000, 0.084651, 0.000000, 0.996411, 0.311108, 0.000000, 1.000000, 0.000000, 0.000000, -0.996411, 0.000000, 0.084651, 0.092545, 0.000000, 0.000000, 0.000000, 1.000000, +repeatHigh, 0.000000, 1.940186, -1.486322, 0.000000, 0.391440, 0.000000, 0.663504, 0.000000, 0.748173, 0.474034, 0.000000, 1.000000, 0.000000, 0.000000, -0.748173, 0.000000, 0.663504, 0.269367, 0.000000, 0.000000, 0.000000, 1.000000, +winestart, -0.550922, 2.147376, -0.743038, -0.055447, -1.430589, 1.647373, 0.879196, -0.054860, -0.473292, 0.332236, -0.475751, -0.046881, -0.878330, -0.197922, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000, +wineget, -0.609784, 2.218792, -0.937567, -0.117771, -1.309136, 1.671756, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.113427, 0.000000, 0.000000, 0.000000, 1.000000, +wine1, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.639763, 0.995012, -0.045873, -0.088587, 0.410574, 0.006629, 0.916446, -0.400104, -0.113175, 0.099539, 0.397521, 0.912178, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, +wine2, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 0.319763, 0.995012, -0.015678, -0.098520, 0.410574, 0.006629, 0.995782, -0.091510, -0.113175, 0.099539, 0.090400, 0.990919, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, +wine3, -0.346864, 1.931170, -0.980517, -0.398801, -1.091111, 1.829763, 0.995012, -0.099290, 0.009662, 0.410574, 0.006629, -0.030838, -0.999502, -0.113175, 0.099539, 0.994581, -0.030026, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, +wine4, -0.609784, 2.103729, -0.954345, -0.123154, -1.178247, 1.688475, 0.879196, -0.054860, -0.473292, 0.366936, -0.475751, -0.046881, -0.878330, -0.243022, 0.025997, 0.997393, -0.067317, 0.155627, 0.000000, 0.000000, 0.000000, 1.000000, +wine2_5, -0.270810, 1.913207, -0.952264, -0.313249, -1.085456, 0.511294, 0.995012, -0.038831, -0.091892, 0.410574, 0.006629, 0.944836, -0.327477, -0.086775, 0.099539, 0.325234, 0.940380, 0.226441, 0.000000, 0.000000, 0.000000, 1.000000, +doorstart, 0.431042, 1.563797, -1.529086, 1.322867, -0.534746, -1.320230, 0.991589, 0.086371, -0.096392, 0.358317, -0.087764, 0.996088, -0.010297, 0.112472, 0.095125, 0.018670, 0.995290, 0.496540, 0.000000, 0.000000, 0.000000, 1.000000, +doorstartflat, 0.520059, 1.879836, -1.920901, 1.443620, -0.682288, -1.471548, 0.980922, 0.157107, -0.114496, 0.436491, -0.159056, 0.987237, -0.008031, 0.180614, 0.111773, 0.026089, 0.993391, 0.503541, 0.000000, 0.000000, 0.000000, 1.000000, +doorpre, 0.431042, 1.475972, -1.314848, 1.466425, -0.591235, -1.494022, 0.984623, 0.149591, 0.090224, 0.332506, -0.157354, 0.983780, 0.086116, 0.094216, -0.075878, -0.098989, 0.992191, 0.441392, 0.000000, 0.000000, 0.000000, 1.000000, +doorstartflat2, 1.085467, 1.374296, -1.381275, 1.454993, -1.117510, -1.514257, 0.993818, 0.028773, -0.107233, 0.198932, -0.029880, 0.999515, -0.008726, 0.193041, 0.106930, 0.011876, 0.994196, 0.503075, 0.000000, 0.000000, 0.000000, 1.000000, +doorpre2, 1.277563, 1.233555, -1.023053, 1.407057, -1.338097, -1.314977, 0.993818, 0.028774, -0.107232, 0.147332, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.426275, 0.000000, 0.000000, 0.000000, 1.000000, +doorstart2, 0.894370, 1.364286, -1.338585, 1.417931, -0.932419, -1.446631, 0.993818, 0.028774, -0.107232, 0.233132, -0.029881, 0.999515, -0.008725, 0.168441, 0.106929, 0.011875, 0.994196, 0.492875, 0.000000, 0.000000, 0.000000, 1.000000, +swardhit01, 0.046000, 1.293000, -1.235000, 0.000000, -0.616000, 0.000000, 0.847418, -0.045984, -0.528931, 0.278086, 0.039009, 0.998942, -0.024348, 0.012801, 0.529491, 0.000000, 0.848316, 0.518108, 0.000000, 0.000000, 0.000000, 1.000000, +swardmiddle01, 0.824000, 1.151000, -0.931000, 0.000000, -0.604000, 0.000000, 0.629821, -0.733869, -0.254484, 0.163849, 0.680424, 0.679291, -0.274931, 0.177013, 0.374632, 0.000000, 0.927174, 0.438554, 0.000000, 0.000000, 0.000000, 1.000000, +swardstart01, 0.000000, 0.861000, -0.455000, 0.000000, -0.518000, 0.000000, 0.993735, 0.000000, -0.111766, 0.154853, 0.000000, 1.000000, 0.000000, 0.000000, 0.111766, 0.000000, 0.993735, 0.304846, 0.000000, 0.000000, 0.000000, 1.000000, +swardpush01, -0.246000, 1.701000, -1.361000, 1.042000, -0.442000, 0.000000, 0.806358, 0.402127, -0.433683, 0.403978, -0.583253, 0.419186, -0.695773, -0.138065, -0.097995, 0.813988, 0.572555, 0.385327, 0.000000, 0.000000, 0.000000, 1.000000, +swardhit02, -0.060000, 2.293000, -2.333000, 0.056000, -0.052000, 0.000000, 0.993808, 0.057636, -0.094994, 0.613902, -0.062615, 0.996770, -0.050289, -0.037159, 0.091789, 0.055926, 0.994207, 0.430516, 0.000000, 0.000000, 0.000000, 1.000000, +swardthroat, 0.018000, 1.842000, -2.130000, -1.372000, -1.373000, 1.128000, 0.116093, -0.744644, -0.657288, 0.368208, 0.963437, 0.245315, -0.107751, 0.099110, 0.241479, -0.620746, 0.745897, 0.588202, 0.000000, 0.000000, 0.000000, 1.000000, +swardswing, 0.000000, 1.947000, -0.899000, -1.022000, -0.948000, 0.000000, 0.658370, -0.739194, -0.141916, 0.382308, 0.692974, 0.521661, 0.497652, 0.066664, -0.293829, -0.425983, 0.855688, 0.167033, 0.000000, 0.000000, 0.000000, 1.000000, +swardstart, 0.000000, 1.695335, -1.003223, -0.000000, -0.980112, 0.000000, 0.958814, 0.000000, -0.284035, 0.392642, 0.000000, 1.000000, 0.000000, 0.000000, 0.284035, 0.000000, 0.958814, 0.324838, 0.000000, 0.000000, 0.000000, 1.000000, +swardmiddle02, -0.009336, 1.762476, -0.547022, 0.071902, -1.570413, -0.026650, 0.934527, 0.085894, -0.345372, 0.306754, -0.080568, 0.996304, 0.029776, -0.009775, 0.346653, -0.000001, 0.937993, 0.218133, 0.000000, 0.000000, 0.000000, 1.000000, +swardend, -0.009336, 0.640476, -0.385022, 0.071902, -0.550413, -0.026650, 0.956080, 0.035230, -0.290981, 0.105389, -0.046503, 0.998408, -0.031915, -0.004599, 0.289393, 0.044045, 0.956196, 0.309459, 0.000000, 0.000000, 0.000000, 1.000000, +swardwalk, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000, +nokiaend, -0.106199, 2.620994, -1.475180, 0.022797, -0.839897, -1.203857, 0.946225, -0.232455, 0.224997, 0.559486, -0.117937, 0.399763, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, -0.002630, 0.000000, 0.000000, 0.000000, 1.000000, +nokiamiddle, -0.136319, 2.083721, -1.059621, -0.017853, -0.718157, -1.184266, 0.946225, -0.232455, 0.224996, 0.455086, -0.117937, 0.399764, 0.908999, -0.061283, -0.301247, -0.886653, 0.350851, 0.150371, 0.000000, 0.000000, 0.000000, 1.000000, +nokiastart, 0.000000, 0.861000, -0.455000, 0.000000, -1.068000, 0.000000, 0.788764, 0.000000, -0.614696, 0.135135, 0.000000, 1.000000, 0.000000, 0.000000, 0.614696, 0.000000, 0.788764, 0.353228, 0.000000, 0.000000, 0.000000, 1.000000, +teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000, +djistart, 0.000000, 1.616000, -0.324000, 0.000000, -1.285000, 0.000000, 0.999976, 0.000000, 0.007000, 0.242349, 0.000000, 1.000000, 0.000000, 0.000000, -0.007000, 0.000000, 0.999976, 0.179422, 0.000000, 0.000000, 0.000000, 1.000000, +anyway, 0.242441, 0.267866, -0.016100, -0.008059, -0.122301, 0.005914, 0.962394, -0.241280, 0.124828, 0.050463, 0.239018, 0.970454, 0.033019, 0.012577, -0.129107, -0.001941, 0.991629, 0.153768, 0.000000, 0.000000, 0.000000, 1.000000, +fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000, +fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000, +fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000, +5kg01, -0.582000, 2.072006, -0.858000, 0.000000, 0.241000, 0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.003142, 0.000000, 0.000000, 0.000000, 1.000000, +5kg02, -0.582000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.096511, 0.549696, 0.829771, 0.280359, -0.063507, 0.835365, -0.546015, -0.184485, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000, +5kg03, 0.524000, 1.854695, -0.950446, -0.000000, 0.550757, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.090142, 0.000000, 0.000000, 0.000000, 1.000000, +5kg04, 0.524000, 2.008825, -0.874538, 0.000000, 0.320718, -0.000000, 0.100030, -0.500347, 0.860027, 0.290582, 0.057806, 0.865825, 0.496997, 0.167923, -0.993304, 0.000000, 0.115532, 0.025942, 0.000000, 0.000000, 0.000000, 1.000000, +hold_light, -0.000024, 1.098000, -1.618000, 0.000000, 0.340994, 0.000000, 0.984021, 0.000024, -0.178051, 0.161036, -0.000023, 1.000000, 0.000004, -0.000004, 0.178051, 0.000000, 0.984021, 0.613488, 0.000000, 0.000000, 0.000000, 1.000000, +give_light, -0.000024, 2.396000, -2.156000, 0.000000, 0.340994, 0.000000, 0.835917, 0.000024, 0.548855, 0.631917, -0.000020, 1.000000, -0.000013, -0.000015, -0.548855, 0.000000, 0.835917, 0.261657, 0.000000, 0.000000, 0.000000, 1.000000, +dance_swing_right, 0.987976, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.546488, -0.834914, -0.065344, 0.034540, 0.829009, 0.550381, -0.099125, 0.052396, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000, +dance_swing_left, -0.970024, 0.692000, -1.152000, 0.000000, 0.340994, 0.000000, 0.561282, 0.824899, -0.067113, 0.035475, -0.819065, 0.565280, 0.097936, -0.051768, 0.118725, 0.000000, 0.992927, 0.505625, 0.000000, 0.000000, 0.000000, 1.000000, +001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000, +002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000, +003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000, +starFlat, -0.003451, 0.000501, 0.009250, -0.064964, 0.000525, 0.000590, 0.998472, 0.002895, -0.055186, 0.085395, -0.002921, 0.999996, -0.000400, -0.000217, 0.055184, 0.000560, 0.998476, 0.161033, 0.000000, 0.000000, 0.000000, 1.000000, +yy, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000, diff --git a/include/FSM/BaseState.h b/include/FSM/BaseState.h new file mode 100755 index 0000000..ea15189 --- /dev/null +++ b/include/FSM/BaseState.h @@ -0,0 +1,30 @@ +#ifndef BASESTATE_H +#define BASESTATE_H + +#include + +class BaseState{ +public: + BaseState(int stateNameEnum, std::string stateNameString) + : _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){} + virtual ~BaseState(){}; + + virtual void enter() = 0; + virtual void run() = 0; + virtual void exit() = 0; + virtual int checkChange(int cmd) = 0; + + bool isState(int stateEnum){ + if(_stateNameEnum == stateEnum){ + return true; + }else{ + return false; + } + } + std::string getStateName(){return _stateNameString;} +protected: + int _stateNameEnum; + std::string _stateNameString; +}; + +#endif \ No newline at end of file diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h new file mode 100755 index 0000000..4e825bd --- /dev/null +++ b/include/FSM/FSMState.h @@ -0,0 +1,77 @@ +#ifndef FSMSTATE_H +#define FSMSTATE_H + +#include +#include +#include +#include "control/CtrlComponents.h" +#include "message/LowlevelCmd.h" +#include "message/LowlevelState.h" +#include "common/enumClass.h" +#include "common/math/mathTools.h" +#include "common/math/mathTypes.h" +#include "unitree_arm_sdk/timer.h" +#include "model/ArmDynKineModel.h" + +#include "FSM/FiniteStateMachine.h" + +class FSMState : public BaseState{ +public: + FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString); + virtual ~FSMState(){} + + virtual void enter() = 0; + virtual void run() = 0; + virtual void exit() = 0; + virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;} + +protected: + void _armCtrl(); + void _tauDynForward(); + void _tauFriction(); + void _qdFeedBack(); + bool _collisionTest(); + void _jointPosProtect(); + void _jointSpeedProtect(); +#ifdef UNITREE_GRIPPER + void _gripperCmd(); + void _gripperCtrl(); + double _gripperPos; + double _gripperW; + double _gripperTau; + double _gripperPosStep; + double _gripperTauStep; + + double _gripperLinearFriction; + double _gripperCoulombFriction; + static double _gripperCoulombDirection; + double _gripperFriction; +#endif + CtrlComponents *_ctrlComp; + ArmFSMStateName _nextStateName; + + IOInterface *_ioInter; + LowlevelCmd *_lowCmd; + LowlevelState *_lowState; + UserValue _userValue; + ArmDynKineModel *_armModel; + + Vec6 _qPast, _qdPast, _q, _qd, _qdd; + Vec6 _tau, _endForce; + + Vec6 _coulombFriction; + static Vec6 _coulombDirection; + Vec6 _linearFriction; + + Vec6 _kpDiag, _kdDiag; + Mat6 _Kp, _Kd; + + std::vector _jointQMax; + std::vector _jointQMin; + std::vector _jointSpeedMax; + + SendCmd _sendCmd; + RecvState _recvState; +}; + +#endif // FSMSTATE_H diff --git a/include/FSM/FiniteStateMachine.h b/include/FSM/FiniteStateMachine.h new file mode 100755 index 0000000..5af327a --- /dev/null +++ b/include/FSM/FiniteStateMachine.h @@ -0,0 +1,42 @@ +#ifndef FSM_H +#define FSM_H + +#include +#include "FSM/BaseState.h" +#include "unitree_arm_sdk/cmdPanel.h" +#include "unitree_arm_sdk/loop.h" + +enum class FSMRunMode{ + NORMAL, + CHANGE +}; + +/* + 状态机初始状态为states的第一个元素 + 为了支持多个状态机从同一个CmdPanel获取触发信号， + 加入了cmdChannel，不同状态机之间的cmdChannel必须不同， + 否则可能会漏过状态触发 +*/ +class FiniteStateMachine{ +public: + FiniteStateMachine(std::vector states, + CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002); + virtual ~FiniteStateMachine(); + +private: + void _run(); + static void* _staticRun(void* obj); + std::vector _states; + + FSMRunMode _mode; + bool _running; + BaseState* _currentState; + BaseState* _nextState; + int _nextStateEnum; + + size_t _cmdChannel; + CmdPanel *_cmdPanel; + LoopFunc *_runThread; +}; + +#endif // FSM_H \ No newline at end of file diff --git a/include/FSM/State_BackToStart.h b/include/FSM/State_BackToStart.h new file mode 100644 index 0000000..1107373 --- /dev/null +++ b/include/FSM/State_BackToStart.h @@ -0,0 +1,13 @@ +#ifndef STATE_BACKTOSTART_H +#define STATE_BACKTOSTART_H + +#include "FSM/State_Trajectory.h" + +class State_BackToStart : public State_Trajectory{ +public: + State_BackToStart(CtrlComponents *ctrlComp); +private: + void _setTraj(); +}; + +#endif // STATE_BACKTOSTART_H \ No newline at end of file diff --git a/include/FSM/State_Calibration.h b/include/FSM/State_Calibration.h new file mode 100644 index 0000000..4668eb9 --- /dev/null +++ b/include/FSM/State_Calibration.h @@ -0,0 +1,18 @@ +#ifndef STATE_CALIBRATION_H +#define STATE_CALIBRATION_H + +#include "FSM/FSMState.h" + +class State_Calibration : public FSMState{ +public: + State_Calibration(CtrlComponents *ctrlComp); + ~State_Calibration(){} + void enter(); + void run(){}; + void exit(){}; + int checkChange(int cmd); +private: + +}; + +#endif // STATE_CALIBRATION_H \ No newline at end of file diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h new file mode 100755 index 0000000..b7bfd66 --- /dev/null +++ b/include/FSM/State_Cartesian.h @@ -0,0 +1,28 @@ +#ifndef CARTESIAN_H +#define CARTESIAN_H + +#include "FSM/FSMState.h" + +class State_Cartesian : public FSMState{ +public: + State_Cartesian(CtrlComponents *ctrlComp); + ~State_Cartesian(){} + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + void quadprogArea(); + double _posSpeed; + double _oriSpeed; + Vec3 _omega; + Vec3 _velocity; + Vec6 _twist; + HomoMat _endHomoGoal, _endHomoGoalPast; + HomoMat _endHomoFeedback; + Vec6 _Pdes; + Vec6 _Pfd; + Vec6 _Pkp; +}; + +#endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_Dance.h b/include/FSM/State_Dance.h new file mode 100644 index 0000000..3654443 --- /dev/null +++ b/include/FSM/State_Dance.h @@ -0,0 +1,21 @@ +#ifndef STATE_DANCETRAJ_H +#define STATE_DANCETRAJ_H + +#include "FSM/State_Trajectory.h" + +class State_Dance : public State_Trajectory{ +public: + State_Dance(CtrlComponents *ctrlComp, + TrajectoryManager *traj, + ArmFSMStateName stateEnum, + ArmFSMStateName nextState, + std::string stateString); + void exit(); + int checkChange(int cmd); +private: + bool _freeBottom = false; + // ArmFSMStateName _nextState; + void _setTraj(){} +}; + +#endif \ No newline at end of file diff --git a/include/FSM/State_JointSpace.h b/include/FSM/State_JointSpace.h new file mode 100755 index 0000000..75a4b29 --- /dev/null +++ b/include/FSM/State_JointSpace.h @@ -0,0 +1,18 @@ +#ifndef JOINTSPACE_H +#define JOINTSPACE_H + +#include "FSM/FSMState.h" + +class State_JointSpace : public FSMState{ +public: + State_JointSpace(CtrlComponents *ctrlComp); + ~State_JointSpace(){} + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + double _angleStep; +}; + +#endif // JOINTSPACE_H \ No newline at end of file diff --git a/include/FSM/State_LowCmd.h b/include/FSM/State_LowCmd.h new file mode 100755 index 0000000..ce00143 --- /dev/null +++ b/include/FSM/State_LowCmd.h @@ -0,0 +1,18 @@ +#ifndef LOWCMD_H +#define LOWCMD_H + +#include "FSMState.h" + +class State_LowCmd : public FSMState{ +public: + State_LowCmd(CtrlComponents *ctrlComp); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + std::vector _kp; + std::vector _kw; +}; + +#endif // LOWCMD_H \ No newline at end of file diff --git a/include/FSM/State_MoveC.h b/include/FSM/State_MoveC.h new file mode 100755 index 0000000..9b77aab --- /dev/null +++ b/include/FSM/State_MoveC.h @@ -0,0 +1,34 @@ +#ifndef MOVEC_H +#define MOVEC_H + +#include "FSM/FSMState.h" +#include "trajectory/CartesionSpaceTraj.h" + +class State_MoveC : public FSMState{ +public: + State_MoveC(CtrlComponents *ctrlComp); + ~State_MoveC(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + void quadprogArea(); + double _posSpeed; + double _oriSpeed; + Vec3 _omega; + Vec3 _velocity; + Vec6 _twist; + Vec6 _pastPosture, _endPosture, _middlePostureGoal, _endPostureGoal, _endTwist; + HomoMat _endHomoFeedback; + Vec6 _Pdes; + Vec6 _Pfd; + Vec6 _Pkp; + Vec6 _endPostureError; + + // 轨迹相关变量 + CartesionSpaceTraj *_cartesionTraj; + bool _reached, _timeReached, _taskReached, _pastTaskReached; +}; + +#endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_MoveJ.h b/include/FSM/State_MoveJ.h new file mode 100755 index 0000000..def1e20 --- /dev/null +++ b/include/FSM/State_MoveJ.h @@ -0,0 +1,23 @@ +#ifndef MOVEJ_H +#define MOVEJ_H + +#include "FSM/FSMState.h" +#include "trajectory/JointSpaceTraj.h" + +class State_MoveJ : public FSMState{ +public: + State_MoveJ(CtrlComponents *ctrlComp); + ~State_MoveJ(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + double _costTime; + HomoMat _goalHomo; + JointSpaceTraj *_jointTraj; + bool _reached, _pastReached, _finalReached; + std::vector _result; +}; + +#endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_MoveL.h b/include/FSM/State_MoveL.h new file mode 100755 index 0000000..b7f0f21 --- /dev/null +++ b/include/FSM/State_MoveL.h @@ -0,0 +1,35 @@ +#ifndef MOVEL_H +#define MOVEL_H + +#include "FSM/FSMState.h" +#include "trajectory/CartesionSpaceTraj.h" + +class State_MoveL : public FSMState{ +public: + State_MoveL(CtrlComponents *ctrlComp); + ~State_MoveL(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + void quadprogArea(); + double _posSpeed; + double _oriSpeed; + Vec3 _omega; + Vec3 _velocity; + Vec6 _twist; + Vec6 _pastPosture, _endPosture, _endTwist; + HomoMat _endHomoFeedback; + Vec6 _Pdes; + Vec6 _Pfd; + Vec6 _Pkp; + Vec6 _endPostureError; + + // 轨迹相关变量 + std::vector > _posture; + CartesionSpaceTraj *_cartesionTraj; + bool _timeReached, _taskReached, _pastTaskReached, _finalReached; +}; + +#endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_Passive.h b/include/FSM/State_Passive.h new file mode 100755 index 0000000..506e215 --- /dev/null +++ b/include/FSM/State_Passive.h @@ -0,0 +1,15 @@ +#ifndef PASSIVE_H +#define PASSIVE_H + +#include "FSMState.h" + +class State_Passive : public FSMState{ +public: + State_Passive(CtrlComponents *ctrlComp); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +}; + +#endif // PASSIVE_H \ No newline at end of file diff --git a/include/FSM/State_SaveState.h b/include/FSM/State_SaveState.h new file mode 100755 index 0000000..cdae1a0 --- /dev/null +++ b/include/FSM/State_SaveState.h @@ -0,0 +1,21 @@ +#ifndef SAVESTATE_H +#define SAVESTATE_H + +#include "FSMState.h" +#include "common/utilities/CSVTool.h" + +class State_SaveState : public FSMState{ +public: + State_SaveState(CtrlComponents *ctrlComp); + ~State_SaveState(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + CSVTool *_csv; + Vec6 _q; + HomoMat _endHomo; +}; + +#endif // SAVESTATE_H \ No newline at end of file diff --git a/include/FSM/State_Teach.h b/include/FSM/State_Teach.h new file mode 100755 index 0000000..533a3cd --- /dev/null +++ b/include/FSM/State_Teach.h @@ -0,0 +1,25 @@ +#ifndef STATETEACH_H +#define STATETEACH_H + +#include "FSM/FSMState.h" +#include "common/utilities/CSVTool.h" + +class State_Teach : public FSMState{ +public: + State_Teach(CtrlComponents *ctrlComp); + ~State_Teach(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + CSVTool *_trajCSV; + size_t _stateID = 0; + + Vec6 _KdDiag; + Vec6 _kpForStable; + Mat6 _Kd; + double _errorBias; +}; + +#endif // STATETEACH_H \ No newline at end of file diff --git a/include/FSM/State_TeachRepeat.h b/include/FSM/State_TeachRepeat.h new file mode 100755 index 0000000..85d20fe --- /dev/null +++ b/include/FSM/State_TeachRepeat.h @@ -0,0 +1,27 @@ +#ifndef STATE_TEACHREPEAT_H +#define STATE_TEACHREPEAT_H + +#include "FSM/FSMState.h" +#include "trajectory/TrajectoryManager.h" +#include "common/utilities/CSVTool.h" + +class State_TeachRepeat : public FSMState{ +public: + State_TeachRepeat(CtrlComponents *ctrlComp); + ~State_TeachRepeat(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + JointSpaceTraj *_toStartTraj; + bool _reachedStart = false; + bool _finishedRepeat = false; + size_t _index = 0; + size_t _indexPast; + Vec6 _trajStartQ, _trajStartQd; + + CSVTool *_csvFile; +}; + +#endif // STATE_TEACHREPEAT_H \ No newline at end of file diff --git a/include/FSM/State_ToState.h b/include/FSM/State_ToState.h new file mode 100755 index 0000000..228d137 --- /dev/null +++ b/include/FSM/State_ToState.h @@ -0,0 +1,24 @@ +#ifndef TOSTATE_H +#define TOSTATE_H + +#include "FSM/FSMState.h" +#include "trajectory/JointSpaceTraj.h" + +class State_ToState : public FSMState{ +public: + State_ToState(CtrlComponents *ctrlComp); + ~State_ToState(); + void enter(); + void run(); + void exit(); + int checkChange(int cmd); +private: + double _costTime; + HomoMat _goalHomo; + JointSpaceTraj *_jointTraj; + bool _reach, _pastReach; + std::string _goalName; + // CSVTool *_csv; +}; + +#endif // TOSTATE_H \ No newline at end of file diff --git a/include/FSM/State_Trajectory.h b/include/FSM/State_Trajectory.h new file mode 100755 index 0000000..e0ad84b --- /dev/null +++ b/include/FSM/State_Trajectory.h @@ -0,0 +1,36 @@ +#ifndef CARTESIANPATH_H +#define CARTESIANPATH_H + +#include "FSM/FSMState.h" +#include "trajectory/EndHomoTraj.h" +#include "trajectory/TrajectoryManager.h" + +class State_Trajectory : public FSMState{ +public: + State_Trajectory(CtrlComponents *ctrlComp, + ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY, + std::string stateString = "trajectory"); + ~State_Trajectory(); + void enter(); + void run(); + virtual void exit(); + virtual int checkChange(int cmd); +protected: + // EndHomoTraj *_traj; + virtual void _setTraj(); + TrajectoryManager *_traj; + HomoMat _goalHomo; + Vec6 _goalTwist; + + JointSpaceTraj *_toStartTraj; + bool _reachedStart = false; + bool _finishedTraj = false; + std::vector _jointTraj; + std::vector _circleTraj; + std::vector _stopTraj; + std::vector _lineTraj; + + long long startTime; +}; + +#endif // CARTESIANPATH_H \ No newline at end of file diff --git a/include/common/enumClass.h b/include/common/enumClass.h new file mode 100755 index 0000000..42dc334 --- /dev/null +++ b/include/common/enumClass.h @@ -0,0 +1,97 @@ +#ifndef ENUMCLASS_H +#define ENUMCLASS_H + +// enum class UserCommand{ +// // EXIT, +// NONE, +// START, +// L2_A, //used by robot +// L2_B, //used by robot +// L2_X, //used by robot +// R2_A, //Arm: joint space control +// R2_X, //Arm: Cartesian +// R2_Y, // +// R2_temp, +// ARM_PASSIVE, //Arm: passive +// ARM_CHECKJOINT_0, +// ARM_TRAJECTORY_0, +// ARM_SAVESTATE_0, +// ARM_TEACH_0, +// ARM_TEACHREPEAT_0, +// ARM_NEXT, +// ARM_BACKTOSTART, + +// HEAD_JOINTCTRL, +// HEAD_TOSTART, +// HEAD_SAVE, +// HEAD_TURN, +// HEAD_CALI, +// HEAD_NEXT, +// HEAD_DANCE01, +// HEAD_DANCE02, +// HEAD_DANCE03, +// HEAD_DANCE04, +// HEAD_DANCE05, +// HEAD_DANCE06, +// HEAD_DANCE07 +// }; + +enum class FrameType{ + BODY, + HIP, + ENDEFFECTOR, + GLOBAL +}; + +enum class FSMMode{ + NORMAL, + CHANGE +}; + +enum class ArmFSMStateName{ + INVALID, + PASSIVE, + JOINTCTRL, + CARTESIAN, + MOVEJ, + MOVEL, + MOVEC, + TRAJECTORY, + TOSTATE, + SAVESTATE, + TEACH, + TEACHREPEAT, + CALIBRATION, + DANCE00, + DANCE01, + DANCE02, + DANCE03, + DANCE04, + DANCE05, + DANCE06, + DANCE07, + DANCE08, + DANCE09, + BACKTOSTART, + GRIPPER_OPEN, + GRIPPER_CLOSE, + NEXT, + LOWCMD +}; + +enum class JointMotorType{ + SINGLE_MOTOR, + DOUBLE_MOTOR +}; + +enum class MotorMountType{ + STATOR_FIRST, + OUTPUT_FIRST +}; + +enum class HeadType{ + TIGER, + DOG +}; + +#endif // ENUMCLASS_H \ No newline at end of file diff --git a/include/common/math/mathTools.h b/include/common/math/mathTools.h new file mode 100755 index 0000000..00daeb0 --- /dev/null +++ b/include/common/math/mathTools.h @@ -0,0 +1,482 @@ +#ifndef MATHTOOLS_H +#define MATHTOOLS_H + +#include +// #include "common/mathTypes.h" + +// template +// inline T1 max(const T1 a, const T2 b){ +// return (a > b ? a : b); +// } + +// template +// inline T1 min(const T1 a, const T2 b){ +// return (a < b ? a : b); +// } + +template +T max(T value){ + return value; +} + +template +inline T max(const T val0, const Args... restVal){ + return val0 > (max(restVal...)) ? val0 : max(restVal...); +} + +template +T min(T value){ + return value; +} + +template +inline T min(const T val0, const Args... restVal){ + return val0 > (min(restVal...)) ? val0 : min(restVal...); +} + +inline Mat2 skew(const double& w){ + Mat2 mat; mat.setZero(); + mat(0, 1) = -w; + mat(1, 0) = w; + return mat; +} + +inline Mat3 skew(const Vec3& v) { + Mat3 m; + m << 0, -v(2), v(1), + v(2), 0, -v(0), + -v(1), v(0), 0; + return m; +} + +enum class TurnDirection{ + NOMATTER, + POSITIVE, + NEGATIVE +}; + +/* first - second */ +inline double angleError(double first, double second, TurnDirection direction = TurnDirection::NOMATTER){ + double firstMod = fmod(first, 2.0*M_PI); + double secondMod = fmod(second, 2.0*M_PI); + + if(direction == TurnDirection::NOMATTER){ + if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){ + return secondMod - firstMod; + }else{ + return firstMod - secondMod; + } + } + else if(direction == TurnDirection::POSITIVE){ + if(firstMod - secondMod < 0.0){ + return 2*M_PI + firstMod - secondMod; + }else{ + return firstMod - secondMod; + } + } + else if(direction == TurnDirection::NEGATIVE){ + if(firstMod - secondMod > 0.0){ + return -2*M_PI + firstMod - secondMod; + }else{ + return firstMod - secondMod; + } + } +} + +/* firstVec - secondVec */ +inline VecX angleError(VecX firstVec, VecX secondVec, TurnDirection directionMatter = TurnDirection::NOMATTER){ + if(firstVec.rows() != secondVec.rows()){ + std::cout << "[ERROR] angleError, the sizes of firstVec and secondVec are different!" << std::endl; + } + + VecX result = firstVec; + for(int i(0); itolerance){ + return false; + } + } + return true; +} + +inline bool inInterval(double value, double limValue1, double limValue2, bool canEqual1 = false, bool canEqual2 = false){ + double lowLim, highLim; + bool lowEqual, highEqual; + if(limValue1 >= limValue2){ + highLim = limValue1; + highEqual = canEqual1; + lowLim = limValue2; + lowEqual = canEqual2; + }else{ + lowLim = limValue1; + lowEqual = canEqual1; + highLim = limValue2; + highEqual = canEqual2; + } + + if((value > highLim) || (value < lowLim)){ + return false; + } + if((value == highLim) && !highEqual){ + return false; + } + if((value == lowLim) && !lowEqual){ + return false; + } + + return true; +} + +inline double saturation(const double a, double limValue1, double limValue2){ + double lowLim, highLim; + if(limValue1 >= limValue2){ + lowLim = limValue2; + highLim= limValue1; + }else{ + lowLim = limValue1; + highLim= limValue2; + } + + if(a < lowLim){ + return lowLim; + } + else if(a > highLim){ + return highLim; + } + else{ + return a; + } +} + +inline double saturation(const double a, Vec2 limits){ + return saturation(a, limits(0), limits(1)); +} + +template +inline T0 killZeroOffset(T0 a, const T1 limit){ + if((a > -limit) && (a < limit)){ + a = 0; + } + return a; +} + +template +inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){ + return (value-minLim)*(max-min)/(maxLim-minLim) + min; +} + +// x: [0, 1], windowRatio: (0, 0.5) +template +inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){ + if((x < 0)||(x > xRange)){ + // printf("[ERROR] The x of function windowFunc should between [0, xRange]\n"); + std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl; + } + if((windowRatio <= 0)||(windowRatio >= 0.5)){ + // printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n"); + std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl; + } + + if(x/xRange < windowRatio){ + return x * yRange / (xRange * windowRatio); + } + else if(x/xRange > 1 - windowRatio){ + return yRange * (xRange - x)/(xRange * windowRatio); + } + else{ + return yRange; + } +} + +template +inline void updateAverage(T1 &exp, T2 newValue, double n){ + if(exp.rows()!=newValue.rows()){ + std::cout << "The size of updateAverage is error" << std::endl; + exit(-1); + } + if(fabs(n - 1) < 0.001){ + exp = newValue; + }else{ + exp = exp + (newValue - exp)/n; + } +} + +template +inline void updateCovariance(T1 &cov, T2 expPast, T3 newValue, double n){ + if( (cov.rows()!=cov.cols()) || (cov.rows() != expPast.rows()) || (expPast.rows()!=newValue.rows())){ + std::cout << "The size of updateCovariance is error" << std::endl; + exit(-1); + } + if(fabs(n - 1) < 0.1){ + cov.setZero(); + }else{ + cov = cov*(n-1)/n + (newValue-expPast)*(newValue-expPast).transpose()*(n-1)/(n*n); + } +} + +template +inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){ + // The order matters!!! covariance first!!! + updateCovariance(cov, exp, newValue, n); + updateAverage(exp, newValue, n); +} + +/* rotate matrix about x axis */ +inline RotMat rotX(const double &theta) { + double s = std::sin(theta); + double c = std::cos(theta); + + RotMat R; + R << 1, 0, 0, 0, c, -s, 0, s, c; + return R; +} + +/* rotate matrix about y axis */ +inline RotMat rotY(const double &theta) { + double s = std::sin(theta); + double c = std::cos(theta); + + RotMat R; + R << c, 0, s, 0, 1, 0, -s, 0, c; + return R; +} + +/* rotate matrix about z axis */ +inline RotMat rotZ(const double &theta) { + double s = std::sin(theta); + double c = std::cos(theta); + + RotMat R; + R << c, -s, 0, s, c, 0, 0, 0, 1; + return R; +} + +inline RotMat so3ToRotMat(const Vec3& _rot){ + RotMat R; + double theta = _rot.norm(); + if (fabs(theta) <1e-6) + R = RotMat::Identity(); + else{ + Vec3 u_axis(_rot/theta); + double cos_theta = cos(theta); + R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose()); + } + return R; +} + +/* row pitch yaw to rotate matrix */ +inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) { + RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row); + return m; +} + +inline RotMat quatToRotMat(const Quat& q) { + double e0 = q(0); + double e1 = q(1); + double e2 = q(2); + double e3 = q(3); + + RotMat R; + R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3), + 2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3), + 1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1), + 2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1), + 1 - 2 * (e1 * e1 + e2 * e2); + return R; +} + +inline Vec3 rotMatToRPY(const Mat3& R) { + Vec3 rpy; + rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2)); + rpy(1) = asin(-R(2,0)); + rpy(2) = atan2(R(1,0),R(0,0)); + return rpy; +} + +/* rotate matrix to exponential coordinate(omega*theta) */ +inline Vec3 rotMatToExp(const RotMat& rm){ + double cosValue = rm.trace()/2.0-1/2.0; + if(cosValue > 1.0f){ + cosValue = 1.0f; + }else if(cosValue < -1.0f){ + cosValue = -1.0f; + } + + double angle = acos(cosValue); + Vec3 exp; + if (fabs(angle) < 1e-5){ + exp=Vec3(0,0,0); + } + else if (fabs(angle - M_PI) < 1e-5){ + exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0))); + } + else{ + exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1)); + } + return exp; +} + +/* add 1 at the end of Vec3 */ +inline Vec4 homoVec(Vec3 v3, double end = 1.0){ + Vec4 v4; + v4.block(0, 0, 3, 1) = v3; + v4(3) = end; + return v4; +} + +/* remove 1 at the end of Vec4 */ +inline Vec3 noHomoVec(Vec4 v4){ + Vec3 v3; + v3 = v4.block(0, 0, 3, 1); + return v3; +} + +/* build Homogeneous Matrix by p and m */ +inline HomoMat homoMatrix(Vec3 p, Mat3 m){ + HomoMat homoM; + homoM.setZero(); + homoM.topLeftCorner(3, 3) = m; + homoM.topRightCorner(3, 1) = p; + homoM(3, 3) = 1; + return homoM; +} + +/* build Homogeneous Matrix by p and w */ +inline HomoMat homoMatrix(Vec3 p, Vec3 w){ + HomoMat homoM; + homoM.setZero(); + homoM.topLeftCorner(3, 3) = so3ToRotMat(w); + homoM.topRightCorner(3, 1) = p; + homoM(3, 3) = 1; + return homoM; +} + +/* build Homogeneous Matrix by x axis, y axis and p */ +inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){ + HomoMat homoM; + homoM.setZero(); + Vec3 xN = x.normalized(); + Vec3 yN = y.normalized(); + homoM.block(0, 0, 3, 1) = xN; + homoM.block(0, 1, 3, 1) = yN; + homoM.block(0, 2, 3, 1) = skew(xN) * yN; + homoM.topRightCorner(3, 1) = p; + homoM(3, 3) = 1; + return homoM; +} + +/* build Homogeneous Matrix of rotate joint */ +inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){ + HomoMat homoM; + Mat3 eye3 = Mat3::Identity(); + Mat3 rotateM = so3ToRotMat(w); + homoM.setZero(); + homoM.topLeftCorner(3, 3) = rotateM; + homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q; + // homoM.topRightCorner(3, 1) = q; + homoM(3, 3) = 1; + + return homoM; +} + +/* build Homogeneous Matrix by posture */ +inline HomoMat homoMatrixPosture(Vec6 posture){ + HomoMat homoM; + homoM.setZero(); + homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2)); + homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5); + homoM(3, 3) = 1; + return homoM; +} + +/* inverse matrix of homogeneous matrix */ +inline HomoMat homoMatrixInverse(HomoMat homoM){ + HomoMat homoInv; + homoInv.setZero(); + homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose(); + homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1); + homoInv(3, 3) = 1; + return homoInv; +} + +/* get position of Homogeneous Matrix */ +inline Vec3 getHomoPosition(HomoMat mat){ + return mat.block(0, 3, 3, 1); +} + +/* get rotate matrix of Homogeneous Matrix */ +inline RotMat getHomoRotMat(HomoMat mat){ + return mat.block(0, 0, 3, 3); +} + +/* convert homogeneous matrix to posture vector */ +inline Vec6 homoToPosture(HomoMat mat){ + Vec6 posture; + posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat)); + posture.block(3,0,3,1) = getHomoPosition(mat); + return posture; +} + +/* convert posture vector matrix to homogeneous */ +inline HomoMat postureToHomo(Vec6 posture){ + HomoMat homoM; + homoM.setZero(); + homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2)); + homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5); + homoM(3, 3) = 1; + return homoM; +} + +// Calculate average value and covariance +class AvgCov{ +public: + AvgCov(unsigned int size, std::string name, bool avgOnly=false, unsigned int showPeriod=1000, unsigned int waitCount=5000, double zoomFactor=10000) + :_size(size), _showPeriod(showPeriod), _waitCount(waitCount), _zoomFactor(zoomFactor), _valueName(name), _avgOnly(avgOnly) { + _exp.resize(size); + _cov.resize(size, size); + _defaultWeight.resize(size, size); + _defaultWeight.setIdentity(); + _measureCount = 0; + } + void measure(VecX newValue){ + ++_measureCount; + + if(_measureCount > _waitCount){ + updateAvgCov(_cov, _exp, newValue, _measureCount-_waitCount); + if(_measureCount % _showPeriod == 0){ + // if(_measureCount < _waitCount + 5){ + std::cout << "******" << _valueName << " measured count: " << _measureCount-_waitCount << "******" << std::endl; + // std::cout << _zoomFactor << " Times newValue of " << _valueName << std::endl << (_zoomFactor*newValue).transpose() << std::endl; + std::cout << _zoomFactor << " Times Average of " << _valueName << std::endl << (_zoomFactor*_exp).transpose() << std::endl; + if(!_avgOnly){ + std::cout << _zoomFactor << " Times Covariance of " << _valueName << std::endl << _zoomFactor*_cov << std::endl; + } + } + } + } +private: + VecX _exp; + MatX _cov; + MatX _defaultWeight; + bool _avgOnly; + unsigned int _size; + unsigned int _measureCount; + unsigned int _showPeriod; + unsigned int _waitCount; + double _zoomFactor; + std::string _valueName; +}; + +#endif // MATHTOOLS_H \ No newline at end of file diff --git a/include/common/math/mathTypes.h b/include/common/math/mathTypes.h new file mode 100755 index 0000000..2ca09b1 --- /dev/null +++ b/include/common/math/mathTypes.h @@ -0,0 +1,99 @@ +#ifndef MATHTYPES_H +#define MATHTYPES_H + +#include + +/************************/ +/******** Vector ********/ +/************************/ +// 2x1 Vector +using Vec2 = typename Eigen::Matrix; + +// 3x1 Vector +using Vec3 = typename Eigen::Matrix; + +// 4x1 Vector +using Vec4 = typename Eigen::Matrix; + +// 6x1 Vector +using Vec6 = typename Eigen::Matrix; + +// Quaternion +using Quat = typename Eigen::Matrix; + +// 4x1 Integer Vector +using VecInt4 = typename Eigen::Matrix; + +// 12x1 Vector +using Vec12 = typename Eigen::Matrix; + +// 18x1 Vector +using Vec18 = typename Eigen::Matrix; + +// Dynamic Length Vector +using VecX = typename Eigen::Matrix; + +/************************/ +/******** Matrix ********/ +/************************/ +// Rotation Matrix +using RotMat = typename Eigen::Matrix; + +// Homogenous Matrix +using HomoMat = typename Eigen::Matrix; + +// 2x2 Matrix +using Mat2 = typename Eigen::Matrix; + +// 3x3 Matrix +using Mat3 = typename Eigen::Matrix; + +// 3x4 Matrix, each column is a 3x1 vector +using Vec34 = typename Eigen::Matrix; + +// 6x6 Matrix +using Mat6 = typename Eigen::Matrix; + +// 12x12 Matrix +using Mat12 = typename Eigen::Matrix; + +// 3x3 Identity Matrix +#define I3 Eigen::MatrixXd::Identity(3, 3) + +// 6x6 Identity Matrix +#define I6 Eigen::MatrixXd::Identity(6, 6) + +// 12x12 Identity Matrix +#define I12 Eigen::MatrixXd::Identity(12, 12) + +// 18x18 Identity Matrix +#define I18 Eigen::MatrixXd::Identity(18, 18) + +// Dynamic Size Matrix +using MatX = typename Eigen::Matrix; + +/************************/ +/****** Functions *******/ +/************************/ +inline Vec34 vec12ToVec34(Vec12 vec12){ + Vec34 vec34; + for(int i(0); i < 4; ++i){ + vec34.col(i) = vec12.segment(3*i, 3); + } + return vec34; +} + +inline Vec12 vec34ToVec12(Vec34 vec34){ + Vec12 vec12; + for(int i(0); i < 4; ++i){ + vec12.segment(3*i, 3) = vec34.col(i); + } + return vec12; +} + +template +inline VecX stdVecToEigenVec(T stdVec){ + VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size()); + return eigenVec; +} +#endif // MATHTYPES_H \ No newline at end of file diff --git a/include/common/utilities/CSVTool.h b/include/common/utilities/CSVTool.h new file mode 100755 index 0000000..c8497ec --- /dev/null +++ b/include/common/utilities/CSVTool.h @@ -0,0 +1,242 @@ +#ifndef CSVTOOL_H +#define CSVTOOL_H + +/* + personal tool for .csv reading and modifying. + only suitable for small .csv file. + for large .csv, try (read only) + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "common/utilities/typeTrans.h" + +enum class FileType{ + READ_WRITE, + CLEAR_DUMP +}; + +class CSVLine{ +public: + // CSVLine(std::string lineTemp, std::streampos filePos); + CSVLine(std::string lineTemp); + CSVLine(std::string label, std::vector values); + ~CSVLine(){} + + // void updateFilePos(std::streampos filePos){_filePos = filePos;} + void getValues(std::vector &values); + void changeValue(std::vector values); + void writeAtEnd(std::fstream &ioStream); + std::string getLabel(){return _label;} + +private: + // std::streampos _filePos; + std::string _label; + std::vector _values; +}; + +/* +FileType::READ_WRITE : must already exist such fileName +FileType::CLEAR_DUMP : if do not exist such file, will create one +*/ +class CSVTool{ +public: + CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6); + ~CSVTool(){_ioStream.close();} + + bool getLine(std::string label, std::vector &values); + template + bool getLineDirect(std::string label, Args&... values); + + void modifyLine(std::string label, std::vector &values, bool addNew); + template + void modifyLineDirect(std::string label, bool addNew, Args&... values); + + void readFile(); + void saveFile(); + +private: + std::string _fileName; + std::fstream _ioStream; + int _precision; + std::string _lineTemp; + std::map _labels; + std::vector _lines; + +}; + +/*************************/ +/* CSVLine */ +/*************************/ +// CSVLine::CSVLine(std::string lineTemp, std::streampos filePos) +// :_filePos(filePos){ + +// // std::cout << lineTemp << std::endl; +// } + +inline CSVLine::CSVLine(std::string lineTemp){ + // delete all spaces + lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end()); + + std::stringstream ss(lineTemp); + std::string stringTemp; + + std::getline(ss, _label, ','); + + while(std::getline(ss, stringTemp, ',')){ + _values.push_back(stod(stringTemp)); + } + + // std::cout << "**********" << std::endl; + // std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl; + // for(int i(0); i<_values.size(); ++i){ + // std::cout << _values.at(i) << ",,, "; + // } + // std::cout << std::endl; +} + +inline CSVLine::CSVLine(std::string label, std::vector values) + :_label(label), _values(values){ + +} + +inline void CSVLine::changeValue(std::vector values){ + if(values.size() != _values.size()){ + std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl; + } + _values = values; +} + +inline void CSVLine::getValues(std::vector &values){ + values = _values; +} + +inline void CSVLine::writeAtEnd(std::fstream &ioStream){ + ioStream << _label << ", "; + + for(int i(0); i<_values.size(); ++i){ + ioStream << _values.at(i) << ", "; + } + + ioStream << std::endl; +} + + +/*************************/ +/* CSVTool */ +/*************************/ +inline CSVTool::CSVTool(std::string fileName, FileType type, int precision) + : _fileName(fileName), _precision(precision){ + + if(type == FileType::READ_WRITE){ + _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); + + if(!_ioStream.is_open()){ + std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl; + exit(-1); + } + + readFile(); + } + else if(type == FileType::CLEAR_DUMP){ + _ioStream.open(_fileName, std::fstream::out); + } + +} + +inline void CSVTool::readFile(){ + if(!_ioStream.is_open()){ + // _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); + std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl; + } + + _lines.clear(); + _labels.clear(); + + _ioStream.seekg(0, std::fstream::beg); + size_t lineNum = 0; + while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){ + _lines.push_back( new CSVLine(_lineTemp) ); + + if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){ + _labels.insert(std::pair(_lines.at(lineNum)->getLabel(), lineNum)); + ++lineNum; + }else{ + std::cout << "[ERROR] CSVTool::readFile, the label " + << _lines.at(lineNum)->getLabel() << " is repeated" << std::endl; + exit(-1); + } + } +} + +inline bool CSVTool::getLine(std::string label, std::vector &values){ + if(_labels.count(label) == 0){ + std::cout << "[ERROR] No such label: " << label << std::endl; + return false; + }else{ + _lines.at(_labels[label])->getValues(values); + return true; + } +} + +template +inline bool CSVTool::getLineDirect(std::string label, Args&... values){ + std::vector vec; + if(getLine(label, vec)){ + typeTrans::extractVector(vec, values...); + return true; + }else{ + return false; + } +} + +template +inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){ + std::vector vec; + typeTrans::combineToVector(vec, values...); + +// std::cout << "CSVTool::modifyLineDirect------" << std::endl; +// std::cout << "label: " << label << std::endl; +// std::cout << "vec: "; +// for(int i(0); iwriteAtEnd(_ioStream); + } + + _ioStream.close(); + _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); +} + +inline void CSVTool::modifyLine(std::string label, std::vector &values, bool addNew =false){ + if(_labels.count(label) == 0){ + if(addNew){ + _labels.insert(std::pair(label, _labels.size())); + _lines.push_back(new CSVLine(label, values)); + }else{ + std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl; + exit(-1); + } + }else{ + _lines.at(_labels[label])->changeValue(values); + } +} + + +#endif // CSVTOOL_H \ No newline at end of file diff --git a/include/common/utilities/LPFilter.h b/include/common/utilities/LPFilter.h new file mode 100755 index 0000000..f9dc43c --- /dev/null +++ b/include/common/utilities/LPFilter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASSFILTER +#define LOWPASSFILTER + +#include +#include + +/* + Low pass filter +*/ +class LPFilter{ +public: + LPFilter(double samplePeriod, double cutFrequency, size_t valueCount); + ~LPFilter(){}; + void clear(); + void addValue(double &newValue); + void addValue(std::vector &vec); + + template + void addValue(Eigen::MatrixBase &eigenVec); + + // double getValue(); +private: + size_t _valueCount; + double _weight; + std::vector _pastValue; + bool _start; +}; + +#endif // LOWPASSFILTER \ No newline at end of file diff --git a/include/common/utilities/PyPlot.h b/include/common/utilities/PyPlot.h new file mode 100644 index 0000000..fae55a5 --- /dev/null +++ b/include/common/utilities/PyPlot.h @@ -0,0 +1,261 @@ +#ifndef PYPLOT_H +#define PYPLOT_H + +#include +#include +#include +#include +#include +#include +#include "common/utilities/matplotlibcpp.h" +#include "unitree_arm_sdk/timer.h" + +namespace plt = matplotlibcpp; + +// enum class PlotType{ +// STATIC, +// ANIME +// }; + +struct Curve{ + std::vector x; + std::vector y; + + void printXY(double xRough, int pointNum){ + for(int i(0); i curves; + std::vector labels; + std::string plotName; + int curveCount; + + std::map curveName2ID; + + Plot(std::string name, int count, std::vector labelVec) + :plotName(name), curveCount(count), labels(labelVec){ + for(int i(0); i < count; ++i){ + curveName2ID.insert(std::pair(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 labelVec); + void addPlot(std::string plotName, int curveCount); + void showPlot(std::string plotName); + void showPlot(std::vector 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 + void addFrame(std::string plotName, T* valueArray); + template + void addFrame(std::string plotName, double x, T* valueArray); + + template + void addFrame(std::string plotName, const Eigen::MatrixBase &vec); + template + void addFrame(std::string plotName, double x, const Eigen::MatrixBase &vec); + + template + void addFrame(std::string plotName, const std::vector &vec); + template + void addFrame(std::string plotName, double x, const std::vector &vec); + +private: + void _checkStart(); + int _plotCount = 0; + std::map _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 labelVec){ + if(_plotName2ID.count(plotName) == 0){ + _plotName2ID.insert(std::pair(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 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 plotNameVec){ + for(std::vector::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 +inline void PyPlot::addFrame(std::string plotName, T* valueArray){ + _checkStart(); + Plot* plot = _getPlotPtr(plotName); + addFrame(plotName, plot->getX(startT), valueArray); +} + +template +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 +inline void PyPlot::addFrame(std::string plotName, const Eigen::MatrixBase &vec){ + _checkStart(); + Plot* plot = _getPlotPtr(plotName); + addFrame(plotName, plot->getX(startT), vec); +} + +template +inline void PyPlot::addFrame(std::string plotName, double x, const Eigen::MatrixBase &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 +inline void PyPlot::addFrame(std::string plotName, const std::vector &vec){ + _checkStart(); + Plot* plot = _getPlotPtr(plotName); + addFrame(plotName, plot->getX(startT), vec); +} + +template +inline void PyPlot::addFrame(std::string plotName, double x, const std::vector &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 \ No newline at end of file diff --git a/include/common/utilities/matplotlibcpp.h b/include/common/utilities/matplotlibcpp.h new file mode 100755 index 0000000..e017e47 --- /dev/null +++ b/include/common/utilities/matplotlibcpp.h @@ -0,0 +1,2557 @@ +#pragma once + +// Python headers must be included before any system headers, since +// they define _POSIX_C_SOURCE +#include + +#include +#include +#include +#include +#include +#include +#include +#include // requires c++11 support +#include + +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include + +# ifdef WITH_OPENCV +# include +# endif // WITH_OPENCV + +/* + * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so + * define the ones we need here. + */ +# if CV_MAJOR_VERSION > 3 +# define CV_BGR2RGB cv::COLOR_BGR2RGB +# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA +# endif +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +# define PyInt_FromLong PyLong_FromLong +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject* s_python_function_arrow; + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_fignum_exists; + PyObject *s_python_function_plot; + PyObject *s_python_function_quiver; + PyObject* s_python_function_contour; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_imshow; + PyObject *s_python_function_scatter; + PyObject *s_python_function_boxplot; + PyObject *s_python_function_subplot; + PyObject *s_python_function_subplot2grid; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ginput; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_axvline; + PyObject *s_python_function_axvspan; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_gca; + PyObject *s_python_function_xticks; + PyObject *s_python_function_yticks; + PyObject* s_python_function_margins; + PyObject *s_python_function_tick_params; + PyObject *s_python_function_grid; + PyObject* s_python_function_cla; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_colormap; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + PyObject *s_python_function_text; + PyObject *s_python_function_suptitle; + PyObject *s_python_function_bar; + PyObject *s_python_function_barh; + PyObject *s_python_function_colorbar; + PyObject *s_python_function_subplots_adjust; + + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. [1] + Furthermore, many python objects expect that they are destructed in the same thread as they + were constructed. [2] So for advanced usage, a `kill()` function is provided so that library + users can manually ensure that the interpreter is constructed and destroyed within the + same thread. + + 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256 + */ + + static _interpreter& get() { + return interkeeper(false); + } + + static _interpreter& kill() { + return interkeeper(true); + } + + // Stores the actual singleton object referenced by `get()` and `kill()`. + static _interpreter& interkeeper(bool should_kill) { + static _interpreter ctx; + if (should_kill) + ctx.~_interpreter(); + return ctx; + } + + PyObject* safe_import(PyObject* module, std::string fname) { + PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); + + if (!fn) + throw std::runtime_error(std::string("Couldn't find required function: ") + fname); + + if (!PyFunction_Check(fn)) + throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); + + return fn; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + + wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified + wchar_t const **argv = dummy_args; + int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; + // Python3: + // PySys_SetArgv(argc, const_cast(argv)); + // Python2: + char** argm = (char **)(argv); PySys_SetArgv(argc, argm); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* cmname = PyString_FromString("matplotlib.cm"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname || !cmname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { + PyErr_Print(); + throw std::runtime_error("Error loading module matplotlib!"); + } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + s_python_colormap = PyImport_Import(cmname); + Py_DECREF(cmname); + if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_arrow = safe_import(pymod, "arrow"); + s_python_function_show = safe_import(pymod, "show"); + s_python_function_close = safe_import(pymod, "close"); + s_python_function_draw = safe_import(pymod, "draw"); + s_python_function_pause = safe_import(pymod, "pause"); + s_python_function_figure = safe_import(pymod, "figure"); + s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); + s_python_function_plot = safe_import(pymod, "plot"); + s_python_function_quiver = safe_import(pymod, "quiver"); + s_python_function_contour = safe_import(pymod, "contour"); + s_python_function_semilogx = safe_import(pymod, "semilogx"); + s_python_function_semilogy = safe_import(pymod, "semilogy"); + s_python_function_loglog = safe_import(pymod, "loglog"); + s_python_function_fill = safe_import(pymod, "fill"); + s_python_function_fill_between = safe_import(pymod, "fill_between"); + s_python_function_hist = safe_import(pymod,"hist"); + s_python_function_scatter = safe_import(pymod,"scatter"); + s_python_function_boxplot = safe_import(pymod,"boxplot"); + s_python_function_subplot = safe_import(pymod, "subplot"); + s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); + s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_ylim = safe_import(pymod, "ylim"); + s_python_function_title = safe_import(pymod, "title"); + s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axvline = safe_import(pymod, "axvline"); + s_python_function_axvspan = safe_import(pymod, "axvspan"); + s_python_function_xlabel = safe_import(pymod, "xlabel"); + s_python_function_ylabel = safe_import(pymod, "ylabel"); + s_python_function_gca = safe_import(pymod, "gca"); + s_python_function_xticks = safe_import(pymod, "xticks"); + s_python_function_yticks = safe_import(pymod, "yticks"); + s_python_function_margins = safe_import(pymod, "margins"); + s_python_function_tick_params = safe_import(pymod, "tick_params"); + s_python_function_grid = safe_import(pymod, "grid"); + s_python_function_xlim = safe_import(pymod, "xlim"); + s_python_function_ion = safe_import(pymod, "ion"); + s_python_function_ginput = safe_import(pymod, "ginput"); + s_python_function_save = safe_import(pylabmod, "savefig"); + s_python_function_annotate = safe_import(pymod,"annotate"); + s_python_function_cla = safe_import(pymod, "cla"); + s_python_function_clf = safe_import(pymod, "clf"); + s_python_function_errorbar = safe_import(pymod, "errorbar"); + s_python_function_tight_layout = safe_import(pymod, "tight_layout"); + s_python_function_stem = safe_import(pymod, "stem"); + s_python_function_xkcd = safe_import(pymod, "xkcd"); + s_python_function_text = safe_import(pymod, "text"); + s_python_function_suptitle = safe_import(pymod, "suptitle"); + s_python_function_bar = safe_import(pymod,"bar"); + s_python_function_barh = safe_import(pymod, "barh"); + s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); + s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); +#ifndef WITHOUT_NUMPY + s_python_function_imshow = safe_import(pymod, "imshow"); +#endif + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +/// Select the backend +/// +/// **NOTE:** This must be called before the first plot command to have +/// any effect. +/// +/// Mainly useful to select the non-interactive 'Agg' backend when running +/// matplotlibcpp in headless mode, for example on a machine with no display. +/// +/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + detail::_interpreter::get(); + + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +namespace detail { + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + +// Sanity checks; comment them out or change the numpy type below if you're compiling on +// a platform where they don't apply +static_assert(sizeof(long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +static_assert(sizeof(unsigned long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; +// TODO: add int, long, etc. + +template +PyObject* get_array(const std::vector& v) +{ + npy_intp vsize = v.size(); + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) { + size_t memsize = v.size()*sizeof(double); + double* dp = static_cast(::malloc(memsize)); + for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); + return varray; + } + + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + + +template +PyObject* get_2darray(const std::vector<::std::vector>& v) +{ + if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); + + npy_intp vsize[2] = {static_cast(v.size()), + static_cast(v[0].size())}; + + PyArrayObject *varray = + (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); + + double *vd_begin = static_cast(PyArray_DATA(varray)); + + for (const ::std::vector &v_row : v) { + if (v_row.size() != static_cast(vsize[1])) + throw std::runtime_error("Missmatched array size"); + std::copy(v_row.begin(), v_row.end(), vd_begin); + vd_begin += vsize[1]; + } + + return reinterpret_cast(varray); +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template +PyObject* get_array(const std::vector& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +// sometimes, for labels and such, we need string arrays +inline PyObject * get_array(const std::vector& strings) +{ + PyObject* list = PyList_New(strings.size()); + for (std::size_t i = 0; i < strings.size(); ++i) { + PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); + } + return list; +} + +// not all matplotlib need 2d arrays, some prefer lists of lists +template +PyObject* get_listlist(const std::vector>& ll) +{ + PyObject* listlist = PyList_New(ll.size()); + for (std::size_t i = 0; i < ll.size(); ++i) { + PyList_SetItem(listlist, i, get_array(ll[i])); + } + return listlist; +} + +} // namespace detail + +/// Plot a line through the given x and y data points.. +/// +/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html +template +bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +// TODO - it should be possible to make this work by implementing +// a non-numpy alternative for `detail::get_2darray()`. +#ifndef WITHOUT_NUMPY +template +void plot_surface(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // We lazily load the modules here the first time this function is called + // because I'm not sure that we can assume "matplotlib installed" implies + // "mpl_toolkits installed" on all platforms, and we don't want to require + // it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); + PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); + if (!plot_surface) throw std::runtime_error("No surface"); + Py_INCREF(plot_surface); + PyObject *res = PyObject_Call(plot_surface, args, kwargs); + if (!res) throw std::runtime_error("failed surface"); + Py_DECREF(plot_surface); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} +#endif // WITHOUT_NUMPY + +template +void plot3(const std::vector &x, + const std::vector &y, + const std::vector &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if (res) Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* y1array = detail::get_array(y1); + PyObject* y2array = detail::get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", + const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { + PyObject* obj_x = PyFloat_FromDouble(x); + PyObject* obj_y = PyFloat_FromDouble(y); + PyObject* obj_end_x = PyFloat_FromDouble(end_x); + PyObject* obj_end_y = PyFloat_FromDouble(end_y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); + PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, obj_x); + PyTuple_SetItem(plot_args, 1, obj_y); + PyTuple_SetItem(plot_args, 2, obj_end_x); + PyTuple_SetItem(plot_args, 3, obj_end_y); + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector& y, long bins=10,std::string color="b", + double alpha=1.0, bool cumulative=false) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +namespace detail { + +inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) +{ + assert(type == NPY_UINT8 || type == NPY_FLOAT); + assert(colors == 1 || colors == 3 || colors == 4); + + detail::_interpreter::get(); + + // construct args + npy_intp dims[3] = { rows, columns, colors }; + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) + throw std::runtime_error("Call to imshow() failed"); + if (out) + *out = res; + else + Py_DECREF(res); +} + +} // namespace detail + +inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); +} + +inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); +} + +#ifdef WITH_OPENCV +void imshow(const cv::Mat &image, const std::map &keywords = {}) +{ + // Convert underlying type of matrix, if needed + cv::Mat image2; + NPY_TYPES npy_type = NPY_UINT8; + switch (image.type() & CV_MAT_DEPTH_MASK) { + case CV_8U: + image2 = image; + break; + case CV_32F: + image2 = image; + npy_type = NPY_FLOAT; + break; + default: + image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); + } + + // If color image, convert from BGR to RGB + switch (image2.channels()) { + case 3: + cv::cvtColor(image2, image2, CV_BGR2RGB); + break; + case 4: + cv::cvtColor(image2, image2, CV_BGRA2RGBA); + } + + detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); +} +#endif // WITH_OPENCV +#endif // WITHOUT_NUMPY + +template +bool scatter(const std::vector& x, + const std::vector& y, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector>& data, + const std::vector& labels = {}, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* listlist = detail::get_listlist(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, listlist); + + PyObject* kwargs = PyDict_New(); + + // kwargs needs the labels, if there are (the correct number of) labels + if (!labels.empty() && labels.size() == data.size()) { + PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); + } + + // take care of the remaining keywords + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector& data, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* vector = detail::get_array(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, vector); + + PyObject* kwargs = PyDict_New(); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & x, + const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject * xarray = detail::get_array(x); + PyObject * yarray = detail::get_array(y); + + PyObject * kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = + keywords.begin(); + it != keywords.end(); + ++it) { + PyDict_SetItemString( + kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject * plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject * res = PyObject_Call( + detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + using T = typename std::remove_reference::type::value_type; + + detail::_interpreter::get(); + + std::vector x; + for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } + + return bar(x, y, ec, ls, lw, keywords); +} + + +template +bool barh(const std::vector &x, const std::vector &y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map &keywords = { }) { + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + + PyObject *kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_barh, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + + +inline bool subplots_adjust(const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(it->second)); + } + + + PyObject* plot_args = PyTuple_New(0); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool contour(const std::vector& x, const std::vector& y, + const std::vector& z, + const std::map& keywords = {}) { + assert(x.size() == y.size() && x.size() == z.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* zarray = get_array(z); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, zarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) +{ + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, uarray); + PyTuple_SetItem(plot_args, 3, warray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* yerrarray = detail::get_array(yerr); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for(size_t i=0; i +bool plot(const std::vector& y, const std::map& keywords) +{ + std::vector x(y.size()); + for(size_t i=0; i +bool stem(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +template +void text(Numeric x, Numeric y, const std::string& s = "") +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); + if(!res) throw std::runtime_error("Call to text() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) +{ + if (mappable == NULL) + throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); + + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, mappable); + + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); + if(!res) throw std::runtime_error("Call to colorbar() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + + +inline long figure(long number = -1) +{ + detail::_interpreter::get(); + + PyObject *res; + if (number == -1) + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + else { + assert(number > 0); + + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); + Py_DECREF(args); + } + + if(!res) throw std::runtime_error("Call to figure() failed."); + + PyObject* num = PyObject_GetAttrString(res, "number"); + if (!num) throw std::runtime_error("Could not get number attribute of figure object"); + const long figureNumber = PyLong_AsLong(num); + + Py_DECREF(num); + Py_DECREF(res); + + return figureNumber; +} + +inline bool fignum_exists(long number) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); + if(!res) throw std::runtime_error("Call to fignum_exists() failed."); + + bool ret = PyObject_IsTrue(res); + Py_DECREF(res); + Py_DECREF(args); + + return ret; +} + +inline void figure_size(size_t w, size_t h) +{ + detail::_interpreter::get(); + + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +inline void legend(const std::map& keywords) +{ + detail::_interpreter::get(); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(kwargs); + Py_DECREF(res); +} + +template +void ylim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template +void xlim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +template +inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to xticks() failed"); + + Py_DECREF(res); +} + +template +inline void xticks(const std::vector &ticks, const std::map& keywords) +{ + xticks(ticks, {}, keywords); +} + +template +inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to yticks() failed"); + + Py_DECREF(res); +} + +template +inline void yticks(const std::vector &ticks, const std::map& keywords) +{ + yticks(ticks, {}, keywords); +} + +template inline void margins(Numeric margin) +{ + // construct positional args + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template inline void margins(Numeric margin_x, Numeric margin_y) +{ + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline void tick_params(const std::map& keywords, const std::string axis = "both") +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args; + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) throw std::runtime_error("Call to tick_params() failed"); + + Py_DECREF(res); +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) +{ + detail::_interpreter::get(); + + PyObject* shape = PyTuple_New(2); + PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); + PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); + + PyObject* loc = PyTuple_New(2); + PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); + PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); + + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, shape); + PyTuple_SetItem(args, 1, loc); + PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); + PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); + if(!res) throw std::runtime_error("Call to subplot2grid() failed."); + + Py_DECREF(shape); + Py_DECREF(loc); + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pysuptitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); + if(!res) throw std::runtime_error("Call to suptitle() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + detail::_interpreter::get(); + + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + // construct positional args + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + if (it->first == "linewidth" || it->first == "alpha") + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); + else + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void xlabel(const std::string &str, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void set_zlabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); + if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); + Py_INCREF(zlabel); + + PyObject *res = PyObject_Call(zlabel, args, kwargs); + if (!res) throw std::runtime_error("Call to set_zlabel() failed."); + Py_DECREF(zlabel); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +inline void grid(bool flag) +{ + detail::_interpreter::get(); + + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + detail::_interpreter::get(); + + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + detail::_interpreter::get(); + + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template +inline void pause(Numeric interval) +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + detail::_interpreter::get(); + + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); + if (!res) throw std::runtime_error("Call to save() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void clf() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_clf, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to clf() failed."); + + Py_DECREF(res); +} + +inline void cla() { + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) + throw std::runtime_error("Call to cla() failed."); + + Py_DECREF(res); +} + +inline void ion() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_ion, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to ion() failed."); + + Py_DECREF(res); +} + +inline std::vector> ginput(const int numClicks = 1, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_ginput, args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(args); + if (!res) throw std::runtime_error("Call to ginput() failed."); + + const size_t len = PyList_Size(res); + std::vector> out; + out.reserve(len); + for (size_t i = 0; i < len; i++) { + PyObject *current = PyList_GetItem(res, i); + std::array position; + position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); + position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); + out.push_back(position); + } + Py_DECREF(res); + + return out; +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +// Support for variadic plot() and initializer lists: + +namespace detail { + +template +using is_function = typename std::is_function>>::type; + +template +struct is_callable_impl; + +template +struct is_callable_impl +{ + typedef is_function type; +}; // a non-object is callable iff it is a function + +template +struct is_callable_impl +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + +public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template +struct is_callable +{ + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; +}; + +template +struct plot_impl { }; + +template<> +struct plot_impl +{ + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + detail::_interpreter::get(); + + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl +{ + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template +bool plot() { return true; } + +template +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); +} + +inline bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); +} + +inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); +} + +/* + * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting + */ +class Plot +{ +public: + // default initialization with plot label, some data and format + template + Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* kwargs = PyDict_New(); + if(name != "") + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if(res) + { + line= PyList_GetItem(res, 0); + + if(line) + set_data_fct = PyObject_GetAttrString(line,"set_data"); + else + Py_DECREF(line); + Py_DECREF(res); + } + } + + // shorter initialization with name or format only + // basically calls line, = plot([], []) + Plot(const std::string& name = "", const std::string& format = "") + : Plot(name, std::vector(), std::vector(), format) {} + + template + bool update(const std::vector& x, const std::vector& y) { + assert(x.size() == y.size()); + if(set_data_fct) + { + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_CallObject(set_data_fct, plot_args); + if (res) Py_DECREF(res); + return res; + } + return false; + } + + // clears the plot but keep it available + bool clear() { + return update(std::vector(), std::vector()); + } + + // definitely remove this line + void remove() { + if(line) + { + auto remove_fct = PyObject_GetAttrString(line,"remove"); + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(remove_fct, args); + if (res) Py_DECREF(res); + } + decref(); + } + + ~Plot() { + decref(); + } +private: + + void decref() { + if(line) + Py_DECREF(line); + if(set_data_fct) + Py_DECREF(set_data_fct); + } + + + PyObject* line = nullptr; + PyObject* set_data_fct = nullptr; +}; + +} // end namespace matplotlibcpp diff --git a/include/common/utilities/typeTrans.h b/include/common/utilities/typeTrans.h new file mode 100755 index 0000000..1dbb7ea --- /dev/null +++ b/include/common/utilities/typeTrans.h @@ -0,0 +1,90 @@ +#ifndef TYPETRANS_H +#define TYPETRANS_H + +#include +#include +#include + +/* 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 &vec, double value){ + vec.push_back(value); +} + +inline double getValue(std::vector &vec, double value){ + value = vec.at(0); + std::vector::iterator begin = vec.begin(); + vec.erase(begin); + + return value; +} + +inline void addValue(std::vector &vec, Eigen::MatrixXd value){ + for(int i(0); i &vec, Eigen::MatrixXd value){ + std::vector::iterator begin = vec.begin(); + std::vector::iterator end = begin; + + for(int i(0); i +inline void combineToVector(std::vector &vec, T value){ + addValue(vec, value); +} + +template +inline void combineToVector(std::vector &vec, const T t, const Args... rest){ + combineToVector(vec, t); + + combineToVector(vec, rest...); +} + + +/* extract different type variables from vector */ +template +inline void extractVector(std::vector &vec, T &value){ + value = getValue(vec, value); +} + +template +inline void extractVector(std::vector &vec, T &t, Args&... rest){ + extractVector(vec, t); + + extractVector(vec, rest...); +} + +} + +#endif // TYPETRANS_H \ No newline at end of file diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h new file mode 100755 index 0000000..5ee7b24 --- /dev/null +++ b/include/control/CtrlComponents.h @@ -0,0 +1,84 @@ +#ifndef CTRLCOMPONENTS_H +#define CTRLCOMPONENTS_H + + +#include "message/LowlevelCmd.h" +#include "message/LowlevelState.h" +#include "interface/IOInterface.h" +#include "interface/IOROS.h" +#include "model/ArmDynKineModel.h" +#include "model/ArmReal.h" +#include "common/utilities/CSVTool.h" +#include +#include + +#ifdef COMPILE_DEBUG +#include "common/utilities/PyPlot.h" +#endif // COMPILE_DEBUG + +struct CtrlComponents{ +public: + CtrlComponents(){} + ~CtrlComponents(){ + delete lowCmd; + delete lowState; + delete ioInter; + delete armModel; + delete stateCSV; + + + +#ifdef COMPILE_DEBUG + delete plot; +#endif // COMPILE_DEBUG + } + + int dof; + std::string armConfigPath; + LowlevelCmd *lowCmd; + LowlevelState *lowState; + IOInterface *ioInter; + ArmDynKineModel *armModel; + CSVTool *stateCSV; + +#ifdef COMPILE_DEBUG + PyPlot *plot; +#endif // COMPILE_DEBUG + + double dt; + bool *running; + + void sendRecv(){ + ioInter->sendRecv(lowCmd, lowState); + } + + void geneObj(){ + lowCmd = new LowlevelCmd(); + lowState = new LowlevelState(dt); + +#ifdef NO_GRIPPER + // armModel = new Z1DynKineModel(); + armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0), + 0.0, Vec3(0.0, 0.0, 0.0), Vec3(0.0, 0.0, 0.0).asDiagonal()); +#endif +#ifdef AG95 + armModel = new Z1DynKineModel(Vec3(0.21, 0, 0), + 1.107, Vec3(0.092, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal()); +#endif +#ifdef MASS3KG + armModel = new Z1DynKineModel(Vec3(0.05, 0, 0), + 3.1, Vec3(0.05, 0, 0), Vec3(0.005, 0.005, 0.005).asDiagonal()); +#endif +#ifdef UNITREE_GRIPPER + // 安装面高出6mm + // armModel = new Z1DynKineModel(Vec3(0.141, 0, -0.005), + // 0.712, Vec3(0.0488, 0.0, 0.002), Vec3(0.000688, 0.000993, 0.000812).asDiagonal()); + armModel = new Z1DynKineModel(Vec3(0.0, 0.0, 0.0), + 0.80225, Vec3(0.0037, 0.0014, -0.0003), Vec3(0.00057593, 0.00099960, 0.00106337).asDiagonal()); +#endif + } + +private: +}; + +#endif // CTRLCOMPONENTS_H \ No newline at end of file diff --git a/include/interface/IOInterface.h b/include/interface/IOInterface.h new file mode 100755 index 0000000..a5ab94c --- /dev/null +++ b/include/interface/IOInterface.h @@ -0,0 +1,35 @@ +#ifndef IOINTERFACE_H +#define IOINTERFACE_H + +#include "message/LowlevelCmd.h" +#include "message/LowlevelState.h" +#include "unitree_arm_sdk/cmdPanel.h" +#include "model/ArmReal.h" +#include "unitree_arm_sdk/keyboard.h" +#include + +class IOInterface{ +public: + IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){} + + virtual ~IOInterface(){delete _cmdPanel;}; + virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0; + + // void zeroCmdPanel(){_cmdPanel->setZero();} + std::string getString(std::string slogan){return _cmdPanel->getString(slogan);} + std::vector stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);} + std::vector > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);} + virtual bool calibration(){}; + +#ifdef CTRL_BY_SDK + SendCmd getSendCmd(){return _cmdPanel->getSendCmd();}; + void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);}; +#endif + +protected: + CmdPanel *_cmdPanel; + + +}; + +#endif //IOINTERFACE_H \ No newline at end of file diff --git a/include/interface/IOROS.h b/include/interface/IOROS.h new file mode 100755 index 0000000..7cdfeb8 --- /dev/null +++ b/include/interface/IOROS.h @@ -0,0 +1,50 @@ +#ifdef COMPILE_WITH_SIMULATION + +#ifndef IOROS_H +#define IOROS_H + +#include +#include "interface/IOInterface.h" +#include "unitree_arm_sdk/keyboard.h" +#include "message/unitree_legged_msgs/MotorCmd.h" +#include "message/unitree_legged_msgs/MotorState.h" + +class IOROS : public IOInterface{ +public: + IOROS(CmdPanel *cmdPanel); + ~IOROS(); + bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); + // void setGripper(double position, double force){}; + // void getGripper(double &position, double &force){}; +private: + ros::NodeHandle _nm; +#ifndef UNITREE_GRIPPER + ros::Subscriber _servo_sub[6]; + ros::Publisher _servo_pub[6]; + unitree_legged_msgs::MotorState _joint_state[6]; + unitree_legged_msgs::MotorCmd _joint_cmd[6]; +#endif +#ifdef UNITREE_GRIPPER + ros::Subscriber _servo_sub[7]; + ros::Publisher _servo_pub[7]; + unitree_legged_msgs::MotorState _joint_state[7]; + unitree_legged_msgs::MotorCmd _joint_cmd[7]; +#endif + void _sendCmd(const LowlevelCmd *cmd); + void _recvState(LowlevelState *state); + void _initRecv(); + void _initSend(); + + void _joint00Callback(const unitree_legged_msgs::MotorState& msg); + void _joint01Callback(const unitree_legged_msgs::MotorState& msg); + void _joint02Callback(const unitree_legged_msgs::MotorState& msg); + void _joint03Callback(const unitree_legged_msgs::MotorState& msg); + void _joint04Callback(const unitree_legged_msgs::MotorState& msg); + void _joint05Callback(const unitree_legged_msgs::MotorState& msg); + void _gripperCallback(const unitree_legged_msgs::MotorState& msg); + +}; + +#endif // IOROS_H + +#endif // COMPILE_WITH_SIMULATION \ No newline at end of file diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h new file mode 100644 index 0000000..e3863d8 --- /dev/null +++ b/include/interface/IOUDP.h @@ -0,0 +1,55 @@ +#ifndef IOUDP_H +#define IOUDP_H + +#include "interface/IOInterface.h" +#include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h" + +// #pragma pack(1) +// struct JointCmd{ +// _Float32 T; +// _Float32 W; +// _Float32 Pos; +// _Float32 K_P; +// _Float32 K_W; +// }; + +// struct JointState{ +// _Float32 T; +// _Float32 W; +// _Float32 Acc; +// _Float32 Pos; +// }; + +// union SendCmd{ +// uint8_t checkCmd; +// JointCmd jointCmd[7]; +// }; + +// union RecvState{ +// uint8_t singleState; +// uint8_t selfCheck[10]; +// JointState jointState[7]; +// uint8_t errorCheck[16]; +// }; +// #pragma pack() + +class IOUDP : public IOInterface{ +public: + IOUDP(CmdPanel *cmdPanel); + ~IOUDP(); + bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); + // void setGripper(int position, int force); + // void getGripper(int &position, int &force); + + bool calibration(); +private: + IOPort *_ioPort; + + UDPSendCmd _cmd; + UDPRecvState _state; + + size_t _motorNum; + size_t _jointNum; +}; + +#endif // IOUDP_H \ No newline at end of file diff --git a/include/message/LowlevelCmd.h b/include/message/LowlevelCmd.h new file mode 100755 index 0000000..aa07686 --- /dev/null +++ b/include/message/LowlevelCmd.h @@ -0,0 +1,61 @@ +#ifndef LOWLEVELCMD_H +#define LOWLEVELCMD_H + +#include "common/math/mathTypes.h" +#include "common/math/mathTools.h" +#include +#include + +struct LowlevelCmd{ +private: + size_t _dof = 6; +public: + std::vector q; + std::vector dq; + std::vector tau; + std::vector kp; + std::vector kd; + + + LowlevelCmd(){ +#ifndef UNITREE_GRIPPER + q.resize(_dof); + dq.resize(_dof); + tau.resize(_dof); + kp.resize(_dof); + kd.resize(_dof); +#endif + +#ifdef UNITREE_GRIPPER + q.resize(_dof+1); + dq.resize(_dof+1); + tau.resize(_dof+1); + kp.resize(_dof+1); + kd.resize(_dof+1); +#endif + } + ~LowlevelCmd(){} + + void setZeroDq(); + void setZeroTau(); + void setZeroKp(); + void setZeroKd(); + void setControlGain(); + void setControlGain(std::vector KP, std::vector KW); + void setQ(std::vector qInput); + void setQ(VecX qInput); + void setQd(VecX qDInput); + void setTau(VecX tauInput); + void setPassive(); + void setGripperGain(); + void setGripperGain(float KP, float KW); + void setGripperZeroGain(); + void setGripperQ(double qInput); + void setGripperQd(double qdInput); + void setGripperTau(double tauInput); + Vec6 getQ(); + Vec6 getQd(); +}; + + +#endif //LOWLEVELCMD_H diff --git a/include/message/LowlevelState.h b/include/message/LowlevelState.h new file mode 100755 index 0000000..6df090a --- /dev/null +++ b/include/message/LowlevelState.h @@ -0,0 +1,167 @@ +#ifndef LOWLEVELSTATE_HPP +#define LOWLEVELSTATE_HPP + +#include +#include +#include "common/math/mathTypes.h" +#include "common/math/mathTools.h" +#include "common/enumClass.h" +#include "common/utilities/LPFilter.h" +#include "message/UserValue.h" + +// struct MotorState +// { +// unsigned int mode; +// float q; +// float dq; +// float ddq; +// float tauEst; + +// MotorState(){ +// q = 0; +// dq = 0; +// ddq = 0; +// tauEst = 0; +// } +// }; + +struct LowlevelState{ +private: + size_t _dof = 6; +public: + LowlevelState(double dt){ +#ifndef UNITREE_GRIPPER + qFilter = new LPFilter(dt, 3.0, _dof); + dqFilter = new LPFilter(dt, 3.0, _dof); + tauFilter = new LPFilter(dt, 5.0, _dof); + + q.resize(_dof); + dq.resize(_dof); + ddq.resize(_dof); + tau.resize(_dof); + qFiltered.resize(_dof); + dqFiltered.resize(_dof); + tauFiltered.resize(_dof); +#endif +#ifdef UNITREE_GRIPPER + qFilter = new LPFilter(dt, 3.0, _dof+1); + dqFilter = new LPFilter(dt, 3.0, _dof+1); + tauFilter = new LPFilter(dt, 5.0, _dof+1); + + q.resize(_dof+1); + dq.resize(_dof+1); + ddq.resize(_dof+1); + tau.resize(_dof+1); + qFiltered.resize(_dof+1); + dqFiltered.resize(_dof+1); + tauFiltered.resize(_dof+1); +#endif + } + ~LowlevelState(){ + delete qFilter; + delete dqFilter; + delete tauFilter; + } + + // MotorState motorState[12]; + std::vector q; + std::vector dq; + std::vector ddq; + std::vector tau; + + std::vector qFiltered; + std::vector dqFiltered; + std::vector tauFiltered; + + // ArmFSMStateName userCmd; + UserValue userValue; + LPFilter *qFilter; + LPFilter *dqFilter; + LPFilter *tauFilter; + + void runFilter(){ + qFiltered = q; + qFilter->addValue(qFiltered); + + dqFiltered = dq; + dqFilter->addValue(dqFiltered); + + tauFiltered = tau; + tauFilter->addValue(tauFiltered); + } + + Vec6 getQ(){ + Vec6 qReturn; + for(int i(0); i < 6; ++i){ + qReturn(i) = q.at(i); + } + return qReturn; + } + + Vec6 getQd(){ + Vec6 qdReturn; + for(int i(0); i < 6; ++i){ + qdReturn(i) = dq.at(i); + } + return qdReturn; + } + + Vec6 getQdd(){ + Vec6 qddReturn; + for(int i(0); i < 6; ++i){ + qddReturn(i) = ddq.at(i); + } + return qddReturn; + } + + Vec6 getTau(){ + Vec6 tauReturn; + for(int i(0); i < 6; ++i){ + tauReturn(i) = tau.at(i); + } + return tauReturn; + } + + Vec6 getQFiltered(){ + Vec6 qReturn; + for(int i(0); i < 6; ++i){ + qReturn(i) = qFiltered.at(i); + } + return qReturn; + } + + Vec6 getQdFiltered(){ + Vec6 dqReturn; + for(int i(0); i < 6; ++i){ + dqReturn(i) = dqFiltered.at(i); + } + return dqReturn; + } + + + Vec6 getTauFiltered(){ + Vec6 tauReturn; + for(int i(0); i < 6; ++i){ + tauReturn(i) = tauFiltered.at(i); + } + return tauReturn; + } + + double getGripperQ(){ + return q.at(q.size()-1); + } + + double getGripperQd(){ + return dq.at(q.size()-1); + } + + double getGripperTau(){ + return tau.at(tau.size()-1); + } + + double getGripperTauFiltered(){ + return tauFiltered.at(tau.size()-1); + } +}; + +#endif //LOWLEVELSTATE_HPP diff --git a/include/message/UserValue.h b/include/message/UserValue.h new file mode 100644 index 0000000..1c208a1 --- /dev/null +++ b/include/message/UserValue.h @@ -0,0 +1,56 @@ +#ifndef USERVALUE_H +#define USERVALUE_H + +#include "common/math/mathTypes.h" +#include "common/utilities/typeTrans.h" +#include + +template +std::vector cutVector(std::vector vec, size_t startID, size_t length){ + std::vector result; + result.assign(vec.begin()+startID, vec.begin()+startID+length); + return result; +} + +struct UserValue{ + Vec6 moveAxis; + double gripperPos; + + void setData(std::vector rawData){ +#ifdef CTRL_BY_SDK + if(rawData.size() != 7){ + std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; + } + gripperPos = rawData.at(6); + rawData = cutVector(rawData, 0, 6); + moveAxis = typeTrans::getValue(rawData, moveAxis); +#endif +#ifdef CTRL_BY_KEYBOARD + if(rawData.size() != 7){ + std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; + } + gripperPos = rawData.at(6); + rawData = cutVector(rawData, 0, 6); + moveAxis = typeTrans::getValue(rawData, moveAxis); +#endif +#ifdef CTRL_BY_JOYSTICK + if(rawData.size() != 7){ + std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl; + } + gripperPos = rawData.at(6); + rawData = cutVector(rawData, 0, 6); + moveAxis = typeTrans::getValue(rawData, moveAxis); +#endif + } + + UserValue(){ + setZero(); + } + void setZero(){ + moveAxis.setZero(); + gripperPos = 0; + // gripperTau = 0; + } +}; + +#endif \ No newline at end of file diff --git a/include/message/unitree_legged_msgs/Cartesian.h b/include/message/unitree_legged_msgs/Cartesian.h new file mode 100644 index 0000000..064a47e --- /dev/null +++ b/include/message/unitree_legged_msgs/Cartesian.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H +#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct Cartesian_ +{ + typedef Cartesian_ Type; + + Cartesian_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Cartesian_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_ const> ConstPtr; + +}; // struct Cartesian_ + +typedef ::unitree_legged_msgs::Cartesian_ > Cartesian; + +typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian > CartesianPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian const> CartesianConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z; +} + +template +bool operator!=(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::Cartesian_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::Cartesian_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::Cartesian_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::Cartesian_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "cc153912f1453b708d221682bc23d9ac"; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } + static const uint64_t static_value1 = 0xcc153912f1453b70ULL; + static const uint64_t static_value2 = 0x8d221682bc23d9acULL; +}; + +template +struct DataType< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "unitree_legged_msgs/Cartesian"; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::Cartesian_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Cartesian_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::Cartesian_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H diff --git a/include/message/unitree_legged_msgs/HighCmd.h b/include/message/unitree_legged_msgs/HighCmd.h new file mode 100644 index 0000000..985ac58 --- /dev/null +++ b/include/message/unitree_legged_msgs/HighCmd.h @@ -0,0 +1,403 @@ +// Generated by gencpp from file unitree_legged_msgs/HighCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace unitree_legged_msgs +{ +template +struct HighCmd_ +{ + typedef HighCmd_ Type; + + HighCmd_() + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , mode(0) + , forwardSpeed(0.0) + , sideSpeed(0.0) + , rotateSpeed(0.0) + , bodyHeight(0.0) + , footRaiseHeight(0.0) + , yaw(0.0) + , pitch(0.0) + , roll(0.0) + , led() + , wirelessRemote() + , AppRemote() + , reserve(0) + , crc(0) { + wirelessRemote.assign(0); + + AppRemote.assign(0); + } + HighCmd_(const ContainerAllocator& _alloc) + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , mode(0) + , forwardSpeed(0.0) + , sideSpeed(0.0) + , rotateSpeed(0.0) + , bodyHeight(0.0) + , footRaiseHeight(0.0) + , yaw(0.0) + , pitch(0.0) + , roll(0.0) + , led() + , wirelessRemote() + , AppRemote() + , reserve(0) + , crc(0) { + (void)_alloc; + led.assign( ::unitree_legged_msgs::LED_ (_alloc)); + + wirelessRemote.assign(0); + + AppRemote.assign(0); + } + + + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint16_t _commVersion_type; + _commVersion_type commVersion; + + typedef uint16_t _robotID_type; + _robotID_type robotID; + + typedef uint32_t _SN_type; + _SN_type SN; + + typedef uint8_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _forwardSpeed_type; + _forwardSpeed_type forwardSpeed; + + typedef float _sideSpeed_type; + _sideSpeed_type sideSpeed; + + typedef float _rotateSpeed_type; + _rotateSpeed_type rotateSpeed; + + typedef float _bodyHeight_type; + _bodyHeight_type bodyHeight; + + typedef float _footRaiseHeight_type; + _footRaiseHeight_type footRaiseHeight; + + typedef float _yaw_type; + _yaw_type yaw; + + typedef float _pitch_type; + _pitch_type pitch; + + typedef float _roll_type; + _roll_type roll; + + typedef boost::array< ::unitree_legged_msgs::LED_ , 4> _led_type; + _led_type led; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef boost::array _AppRemote_type; + _AppRemote_type AppRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef int32_t _crc_type; + _crc_type crc; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_ const> ConstPtr; + +}; // struct HighCmd_ + +typedef ::unitree_legged_msgs::HighCmd_ > HighCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd > HighCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd const> HighCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & rhs) +{ + return lhs.levelFlag == rhs.levelFlag && + lhs.commVersion == rhs.commVersion && + lhs.robotID == rhs.robotID && + lhs.SN == rhs.SN && + lhs.bandWidth == rhs.bandWidth && + lhs.mode == rhs.mode && + lhs.forwardSpeed == rhs.forwardSpeed && + lhs.sideSpeed == rhs.sideSpeed && + lhs.rotateSpeed == rhs.rotateSpeed && + lhs.bodyHeight == rhs.bodyHeight && + lhs.footRaiseHeight == rhs.footRaiseHeight && + lhs.yaw == rhs.yaw && + lhs.pitch == rhs.pitch && + lhs.roll == rhs.roll && + lhs.led == rhs.led && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.AppRemote == rhs.AppRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc; +} + +template +bool operator!=(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "1a655499a3f64905db59ceed65ca774a"; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } + static const uint64_t static_value1 = 0x1a655499a3f64905ULL; + static const uint64_t static_value2 = 0xdb59ceed65ca774aULL; +}; + +template +struct DataType< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/HighCmd"; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "uint8 levelFlag\n" +"uint16 commVersion # Old version Aliengo does not have\n" +"uint16 robotID # Old version Aliengo does not have\n" +"uint32 SN # Old version Aliengo does not have\n" +"uint8 bandWidth # Old version Aliengo does not have\n" +"uint8 mode\n" +"float32 forwardSpeed\n" +"float32 sideSpeed\n" +"float32 rotateSpeed \n" +"float32 bodyHeight\n" +"float32 footRaiseHeight\n" +"float32 yaw\n" +"float32 pitch\n" +"float32 roll\n" +"LED[4] led\n" +"uint8[40] wirelessRemote\n" +"uint8[40] AppRemote # Old version Aliengo does not have\n" +"uint32 reserve # Old version Aliengo does not have\n" +"int32 crc\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/LED\n" +"uint8 r\n" +"uint8 g\n" +"uint8 b\n" +; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::HighCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.levelFlag); + stream.next(m.commVersion); + stream.next(m.robotID); + stream.next(m.SN); + stream.next(m.bandWidth); + stream.next(m.mode); + stream.next(m.forwardSpeed); + stream.next(m.sideSpeed); + stream.next(m.rotateSpeed); + stream.next(m.bodyHeight); + stream.next(m.footRaiseHeight); + stream.next(m.yaw); + stream.next(m.pitch); + stream.next(m.roll); + stream.next(m.led); + stream.next(m.wirelessRemote); + stream.next(m.AppRemote); + stream.next(m.reserve); + stream.next(m.crc); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct HighCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::HighCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_& v) + { + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "commVersion: "; + Printer::stream(s, indent + " ", v.commVersion); + s << indent << "robotID: "; + Printer::stream(s, indent + " ", v.robotID); + s << indent << "SN: "; + Printer::stream(s, indent + " ", v.SN); + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "forwardSpeed: "; + Printer::stream(s, indent + " ", v.forwardSpeed); + s << indent << "sideSpeed: "; + Printer::stream(s, indent + " ", v.sideSpeed); + s << indent << "rotateSpeed: "; + Printer::stream(s, indent + " ", v.rotateSpeed); + s << indent << "bodyHeight: "; + Printer::stream(s, indent + " ", v.bodyHeight); + s << indent << "footRaiseHeight: "; + Printer::stream(s, indent + " ", v.footRaiseHeight); + s << indent << "yaw: "; + Printer::stream(s, indent + " ", v.yaw); + s << indent << "pitch: "; + Printer::stream(s, indent + " ", v.pitch); + s << indent << "roll: "; + Printer::stream(s, indent + " ", v.roll); + s << indent << "led[]" << std::endl; + for (size_t i = 0; i < v.led.size(); ++i) + { + s << indent << " led[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::LED_ >::stream(s, indent + " ", v.led[i]); + } + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "AppRemote[]" << std::endl; + for (size_t i = 0; i < v.AppRemote.size(); ++i) + { + s << indent << " AppRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.AppRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H diff --git a/include/message/unitree_legged_msgs/HighState.h b/include/message/unitree_legged_msgs/HighState.h new file mode 100644 index 0000000..5cadaf6 --- /dev/null +++ b/include/message/unitree_legged_msgs/HighState.h @@ -0,0 +1,497 @@ +// Generated by gencpp from file unitree_legged_msgs/HighState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace unitree_legged_msgs +{ +template +struct HighState_ +{ + typedef HighState_ Type; + + HighState_() + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , mode(0) + , imu() + , forwardSpeed(0.0) + , sideSpeed(0.0) + , rotateSpeed(0.0) + , bodyHeight(0.0) + , updownSpeed(0.0) + , forwardPosition(0.0) + , sidePosition(0.0) + , footPosition2Body() + , footSpeed2Body() + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForce() + , jointP() { + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + + jointP.assign(0.0); + } + HighState_(const ContainerAllocator& _alloc) + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , mode(0) + , imu(_alloc) + , forwardSpeed(0.0) + , sideSpeed(0.0) + , rotateSpeed(0.0) + , bodyHeight(0.0) + , updownSpeed(0.0) + , forwardPosition(0.0) + , sidePosition(0.0) + , footPosition2Body() + , footSpeed2Body() + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForce() + , jointP() { + (void)_alloc; + footPosition2Body.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + + eeForce.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + jointP.assign(0.0); + } + + + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint16_t _commVersion_type; + _commVersion_type commVersion; + + typedef uint16_t _robotID_type; + _robotID_type robotID; + + typedef uint32_t _SN_type; + _SN_type SN; + + typedef uint8_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef ::unitree_legged_msgs::IMU_ _imu_type; + _imu_type imu; + + typedef float _forwardSpeed_type; + _forwardSpeed_type forwardSpeed; + + typedef float _sideSpeed_type; + _sideSpeed_type sideSpeed; + + typedef float _rotateSpeed_type; + _rotateSpeed_type rotateSpeed; + + typedef float _bodyHeight_type; + _bodyHeight_type bodyHeight; + + typedef float _updownSpeed_type; + _updownSpeed_type updownSpeed; + + typedef float _forwardPosition_type; + _forwardPosition_type forwardPosition; + + typedef float _sidePosition_type; + _sidePosition_type sidePosition; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _footPosition2Body_type; + _footPosition2Body_type footPosition2Body; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _footSpeed2Body_type; + _footSpeed2Body_type footSpeed2Body; + + typedef boost::array _footForce_type; + _footForce_type footForce; + + typedef boost::array _footForceEst_type; + _footForceEst_type footForceEst; + + typedef uint32_t _tick_type; + _tick_type tick; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForce_type; + _eeForce_type eeForce; + + typedef boost::array _jointP_type; + _jointP_type jointP; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ const> ConstPtr; + +}; // struct HighState_ + +typedef ::unitree_legged_msgs::HighState_ > HighState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::HighState > HighStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::HighState const> HighStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::HighState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & rhs) +{ + return lhs.levelFlag == rhs.levelFlag && + lhs.commVersion == rhs.commVersion && + lhs.robotID == rhs.robotID && + lhs.SN == rhs.SN && + lhs.bandWidth == rhs.bandWidth && + lhs.mode == rhs.mode && + lhs.imu == rhs.imu && + lhs.forwardSpeed == rhs.forwardSpeed && + lhs.sideSpeed == rhs.sideSpeed && + lhs.rotateSpeed == rhs.rotateSpeed && + lhs.bodyHeight == rhs.bodyHeight && + lhs.updownSpeed == rhs.updownSpeed && + lhs.forwardPosition == rhs.forwardPosition && + lhs.sidePosition == rhs.sidePosition && + lhs.footPosition2Body == rhs.footPosition2Body && + lhs.footSpeed2Body == rhs.footSpeed2Body && + lhs.footForce == rhs.footForce && + lhs.footForceEst == rhs.footForceEst && + lhs.tick == rhs.tick && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc && + lhs.eeForce == rhs.eeForce && + lhs.jointP == rhs.jointP; +} + +template +bool operator!=(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::HighState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighState_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "a12e8b22df896c82203810ec6d521dad"; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } + static const uint64_t static_value1 = 0xa12e8b22df896c82ULL; + static const uint64_t static_value2 = 0x203810ec6d521dadULL; +}; + +template +struct DataType< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/HighState"; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "uint8 levelFlag\n" +"uint16 commVersion # Old version Aliengo does not have\n" +"uint16 robotID # Old version Aliengo does not have\n" +"uint32 SN # Old version Aliengo does not have\n" +"uint8 bandWidth # Old version Aliengo does not have\n" +"uint8 mode\n" +"IMU imu\n" +"float32 forwardSpeed\n" +"float32 sideSpeed\n" +"float32 rotateSpeed\n" +"float32 bodyHeight\n" +"float32 updownSpeed\n" +"float32 forwardPosition # (will be float type next version) # Old version Aliengo is different\n" +"float32 sidePosition # (will be float type next version) # Old version Aliengo is different\n" +"Cartesian[4] footPosition2Body\n" +"Cartesian[4] footSpeed2Body\n" +"int16[4] footForce # Old version Aliengo is different\n" +"int16[4] footForceEst # Old version Aliengo does not have\n" +"uint32 tick \n" +"uint8[40] wirelessRemote\n" +"uint32 reserve # Old version Aliengo does not have\n" +"uint32 crc\n" +"\n" +"# Under are not defined in SDK yet. # Old version Aliengo does not have\n" +"Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.\n" +"float32[12] jointP # for visualization\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/IMU\n" +"float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"int8 temperature\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/Cartesian\n" +"float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::HighState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.levelFlag); + stream.next(m.commVersion); + stream.next(m.robotID); + stream.next(m.SN); + stream.next(m.bandWidth); + stream.next(m.mode); + stream.next(m.imu); + stream.next(m.forwardSpeed); + stream.next(m.sideSpeed); + stream.next(m.rotateSpeed); + stream.next(m.bodyHeight); + stream.next(m.updownSpeed); + stream.next(m.forwardPosition); + stream.next(m.sidePosition); + stream.next(m.footPosition2Body); + stream.next(m.footSpeed2Body); + stream.next(m.footForce); + stream.next(m.footForceEst); + stream.next(m.tick); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + stream.next(m.eeForce); + stream.next(m.jointP); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct HighState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::HighState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_& v) + { + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "commVersion: "; + Printer::stream(s, indent + " ", v.commVersion); + s << indent << "robotID: "; + Printer::stream(s, indent + " ", v.robotID); + s << indent << "SN: "; + Printer::stream(s, indent + " ", v.SN); + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "imu: "; + s << std::endl; + Printer< ::unitree_legged_msgs::IMU_ >::stream(s, indent + " ", v.imu); + s << indent << "forwardSpeed: "; + Printer::stream(s, indent + " ", v.forwardSpeed); + s << indent << "sideSpeed: "; + Printer::stream(s, indent + " ", v.sideSpeed); + s << indent << "rotateSpeed: "; + Printer::stream(s, indent + " ", v.rotateSpeed); + s << indent << "bodyHeight: "; + Printer::stream(s, indent + " ", v.bodyHeight); + s << indent << "updownSpeed: "; + Printer::stream(s, indent + " ", v.updownSpeed); + s << indent << "forwardPosition: "; + Printer::stream(s, indent + " ", v.forwardPosition); + s << indent << "sidePosition: "; + Printer::stream(s, indent + " ", v.sidePosition); + s << indent << "footPosition2Body[]" << std::endl; + for (size_t i = 0; i < v.footPosition2Body.size(); ++i) + { + s << indent << " footPosition2Body[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.footPosition2Body[i]); + } + s << indent << "footSpeed2Body[]" << std::endl; + for (size_t i = 0; i < v.footSpeed2Body.size(); ++i) + { + s << indent << " footSpeed2Body[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.footSpeed2Body[i]); + } + s << indent << "footForce[]" << std::endl; + for (size_t i = 0; i < v.footForce.size(); ++i) + { + s << indent << " footForce[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForce[i]); + } + s << indent << "footForceEst[]" << std::endl; + for (size_t i = 0; i < v.footForceEst.size(); ++i) + { + s << indent << " footForceEst[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForceEst[i]); + } + s << indent << "tick: "; + Printer::stream(s, indent + " ", v.tick); + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + s << indent << "eeForce[]" << std::endl; + for (size_t i = 0; i < v.eeForce.size(); ++i) + { + s << indent << " eeForce[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.eeForce[i]); + } + s << indent << "jointP[]" << std::endl; + for (size_t i = 0; i < v.jointP.size(); ++i) + { + s << indent << " jointP[" << i << "]: "; + Printer::stream(s, indent + " ", v.jointP[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H diff --git a/include/message/unitree_legged_msgs/IMU.h b/include/message/unitree_legged_msgs/IMU.h new file mode 100644 index 0000000..0df8a22 --- /dev/null +++ b/include/message/unitree_legged_msgs/IMU.h @@ -0,0 +1,247 @@ +// Generated by gencpp from file unitree_legged_msgs/IMU.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_IMU_H +#define UNITREE_LEGGED_MSGS_MESSAGE_IMU_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct IMU_ +{ + typedef IMU_ Type; + + IMU_() + : quaternion() + , gyroscope() + , accelerometer() + , temperature(0) { + quaternion.assign(0.0); + + gyroscope.assign(0.0); + + accelerometer.assign(0.0); + } + IMU_(const ContainerAllocator& _alloc) + : quaternion() + , gyroscope() + , accelerometer() + , temperature(0) { + (void)_alloc; + quaternion.assign(0.0); + + gyroscope.assign(0.0); + + accelerometer.assign(0.0); + } + + + + typedef boost::array _quaternion_type; + _quaternion_type quaternion; + + typedef boost::array _gyroscope_type; + _gyroscope_type gyroscope; + + typedef boost::array _accelerometer_type; + _accelerometer_type accelerometer; + + typedef int8_t _temperature_type; + _temperature_type temperature; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ const> ConstPtr; + +}; // struct IMU_ + +typedef ::unitree_legged_msgs::IMU_ > IMU; + +typedef boost::shared_ptr< ::unitree_legged_msgs::IMU > IMUPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::IMU const> IMUConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::IMU_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) +{ + return lhs.quaternion == rhs.quaternion && + lhs.gyroscope == rhs.gyroscope && + lhs.accelerometer == rhs.accelerometer && + lhs.temperature == rhs.temperature; +} + +template +bool operator!=(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::IMU_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::IMU_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::IMU_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::IMU_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::IMU_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::IMU_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "dd4bb4e42aa2f15aa1fb1b6a3c3752cb"; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } + static const uint64_t static_value1 = 0xdd4bb4e42aa2f15aULL; + static const uint64_t static_value2 = 0xa1fb1b6a3c3752cbULL; +}; + +template +struct DataType< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "unitree_legged_msgs/IMU"; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"int8 temperature\n" +; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::IMU_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.quaternion); + stream.next(m.gyroscope); + stream.next(m.accelerometer); + stream.next(m.temperature); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct IMU_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::IMU_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_& v) + { + s << indent << "quaternion[]" << std::endl; + for (size_t i = 0; i < v.quaternion.size(); ++i) + { + s << indent << " quaternion[" << i << "]: "; + Printer::stream(s, indent + " ", v.quaternion[i]); + } + s << indent << "gyroscope[]" << std::endl; + for (size_t i = 0; i < v.gyroscope.size(); ++i) + { + s << indent << " gyroscope[" << i << "]: "; + Printer::stream(s, indent + " ", v.gyroscope[i]); + } + s << indent << "accelerometer[]" << std::endl; + for (size_t i = 0; i < v.accelerometer.size(); ++i) + { + s << indent << " accelerometer[" << i << "]: "; + Printer::stream(s, indent + " ", v.accelerometer[i]); + } + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H diff --git a/include/message/unitree_legged_msgs/LED.h b/include/message/unitree_legged_msgs/LED.h new file mode 100644 index 0000000..be563ab --- /dev/null +++ b/include/message/unitree_legged_msgs/LED.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file unitree_legged_msgs/LED.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LED_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LED_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct LED_ +{ + typedef LED_ Type; + + LED_() + : r(0) + , g(0) + , b(0) { + } + LED_(const ContainerAllocator& _alloc) + : r(0) + , g(0) + , b(0) { + (void)_alloc; + } + + + + typedef uint8_t _r_type; + _r_type r; + + typedef uint8_t _g_type; + _g_type g; + + typedef uint8_t _b_type; + _b_type b; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LED_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LED_ const> ConstPtr; + +}; // struct LED_ + +typedef ::unitree_legged_msgs::LED_ > LED; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LED > LEDPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LED const> LEDConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LED_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) +{ + return lhs.r == rhs.r && + lhs.g == rhs.g && + lhs.b == rhs.b; +} + +template +bool operator!=(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::LED_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LED_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LED_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LED_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LED_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LED_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "353891e354491c51aabe32df673fb446"; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } + static const uint64_t static_value1 = 0x353891e354491c51ULL; + static const uint64_t static_value2 = 0xaabe32df673fb446ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LED"; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "uint8 r\n" +"uint8 g\n" +"uint8 b\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LED_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.r); + stream.next(m.g); + stream.next(m.b); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LED_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LED_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_& v) + { + s << indent << "r: "; + Printer::stream(s, indent + " ", v.r); + s << indent << "g: "; + Printer::stream(s, indent + " ", v.g); + s << indent << "b: "; + Printer::stream(s, indent + " ", v.b); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H diff --git a/include/message/unitree_legged_msgs/LowCmd.h b/include/message/unitree_legged_msgs/LowCmd.h new file mode 100644 index 0000000..2d2f7c6 --- /dev/null +++ b/include/message/unitree_legged_msgs/LowCmd.h @@ -0,0 +1,348 @@ +// Generated by gencpp from file unitree_legged_msgs/LowCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace unitree_legged_msgs +{ +template +struct LowCmd_ +{ + typedef LowCmd_ Type; + + LowCmd_() + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , motorCmd() + , led() + , wirelessRemote() + , reserve(0) + , crc(0) + , ff() { + wirelessRemote.assign(0); + } + LowCmd_(const ContainerAllocator& _alloc) + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , motorCmd() + , led() + , wirelessRemote() + , reserve(0) + , crc(0) + , ff() { + (void)_alloc; + motorCmd.assign( ::unitree_legged_msgs::MotorCmd_ (_alloc)); + + led.assign( ::unitree_legged_msgs::LED_ (_alloc)); + + wirelessRemote.assign(0); + + ff.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + } + + + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint16_t _commVersion_type; + _commVersion_type commVersion; + + typedef uint16_t _robotID_type; + _robotID_type robotID; + + typedef uint32_t _SN_type; + _SN_type SN; + + typedef uint8_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef boost::array< ::unitree_legged_msgs::MotorCmd_ , 20> _motorCmd_type; + _motorCmd_type motorCmd; + + typedef boost::array< ::unitree_legged_msgs::LED_ , 4> _led_type; + _led_type led; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _ff_type; + _ff_type ff; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ const> ConstPtr; + +}; // struct LowCmd_ + +typedef ::unitree_legged_msgs::LowCmd_ > LowCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd > LowCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd const> LowCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & rhs) +{ + return lhs.levelFlag == rhs.levelFlag && + lhs.commVersion == rhs.commVersion && + lhs.robotID == rhs.robotID && + lhs.SN == rhs.SN && + lhs.bandWidth == rhs.bandWidth && + lhs.motorCmd == rhs.motorCmd && + lhs.led == rhs.led && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc && + lhs.ff == rhs.ff; +} + +template +bool operator!=(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "357432b2562edd0a8e89b9c9f5fc4821"; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } + static const uint64_t static_value1 = 0x357432b2562edd0aULL; + static const uint64_t static_value2 = 0x8e89b9c9f5fc4821ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LowCmd"; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "uint8 levelFlag \n" +"uint16 commVersion # Old version Aliengo does not have\n" +"uint16 robotID # Old version Aliengo does not have\n" +"uint32 SN # Old version Aliengo does not have\n" +"uint8 bandWidth # Old version Aliengo does not have\n" +"MotorCmd[20] motorCmd\n" +"LED[4] led\n" +"uint8[40] wirelessRemote\n" +"uint32 reserve # Old version Aliengo does not have\n" +"uint32 crc\n" +"\n" +"Cartesian[4] ff # will delete # Old version Aliengo does not have\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/MotorCmd\n" +"uint8 mode # motor target mode\n" +"float32 q # motor target position\n" +"float32 dq # motor target velocity\n" +"float32 tau # motor target torque\n" +"float32 Kp # motor spring stiffness coefficient\n" +"float32 Kd # motor damper coefficient\n" +"uint32[3] reserve # motor target torque\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/LED\n" +"uint8 r\n" +"uint8 g\n" +"uint8 b\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/Cartesian\n" +"float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LowCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.levelFlag); + stream.next(m.commVersion); + stream.next(m.robotID); + stream.next(m.SN); + stream.next(m.bandWidth); + stream.next(m.motorCmd); + stream.next(m.led); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + stream.next(m.ff); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LowCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LowCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_& v) + { + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "commVersion: "; + Printer::stream(s, indent + " ", v.commVersion); + s << indent << "robotID: "; + Printer::stream(s, indent + " ", v.robotID); + s << indent << "SN: "; + Printer::stream(s, indent + " ", v.SN); + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "motorCmd[]" << std::endl; + for (size_t i = 0; i < v.motorCmd.size(); ++i) + { + s << indent << " motorCmd[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::MotorCmd_ >::stream(s, indent + " ", v.motorCmd[i]); + } + s << indent << "led[]" << std::endl; + for (size_t i = 0; i < v.led.size(); ++i) + { + s << indent << " led[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::LED_ >::stream(s, indent + " ", v.led[i]); + } + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + s << indent << "ff[]" << std::endl; + for (size_t i = 0; i < v.ff.size(); ++i) + { + s << indent << " ff[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.ff[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H diff --git a/include/message/unitree_legged_msgs/LowState.h b/include/message/unitree_legged_msgs/LowState.h new file mode 100644 index 0000000..118acda --- /dev/null +++ b/include/message/unitree_legged_msgs/LowState.h @@ -0,0 +1,448 @@ +// Generated by gencpp from file unitree_legged_msgs/LowState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace unitree_legged_msgs +{ +template +struct LowState_ +{ + typedef LowState_ Type; + + LowState_() + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , imu() + , motorState() + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForceRaw() + , eeForce() + , position() + , velocity() + , velocity_w() { + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + } + LowState_(const ContainerAllocator& _alloc) + : levelFlag(0) + , commVersion(0) + , robotID(0) + , SN(0) + , bandWidth(0) + , imu(_alloc) + , motorState() + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForceRaw() + , eeForce() + , position(_alloc) + , velocity(_alloc) + , velocity_w(_alloc) { + (void)_alloc; + motorState.assign( ::unitree_legged_msgs::MotorState_ (_alloc)); + + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + + eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + eeForce.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + } + + + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint16_t _commVersion_type; + _commVersion_type commVersion; + + typedef uint16_t _robotID_type; + _robotID_type robotID; + + typedef uint32_t _SN_type; + _SN_type SN; + + typedef uint8_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef ::unitree_legged_msgs::IMU_ _imu_type; + _imu_type imu; + + typedef boost::array< ::unitree_legged_msgs::MotorState_ , 20> _motorState_type; + _motorState_type motorState; + + typedef boost::array _footForce_type; + _footForce_type footForce; + + typedef boost::array _footForceEst_type; + _footForceEst_type footForceEst; + + typedef uint32_t _tick_type; + _tick_type tick; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForceRaw_type; + _eeForceRaw_type eeForceRaw; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForce_type; + _eeForce_type eeForce; + + typedef ::unitree_legged_msgs::Cartesian_ _position_type; + _position_type position; + + typedef ::unitree_legged_msgs::Cartesian_ _velocity_type; + _velocity_type velocity; + + typedef ::unitree_legged_msgs::Cartesian_ _velocity_w_type; + _velocity_w_type velocity_w; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ const> ConstPtr; + +}; // struct LowState_ + +typedef ::unitree_legged_msgs::LowState_ > LowState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LowState > LowStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LowState const> LowStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LowState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & rhs) +{ + return lhs.levelFlag == rhs.levelFlag && + lhs.commVersion == rhs.commVersion && + lhs.robotID == rhs.robotID && + lhs.SN == rhs.SN && + lhs.bandWidth == rhs.bandWidth && + lhs.imu == rhs.imu && + lhs.motorState == rhs.motorState && + lhs.footForce == rhs.footForce && + lhs.footForceEst == rhs.footForceEst && + lhs.tick == rhs.tick && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc && + lhs.eeForceRaw == rhs.eeForceRaw && + lhs.eeForce == rhs.eeForce && + lhs.position == rhs.position && + lhs.velocity == rhs.velocity && + lhs.velocity_w == rhs.velocity_w; +} + +template +bool operator!=(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::LowState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowState_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "cef9d4f6b5a89bd6330896affb1bca88"; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } + static const uint64_t static_value1 = 0xcef9d4f6b5a89bd6ULL; + static const uint64_t static_value2 = 0x330896affb1bca88ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LowState"; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "uint8 levelFlag\n" +"uint16 commVersion # Old version Aliengo does not have\n" +"uint16 robotID # Old version Aliengo does not have\n" +"uint32 SN # Old version Aliengo does not have\n" +"uint8 bandWidth # Old version Aliengo does not have\n" +"IMU imu\n" +"MotorState[20] motorState\n" +"int16[4] footForce # force sensors # Old version Aliengo is different\n" +"int16[4] footForceEst # force sensors # Old version Aliengo does not have\n" +"uint32 tick # reference real-time from motion controller (unit: us)\n" +"uint8[40] wirelessRemote # wireless commands\n" +"uint32 reserve # Old version Aliengo does not have\n" +"uint32 crc\n" +"\n" +"# Old version Aliengo does not have:\n" +"Cartesian[4] eeForceRaw\n" +"Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization \n" +"Cartesian position # will delete\n" +"Cartesian velocity # will delete\n" +"Cartesian velocity_w # will delete\n" +"\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/IMU\n" +"float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"int8 temperature\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/MotorState\n" +"uint8 mode # motor current mode \n" +"float32 q # motor current position（rad）\n" +"float32 dq # motor current speed（rad/s）\n" +"float32 ddq # motor current speed（rad/s）\n" +"float32 tauEst # current estimated output torque（N*m）\n" +"float32 q_raw # motor current position（rad）\n" +"float32 dq_raw # motor current speed（rad/s）\n" +"float32 ddq_raw # motor current speed（rad/s）\n" +"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" +"uint32[2] reserve\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/Cartesian\n" +"float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LowState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.levelFlag); + stream.next(m.commVersion); + stream.next(m.robotID); + stream.next(m.SN); + stream.next(m.bandWidth); + stream.next(m.imu); + stream.next(m.motorState); + stream.next(m.footForce); + stream.next(m.footForceEst); + stream.next(m.tick); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + stream.next(m.eeForceRaw); + stream.next(m.eeForce); + stream.next(m.position); + stream.next(m.velocity); + stream.next(m.velocity_w); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LowState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LowState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_& v) + { + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "commVersion: "; + Printer::stream(s, indent + " ", v.commVersion); + s << indent << "robotID: "; + Printer::stream(s, indent + " ", v.robotID); + s << indent << "SN: "; + Printer::stream(s, indent + " ", v.SN); + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "imu: "; + s << std::endl; + Printer< ::unitree_legged_msgs::IMU_ >::stream(s, indent + " ", v.imu); + s << indent << "motorState[]" << std::endl; + for (size_t i = 0; i < v.motorState.size(); ++i) + { + s << indent << " motorState[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, indent + " ", v.motorState[i]); + } + s << indent << "footForce[]" << std::endl; + for (size_t i = 0; i < v.footForce.size(); ++i) + { + s << indent << " footForce[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForce[i]); + } + s << indent << "footForceEst[]" << std::endl; + for (size_t i = 0; i < v.footForceEst.size(); ++i) + { + s << indent << " footForceEst[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForceEst[i]); + } + s << indent << "tick: "; + Printer::stream(s, indent + " ", v.tick); + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + s << indent << "eeForceRaw[]" << std::endl; + for (size_t i = 0; i < v.eeForceRaw.size(); ++i) + { + s << indent << " eeForceRaw[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.eeForceRaw[i]); + } + s << indent << "eeForce[]" << std::endl; + for (size_t i = 0; i < v.eeForce.size(); ++i) + { + s << indent << " eeForce[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.eeForce[i]); + } + s << indent << "position: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.position); + s << indent << "velocity: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity); + s << indent << "velocity_w: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity_w); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H diff --git a/include/message/unitree_legged_msgs/MotorCmd.h b/include/message/unitree_legged_msgs/MotorCmd.h new file mode 100644 index 0000000..9efff2d --- /dev/null +++ b/include/message/unitree_legged_msgs/MotorCmd.h @@ -0,0 +1,261 @@ +// Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct MotorCmd_ +{ + typedef MotorCmd_ Type; + + MotorCmd_() + : mode(0) + , q(0.0) + , dq(0.0) + , tau(0.0) + , Kp(0.0) + , Kd(0.0) + , reserve() { + reserve.assign(0); + } + MotorCmd_(const ContainerAllocator& _alloc) + : mode(0) + , q(0.0) + , dq(0.0) + , tau(0.0) + , Kp(0.0) + , Kd(0.0) + , reserve() { + (void)_alloc; + reserve.assign(0); + } + + + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _q_type; + _q_type q; + + typedef float _dq_type; + _dq_type dq; + + typedef float _tau_type; + _tau_type tau; + + typedef float _Kp_type; + _Kp_type Kp; + + typedef float _Kd_type; + _Kd_type Kd; + + typedef boost::array _reserve_type; + _reserve_type reserve; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ const> ConstPtr; + +}; // struct MotorCmd_ + +typedef ::unitree_legged_msgs::MotorCmd_ > MotorCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) +{ + return lhs.mode == rhs.mode && + lhs.q == rhs.q && + lhs.dq == rhs.dq && + lhs.tau == rhs.tau && + lhs.Kp == rhs.Kp && + lhs.Kd == rhs.Kd && + lhs.reserve == rhs.reserve; +} + +template +bool operator!=(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "bbb3b7d91319c3a1b99055f0149ba221"; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } + static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL; + static const uint64_t static_value2 = 0xb99055f0149ba221ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/MotorCmd"; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "uint8 mode # motor target mode\n" +"float32 q # motor target position\n" +"float32 dq # motor target velocity\n" +"float32 tau # motor target torque\n" +"float32 Kp # motor spring stiffness coefficient\n" +"float32 Kd # motor damper coefficient\n" +"uint32[3] reserve # motor target torque\n" +; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::MotorCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.mode); + stream.next(m.q); + stream.next(m.dq); + stream.next(m.tau); + stream.next(m.Kp); + stream.next(m.Kd); + stream.next(m.reserve); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MotorCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::MotorCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_& v) + { + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "q: "; + Printer::stream(s, indent + " ", v.q); + s << indent << "dq: "; + Printer::stream(s, indent + " ", v.dq); + s << indent << "tau: "; + Printer::stream(s, indent + " ", v.tau); + s << indent << "Kp: "; + Printer::stream(s, indent + " ", v.Kp); + s << indent << "Kd: "; + Printer::stream(s, indent + " ", v.Kd); + s << indent << "reserve[]" << std::endl; + for (size_t i = 0; i < v.reserve.size(); ++i) + { + s << indent << " reserve[" << i << "]: "; + Printer::stream(s, indent + " ", v.reserve[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H diff --git a/include/message/unitree_legged_msgs/MotorState.h b/include/message/unitree_legged_msgs/MotorState.h new file mode 100644 index 0000000..e7183e0 --- /dev/null +++ b/include/message/unitree_legged_msgs/MotorState.h @@ -0,0 +1,291 @@ +// Generated by gencpp from file unitree_legged_msgs/MotorState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct MotorState_ +{ + typedef MotorState_ Type; + + MotorState_() + : mode(0) + , q(0.0) + , dq(0.0) + , ddq(0.0) + , tauEst(0.0) + , q_raw(0.0) + , dq_raw(0.0) + , ddq_raw(0.0) + , temperature(0) + , reserve() { + reserve.assign(0); + } + MotorState_(const ContainerAllocator& _alloc) + : mode(0) + , q(0.0) + , dq(0.0) + , ddq(0.0) + , tauEst(0.0) + , q_raw(0.0) + , dq_raw(0.0) + , ddq_raw(0.0) + , temperature(0) + , reserve() { + (void)_alloc; + reserve.assign(0); + } + + + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _q_type; + _q_type q; + + typedef float _dq_type; + _dq_type dq; + + typedef float _ddq_type; + _ddq_type ddq; + + typedef float _tauEst_type; + _tauEst_type tauEst; + + typedef float _q_raw_type; + _q_raw_type q_raw; + + typedef float _dq_raw_type; + _dq_raw_type dq_raw; + + typedef float _ddq_raw_type; + _ddq_raw_type ddq_raw; + + typedef int8_t _temperature_type; + _temperature_type temperature; + + typedef boost::array _reserve_type; + _reserve_type reserve; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ const> ConstPtr; + +}; // struct MotorState_ + +typedef ::unitree_legged_msgs::MotorState_ > MotorState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) +{ + return lhs.mode == rhs.mode && + lhs.q == rhs.q && + lhs.dq == rhs.dq && + lhs.ddq == rhs.ddq && + lhs.tauEst == rhs.tauEst && + lhs.q_raw == rhs.q_raw && + lhs.dq_raw == rhs.dq_raw && + lhs.ddq_raw == rhs.ddq_raw && + lhs.temperature == rhs.temperature && + lhs.reserve == rhs.reserve; +} + +template +bool operator!=(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorState_ const> + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "94c55ee3b7852be2bd437b20ce12a254"; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } + static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL; + static const uint64_t static_value2 = 0xbd437b20ce12a254ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/MotorState"; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "uint8 mode # motor current mode \n" +"float32 q # motor current position（rad）\n" +"float32 dq # motor current speed（rad/s）\n" +"float32 ddq # motor current speed（rad/s）\n" +"float32 tauEst # current estimated output torque（N*m）\n" +"float32 q_raw # motor current position（rad）\n" +"float32 dq_raw # motor current speed（rad/s）\n" +"float32 ddq_raw # motor current speed（rad/s）\n" +"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" +"uint32[2] reserve\n" +; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::MotorState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.mode); + stream.next(m.q); + stream.next(m.dq); + stream.next(m.ddq); + stream.next(m.tauEst); + stream.next(m.q_raw); + stream.next(m.dq_raw); + stream.next(m.ddq_raw); + stream.next(m.temperature); + stream.next(m.reserve); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MotorState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::MotorState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_& v) + { + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "q: "; + Printer::stream(s, indent + " ", v.q); + s << indent << "dq: "; + Printer::stream(s, indent + " ", v.dq); + s << indent << "ddq: "; + Printer::stream(s, indent + " ", v.ddq); + s << indent << "tauEst: "; + Printer::stream(s, indent + " ", v.tauEst); + s << indent << "q_raw: "; + Printer::stream(s, indent + " ", v.q_raw); + s << indent << "dq_raw: "; + Printer::stream(s, indent + " ", v.dq_raw); + s << indent << "ddq_raw: "; + Printer::stream(s, indent + " ", v.ddq_raw); + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + s << indent << "reserve[]" << std::endl; + for (size_t i = 0; i < v.reserve.size(); ++i) + { + s << indent << " reserve[" << i << "]: "; + Printer::stream(s, indent + " ", v.reserve[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H diff --git a/include/model/ArmDynKineModel.h b/include/model/ArmDynKineModel.h new file mode 100755 index 0000000..cb692e2 --- /dev/null +++ b/include/model/ArmDynKineModel.h @@ -0,0 +1,114 @@ +#ifndef ARMDYNCKINEMODEL_H +#define ARMDYNCKINEMODEL_H + +#include +#include +#include "common/math/mathTypes.h" +#include "quadProgpp/include/QuadProg++.hh" + +/* Kinematics only suitable for 6 DOF arm */ +class ArmDynKineModel{ +public: + ArmDynKineModel(int dof, Vec3 endPosLocal, + double endEffectorMass, + Vec3 endEffectorCom, Mat3 endEffectorInertia); + virtual ~ArmDynKineModel(); + + Vec6 forDerivativeKinematice(Vec6 q, Vec6 qd); + Vec6 invDerivativeKinematice(Vec6 q, Vec6 twistSpatial); + Vec6 invDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 endForce); + + int getDOF(){return _dof;} + std::vector getJointQMax(){return _jointQMax;} + std::vector getJointQMin(){return _jointQMin;} + std::vector getJointSpeedMax(){return _jointSpeedMax;} + + void solveQP(Vec6 twist, Vec6 pastAngle, Vec6 &result, double dt, bool setJ0QdZero, bool setQdZero); + bool invKinematics(HomoMat gDes, Vec6 pastAngle, Vec6 &result, bool checkInWorkSpace = false); + HomoMat forwardKinematics(Vec6 q, int index=6); // index from 0 to 5, used for fourth version + bool _useIKSolver = false; + +protected: + void _dofCheck(); + void _buildModel(); + + int _dof; + RigidBodyDynamics::Model *_model; + std::vector _linkID; + + std::vector _body; + std::vector _linkMass; + std::vector _linkCom; + std::vector _linkInertia; + double _endEffectorMass; + Vec3 _endEffectorCom; + Mat3 _endEffectorInertia; + + std::vector _joint; + std::vector _jointAxis; + std::vector _jointPos; + std::vector _jointQMax; + std::vector _jointQMin; + std::vector _jointSpeedMax; + Vec3 _endPosGlobal, _endPosLocal; // based on mount pos + Vec3 _endMountPosGlobal, _endMountPosLocal; + std::vector _theta; + + /* inverse derivative kinematics */ + RigidBodyDynamics::Math::MatrixNd _Jacobian; + + /* inverse dynamics */ + std::vector _endForceVec; + + /* inverse kinematics */ + bool _checkAngle(Vec6 angle); // check all angles once + Vec6 JointQMax; + Vec6 JointQMin; + Vec6 JointQdMax; + Vec6 JointQdMin; + double theta0[2], theta1[4], theta2[4], theta3[4], theta4[4], theta5[4]; + double theta2Bias; + + std::vector _linkPosLocal; + std::vector _disJoint; + double _disP13; + Vec3 _vecP45; + Vec3 _vecP14; + Vec3 _vecP13; + Vec3 _vecP34; + + Vec3 _joint4Axis; + Vec3 _joint5Axis; + + Vec3 _plane324X, _plane324Z; + double _vec34ProjX, _vec34ProjZ; + Vec3 _plane1234Y, _plane1234_X; + Vec3 _vecP04ProjXY; + + double _angle213, _angle123, _angle132; + double _angle_x13; + + HomoMat _T46; + + /* quadprog */ + quadprogpp::Matrix G, CE, CI; + quadprogpp::Vector g0, ce0, ci0, x; + + Mat6 _G; + Vec6 _g0; + VecX _ci0; + MatX _CI; +}; + +class Z1DynKineModel : public ArmDynKineModel{ +public: + Z1DynKineModel(Vec3 endPosLocal = Vec3::Zero(), + double endEffectorMass = 0.0, + Vec3 endEffectorCom = Vec3::Zero(), + Mat3 endEffectorInertia = Mat3::Zero()); + ~Z1DynKineModel(){} + +}; + + +#endif // ARMDYNCKINEMODEL_H \ No newline at end of file diff --git a/include/model/ArmReal.h b/include/model/ArmReal.h new file mode 100755 index 0000000..831ce6a --- /dev/null +++ b/include/model/ArmReal.h @@ -0,0 +1,82 @@ +#ifndef ARM_H +#define ARM_H + +#include +#include "common/math/mathTypes.h" +#include "model/Joint.h" +#include "model/Motor.h" +#include "unitree_arm_sdk/udp.h" + +class ArmReal{ +public: + ArmReal(int dof, int motorNum, IOPort *ioPort); + virtual ~ArmReal(); + void setJointKp(std::vector jointKp); + void setJointKd(std::vector jointKd); + void setJointQ(std::vector jointQ); + void setJointDq(std::vector jointDq); + void setJointTau(std::vector jointTau); + + void getJointQ(std::vector &jointQState); + void getJointDq(std::vector &jointDqState); + void getJointDDq(std::vector &jointDDqState); + void getJointTau(std::vector &jointTauState); + + void getMotorQ(std::vector &motorQ); + void getMotorDq(std::vector &motorDq); + void getMotorTau(std::vector &motorTau); + + void armCalibration(); + void armCalibration(std::vector motorAngleBias); + bool armComm(); + size_t getDof(){return _dof;} + size_t getMotorNum(){return _motorNum;} + // int getMotorNum(){return _motorNum;} + +protected: + void _init(); + void _getCmd(std::vector &cmd); + void _setState(std::vector &motorState); + void _getMotorQBias(); + // void _setComm(); + void _setCaliBias(); + + int _motorNum; + int _dof; + int _commReturn; + bool _commYN, _motorCommYN; + std::vector _joints; + std::vector _jointCaliAngle; + std::vector _motorAngleBias; + std::vector _motorCmd; + std::vector _motorState; + + /* serial or UDP Communication */ + IOPort *_ioPort; +}; + +class z1Real : public ArmReal{ +public: + z1Real(IOPort *ioPort); + ~z1Real(){} +}; + +class TigerHeadReal : public ArmReal{ +public: + TigerHeadReal(IOPort *ioPort); + ~TigerHeadReal(){} +}; + +class DogHeadReal : public ArmReal{ +public: + DogHeadReal(IOPort *ioPort); + ~DogHeadReal(){} +}; + +class TestSerial : public ArmReal{ +public: + TestSerial(IOPort *ioPort); + ~TestSerial(){} +}; + +#endif // ARM_H \ No newline at end of file diff --git a/include/model/Joint.h b/include/model/Joint.h new file mode 100755 index 0000000..9133718 --- /dev/null +++ b/include/model/Joint.h @@ -0,0 +1,55 @@ +#ifndef JOINT_H +#define JOINT_H + +#include +#include "model/Motor.h" +#include "common/enumClass.h" + +class Joint{ +public: + Joint(){} + virtual ~Joint(){} + void setJointKp (double jointKp); + void setJointKd (double jointKd); + void setJointQ (double jointQ); + void setJointDq (double jointDq); + void setJointTau(double jointTau); + + double getJointQ(); + double getJointDq(); + double getJointDDq(); + double getJointTau(); + + void getMotorQ(std::vector &motorQ); + void getMotorDq(std::vector &motorDq); + void getMotorDDq(std::vector &motorDDq); + void getMotorTau(std::vector &motorTau); + + void getCmd(std::vector &cmd); + void setState(std::vector &state); + void setCaliBias(double cali, std::vector bias); + +protected: + JointMotorType _motorType; + int _motorNum; + double _jointInitAngle; + std::vector _motor; +}; + +class Jointz1 : public Joint{ +public: + Jointz1(int motorID, double direction); + Jointz1(int motorID0, int motorID1, double direction0, double direction1); + Jointz1(int motorID, int motorMsgOrder, double direction); + Jointz1(int motorID0, int motorID1, int motorMsgOrder0, int motorMsgOrder1, double direction0, double direction1); + ~Jointz1(){} +}; + +class JointTigerHead : public Joint{ +public: + JointTigerHead(int motorID, double direction); + JointTigerHead(int motorID, int motorMsgOrder, double direction); + ~JointTigerHead(){} +}; + +#endif // JOINT_H \ No newline at end of file diff --git a/include/model/Motor.h b/include/model/Motor.h new file mode 100755 index 0000000..cc32160 --- /dev/null +++ b/include/model/Motor.h @@ -0,0 +1,56 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include +#include "common/enumClass.h" +#include "unitree_arm_sdk/common/arm_motor_common.h" + +class Motor{ +public: + Motor(int id, int motorMsgOrder, double reductionRatio, MotorMountType type, double direction); + virtual ~Motor(){} + + void setMotorKp (double jointKp); + void setMotorKd (double jointKd); + void setMotorQ (double jointQ); + void setMotorDq (double jointDq); + void setMotorTau(double jointTau); + + double getJointQ(){return _qJoint;} + double getJointDq(){return _dqJoint;} + double getJointDDq(){return _ddqJoint;} + double getJointTau(){return _tauJoint;} + + void getMotorQ(std::vector &motorQ); + void getMotorDq(std::vector &motorDq); + void getMotorDDq(std::vector &motorDDq); + void getMotorTau(std::vector &motorTau); + + void setQBias(std::vector qBias); + void getCmd(std::vector &cmd); + void setState(std::vector &state); +protected: + int _id; + int _motorMsgOrder; + double _kp, _kd, _q, _dq, _tau; + double _qJoint, _dqJoint, _ddqJoint, _tauJoint; + double _qMotor, _dqMotor, _ddqMotor, _tauMotor; + double _reductionRatio; + double _directon; + double _qBias; + MotorMountType _type; +}; + +class Motorz1 : public Motor{ +public: + Motorz1(int id, int motorMsgOrder, MotorMountType type, double direction); + ~Motorz1(){} +}; + +class MotorGo1 : public Motor{ +public: + MotorGo1(int id, int motorMsgOrder, MotorMountType type, double direction); + ~MotorGo1(){} +}; + +#endif // MOTOR_H \ No newline at end of file diff --git a/include/trajectory/CartesionSpaceTraj.h b/include/trajectory/CartesionSpaceTraj.h new file mode 100755 index 0000000..beb5bdb --- /dev/null +++ b/include/trajectory/CartesionSpaceTraj.h @@ -0,0 +1,45 @@ +#ifndef CartesionSpaceTraj_H +#define CartesionSpaceTraj_H + +#include +#include +#include +#include "control/CtrlComponents.h" + +class CartesionSpaceTraj : public Trajectory{ +public: + CartesionSpaceTraj(CtrlComponents *ctrlComp); + CartesionSpaceTraj(ArmDynKineModel *armModel); + CartesionSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState); + ~CartesionSpaceTraj(){} + + void setCartesionTraj(Vec6 startP, Vec6 endP, double oriSpeed, double posSpeed); + void setCartesionTraj(Vec6 startP, Vec6 middleP, double middleS, Vec6 endP, double oriSpeed, double posSpeed); + bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist); + + void setCartesionTrajMoveC(Vec6 startP, Vec6 middleP, Vec6 endP, double oriSpeed, double posSpeed); + bool getCartesionCmdMoveC(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist); + + double _radius; + +private: + void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3); + bool _checkPostureValid(Vec6 endPosture, int pointOrder); + void _generateA345(); + + double _pathTimeTemp; // path total time + double _s, _sDot; // [0, 1] + double _a3, _a4, _a5; // parameters of path + + int _trajOrder; // The order of trajectory curve + std::vector _curveParam; + + /* MoveC instruct */ + Vec3 _center; + double _theta; + Vec3 _axis_x; + Vec3 _axis_y; + Vec3 _axis_z; +}; + +#endif // JOINTSPACETRAJ_H \ No newline at end of file diff --git a/include/trajectory/EndCircleTraj.h b/include/trajectory/EndCircleTraj.h new file mode 100755 index 0000000..9306b99 --- /dev/null +++ b/include/trajectory/EndCircleTraj.h @@ -0,0 +1,32 @@ +#ifndef ENDCIRCLETRAJ_H +#define ENDCIRCLETRAJ_H + +#include "model/ArmDynKineModel.h" +#include "trajectory/EndHomoTraj.h" + +class EndCircleTraj: public EndHomoTraj{ +public: + EndCircleTraj(CtrlComponents *ctrlComp); + EndCircleTraj(ArmDynKineModel *armModel); + ~EndCircleTraj(){} + + void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit, + Vec3 axisDirection, double maxSpeed, double angle, + bool keepOrientation = true); + void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit, + Vec3 axisDirection, double maxSpeed, double angle, + bool keepOrientation = true); +private: + bool _getEndTraj(HomoMat &homo, Vec6 &twist); + Vec3 _center; + double _radius; + HomoMat _centerHomo; + HomoMat _initHomoToCenter; + RotMat _initOri; + double _maxSpeed, _goalAngle, _speed, _angle; + Vec3 _omegaAxis; + + bool _keepOrientation; +}; + +#endif // ENDCIRCLETRAJ_H \ No newline at end of file diff --git a/include/trajectory/EndHomoTraj.h b/include/trajectory/EndHomoTraj.h new file mode 100755 index 0000000..138324a --- /dev/null +++ b/include/trajectory/EndHomoTraj.h @@ -0,0 +1,28 @@ +#ifndef ENDHOMOTRAJ_H +#define ENDHOMOTRAJ_H + +#include "model/ArmDynKineModel.h" +#include "trajectory/Trajectory.h" +#include "trajectory/SCurve.h" + +class EndHomoTraj : public Trajectory{ +public: + EndHomoTraj(CtrlComponents *ctrlComp); + EndHomoTraj(ArmDynKineModel *armModel); + virtual ~EndHomoTraj(); + bool getJointCmd(Vec6 &q, Vec6 &qd); + bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); + +protected: + virtual bool _getEndTraj(HomoMat &homo, Vec6 &twist) = 0; + + HomoMat _cmdHomo; + Vec6 _cmdTwist; + Vec6 _qPast; + // double _currentTime; + + SCurve *_sCurve; + +}; + +#endif // ENDHOMOTRAJ_H diff --git a/include/trajectory/EndLineTraj.h b/include/trajectory/EndLineTraj.h new file mode 100755 index 0000000..4fed4da --- /dev/null +++ b/include/trajectory/EndLineTraj.h @@ -0,0 +1,30 @@ +#ifndef ENDLINETRAJ_H +#define ENDLINETRAJ_H + +#include "trajectory/EndHomoTraj.h" +#include "trajectory/SCurve.h" + +class EndLineTraj : public EndHomoTraj{ +public: + EndLineTraj(CtrlComponents *ctrlComp); + EndLineTraj(ArmDynKineModel *armModel); + ~EndLineTraj(){} + + void setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed); + void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed); + +private: + bool _getEndTraj(HomoMat &homo, Vec6 &twist); + + // SCurve _sCurves[2]; // 0: position, 1: orientation + Vec3 _movingAxis, _turningAxis; + double _movingDistance, _turningAngle; + Vec3 _currentDistance, _currentAngle; + Vec3 _currentVelocity, _currentOmega; + + + SCurve _posCurve; + SCurve _oriCurve; +}; + +#endif // ENDLINETRAJ_H \ No newline at end of file diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h new file mode 100755 index 0000000..bbb2d4b --- /dev/null +++ b/include/trajectory/JointSpaceTraj.h @@ -0,0 +1,45 @@ +#ifndef JOINTSPACETRAJ_H +#define JOINTSPACETRAJ_H + +#include +#include +#include +#include "control/CtrlComponents.h" + +class JointSpaceTraj : public Trajectory{ +public: + JointSpaceTraj(CtrlComponents *ctrlComp); + JointSpaceTraj(ArmDynKineModel *armModel); + JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState); + ~JointSpaceTraj(){} + + bool getJointCmd(Vec6 &q, Vec6 &qd); + bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); + void setJointTraj(Vec6 startQ, Vec6 endQ, double speed); + void setJointTraj(Vec6 startQ, std::string endName, double speed); + void setJointTraj(std::string startName, std::string endName, double speed); + + void setJointTraj(Vec6 startQ, Vec6 middleQ, double middleS, Vec6 endQ, double speed); + void setJointTraj(std::string startName, std::string middleName, double middleS, std::string endName, double speed); + void setJointTraj(std::vector stateQ, std::vector stateS, double pathTime); + void setJointTraj(std::vector stateName, std::vector stateS, double pathTime); + +private: + void _checkAngleValid(const Vec6 &q, int pointOrder); + bool _checkJointAngleValid(const double &q, int jointOrder); + void _generateA345(); + + int _jointNum; + double _pathTimeTemp; // path total time + double _s, _sDot; // [0, 1] + double _a3, _a4, _a5; // parameters of path + // double _speedFactor; // [0, 1] + std::vector _jointMaxQ; + std::vector _jointMinQ; + std::vector _jointMaxSpeed; + + int _trajOrder; // The order of trajectory curve + std::vector _curveParam; +}; + +#endif // JOINTSPACETRAJ_H \ No newline at end of file diff --git a/include/trajectory/SCurve.h b/include/trajectory/SCurve.h new file mode 100755 index 0000000..569adc4 --- /dev/null +++ b/include/trajectory/SCurve.h @@ -0,0 +1,50 @@ +#ifndef SCURVE_H +#define SCURVE_H + +/* 归一化后的s曲线 */ + +#include +#include + +class SCurve{ +public: + SCurve(){} + ~SCurve(){} + void setSCurve(double deltaQ, double dQMax, double ddQMax, double dddQMax); + void restart(); +/* +考虑到归一化，在实际使用时，物理量为： +加速度 = deltaQ * getDDs(); +*/ + double getDDs(); + double getDDs(double t); +/* +考虑到归一化，在实际使用时，物理量为： +速度 = deltaQ * getDs(); +*/ + double getDs(); + double getDs(double t); +/* +考虑到归一化，在实际使用时，物理量为： +位置 = deltaQ * gets(); +*/ + double gets(); + double gets(double t); + + double getT(); +private: + int _getSegment(double t); + void _setFunc(); + double _runTime(); + + bool _started = false; + double _startTime; + + double _J, _aMax, _vMax; + double _T[7]; // period + double _t[7]; // moment + double _v0, _v1; // ds at _t[0], _t[1] + double _s0, _s1, _s2, _s3, _s4, _s5, _s6; // s at _t[0], _t[1] +}; + +#endif // SCURVE_H \ No newline at end of file diff --git a/include/trajectory/StopForTime.h b/include/trajectory/StopForTime.h new file mode 100755 index 0000000..889c529 --- /dev/null +++ b/include/trajectory/StopForTime.h @@ -0,0 +1,24 @@ +#ifndef STOPFORTIME_H +#define STOPFORTIME_H + +#include "trajectory/Trajectory.h" + +class StopForTime : public Trajectory{ +public: + StopForTime(CtrlComponents *ctrlComp); + StopForTime(ArmDynKineModel *armModel); + StopForTime(ArmDynKineModel *armModel, CSVTool *csvState); + + ~StopForTime(){} + + bool getJointCmd(Vec6 &q, Vec6 &qd); + bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); + + void setStop(Vec6 stopQ, double stopTime); + void setStop(std::string stateName, double stopTime); +private: + // Vec6 _stopQ; + // double _stopTime; +}; + +#endif // STOPFORTIME_H \ No newline at end of file diff --git a/include/trajectory/Trajectory.h b/include/trajectory/Trajectory.h new file mode 100755 index 0000000..fa82fe2 --- /dev/null +++ b/include/trajectory/Trajectory.h @@ -0,0 +1,100 @@ +#ifndef TRAJECTORY_H +#define TRAJECTORY_H + +#include "common/math/mathTypes.h" +#include "control/CtrlComponents.h" +#include "unitree_arm_sdk/timer.h" + +class Trajectory{ +public: + Trajectory(CtrlComponents *ctrlComp){ + _ctrlComp = ctrlComp; + _armModel = ctrlComp->armModel; + _csvState = ctrlComp->stateCSV; + _dof = _armModel->getDOF(); + _hasKinematic = true; + } + Trajectory(ArmDynKineModel *armModel){ + _ctrlComp = nullptr; + _armModel = armModel; + _csvState = nullptr; + _dof = armModel->getDOF(); + if(_dof == 6){ + _hasKinematic = true; + } + } + Trajectory(ArmDynKineModel *armModel, CSVTool *csvState):Trajectory(armModel){ + _csvState = csvState; + } + virtual ~Trajectory(){} + virtual bool getJointCmd(Vec6 &q, Vec6 &qd){}; + virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){}; + virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){}; + + void restart(){ + _pathStarted = false; + _reached = false; + } + void setGripper(double startQ, double endQ){ + _gripperStartQ = startQ; + _gripperEndQ = endQ; + } + bool correctYN(){return _settingCorrect;} + Vec6 getStartQ(){return _startQ;} + Vec6 getEndQ(){return _endQ;} + HomoMat getStartHomo(){ + if(_hasKinematic){ + return _startHomo; + }else{ + std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl; + exit(-1); + } + } + HomoMat getEndHomo(){ + if(_hasKinematic){ + return _endHomo; + }else{ + std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl; + exit(-1); + } + } + + double getPathTime(){return _pathTime;} +protected: + CtrlComponents *_ctrlComp; + ArmDynKineModel *_armModel; + CSVTool *_csvState; + bool _pathStarted = false; + bool _reached = false; + bool _paused = false; + bool _settingCorrect = true; + double _startTime; + double _currentTime; + double _pathTime; + double _tCost; + Vec6 _startQ, _endQ; + HomoMat _startHomo, _endHomo; + + double _gripperStartQ; + double _gripperEndQ; + + size_t _dof; + bool _hasKinematic; + + void _runTime(){ + _currentTime = getTimeSecond(); + + if(!_pathStarted){ + _pathStarted = true; + _startTime = _currentTime; + _tCost = 0; + } + + _tCost = _currentTime - _startTime; + + _reached = (_tCost>_pathTime) ? true : false; + _tCost = (_tCost>_pathTime) ? _pathTime : _tCost; + } +}; + +#endif // TRAJECTORY_H \ No newline at end of file diff --git a/include/trajectory/TrajectoryManager.h b/include/trajectory/TrajectoryManager.h new file mode 100755 index 0000000..21af52a --- /dev/null +++ b/include/trajectory/TrajectoryManager.h @@ -0,0 +1,37 @@ +#ifndef TRAJECTORY_MANAGER_H +#define TRAJECTORY_MANAGER_H + +#include +#include "control/CtrlComponents.h" +#include "trajectory/JointSpaceTraj.h" +#include "trajectory/EndLineTraj.h" +#include "trajectory/EndCircleTraj.h" +#include "trajectory/StopForTime.h" + +class TrajectoryManager{ +public: + TrajectoryManager(CtrlComponents *ctrlComp); + TrajectoryManager(ArmDynKineModel *armModel); + TrajectoryManager(ArmDynKineModel *armModel, CSVTool *csvState); + ~TrajectoryManager(){} + bool getJointCmd(Vec6 &q, Vec6 &qd); + bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); + void addTrajectory(Trajectory* traj); + void setLoop(double backSpeed = 0.7); + void restartTraj(); + void emptyTraj(); + Vec6 getStartQ(); + Vec6 getEndQ(); + HomoMat getEndHomo(); + // bool checkTrajectoryContinuous(); +private: + CtrlComponents *_ctrlComp; + JointSpaceTraj *_trajBack; + std::vector _trajVec; + int _trajID = 0; + double _jointErr = 0.05; + bool _trajCorrect = true; + bool _loop = false; +}; + +#endif // TRAJECTORY_MANAGER_H \ No newline at end of file diff --git a/lib/libUnitree_motor_SDK_Linux64.so b/lib/libUnitree_motor_SDK_Linux64.so new file mode 100755 index 0000000..d118bc7 Binary files /dev/null and b/lib/libUnitree_motor_SDK_Linux64.so differ diff --git a/lib/libUnitree_motor_SDK_Linux64_EPOLL.so b/lib/libUnitree_motor_SDK_Linux64_EPOLL.so new file mode 100755 index 0000000..672e404 Binary files /dev/null and b/lib/libUnitree_motor_SDK_Linux64_EPOLL.so differ diff --git a/lib/libZ1_JOYSTICK_ROS_Linux64.so b/lib/libZ1_JOYSTICK_ROS_Linux64.so new file mode 100755 index 0000000..1f32457 Binary files /dev/null and b/lib/libZ1_JOYSTICK_ROS_Linux64.so differ diff --git a/lib/libZ1_JOYSTICK_UDP_Linux64.so b/lib/libZ1_JOYSTICK_UDP_Linux64.so new file mode 100755 index 0000000..fce861d Binary files /dev/null and b/lib/libZ1_JOYSTICK_UDP_Linux64.so differ diff --git a/lib/libZ1_KEYBOARD_ROS_Linux64.so b/lib/libZ1_KEYBOARD_ROS_Linux64.so new file mode 100755 index 0000000..d32f940 Binary files /dev/null and b/lib/libZ1_KEYBOARD_ROS_Linux64.so differ diff --git a/lib/libZ1_KEYBOARD_UDP_Linux64.so b/lib/libZ1_KEYBOARD_UDP_Linux64.so new file mode 100755 index 0000000..7c980e5 Binary files /dev/null and b/lib/libZ1_KEYBOARD_UDP_Linux64.so differ diff --git a/lib/libZ1_SDK_ROS_Linux64.so b/lib/libZ1_SDK_ROS_Linux64.so new file mode 100755 index 0000000..f3b7dc5 Binary files /dev/null and b/lib/libZ1_SDK_ROS_Linux64.so differ diff --git a/lib/libZ1_SDK_UDP_Linux64.so b/lib/libZ1_SDK_UDP_Linux64.so new file mode 100755 index 0000000..fac7e1f Binary files /dev/null and b/lib/libZ1_SDK_UDP_Linux64.so differ diff --git a/main.cpp b/main.cpp new file mode 100755 index 0000000..14df459 --- /dev/null +++ b/main.cpp @@ -0,0 +1,249 @@ +#include +#include +#include +#include +#include +#include "control/CtrlComponents.h" +#include "model/ArmReal.h" +#include "model/ArmDynKineModel.h" +#include "interface/IOUDP.h" + +#include "FSM/State_BackToStart.h" +#include "FSM/State_Cartesian.h" +#include "FSM/State_MoveJ.h" +#include "FSM/State_MoveL.h" +#include "FSM/State_MoveC.h" +#include "FSM/State_Dance.h" +#include "FSM/State_JointSpace.h" +#include "FSM/State_Passive.h" +#include "FSM/State_SaveState.h" +#include "FSM/State_Teach.h" +#include "FSM/State_TeachRepeat.h" +#include "FSM/State_ToState.h" +#include "FSM/State_Trajectory.h" +#include "FSM/State_Calibration.h" +#include "FSM/State_LowCmd.h" +#include "FSM/FiniteStateMachine.h" + +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +bool running = true; + +// over watch the ctrl+c command +void ShutDown(int sig){ + std::cout << "[STATE] stop the controller" << std::endl; + running = false; +} + +void setProcessScheduler(){ + pid_t pid = getpid(); + sched_param param; + param.sched_priority = sched_get_priority_max(SCHED_FIFO); + if (sched_setscheduler(pid, SCHED_FIFO, ¶m) == -1) { + std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl; + } +} + +int main(int argc, char **argv){ + /* set real-time process */ + setProcessScheduler(); + /* set the print format */ + std::cout << std::fixed << std::setprecision(3); + + EmptyAction emptyAction((int)ArmFSMStateName::INVALID); + std::vector events; + CmdPanel *cmdPanel; +#ifdef CTRL_BY_SDK + events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); + events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD)); + + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction); +#endif +#ifdef CTRL_BY_KEYBOARD + events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT)); + + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + cmdPanel = new Keyboard(events, emptyAction); +#endif +#ifdef CTRL_BY_JOYSTICK + events.push_back(new StateAction("r12_passive", (int)ArmFSMStateName::PASSIVE)); + events.push_back(new StateAction("r2_backToState", (int)ArmFSMStateName::BACKTOSTART)); + events.push_back(new StateAction("r1_cartesian", (int)ArmFSMStateName::CARTESIAN)); + events.push_back(new StateAction("select", (int)ArmFSMStateName::NEXT)); + + events.push_back(new ValueAction("ly_forTranY", "ly_invTranY", 1.0)); + events.push_back(new ValueAction("lx_forTranX", "lx_invTranX", 1.0)); + events.push_back(new ValueAction("up_forTranZ", "down_invTranZ", 1.0)); + events.push_back(new ValueAction("ry_forRotY", "ry_invRotY", 1.0)); + events.push_back(new ValueAction("rx_forRotX", "rx_invRotX", 1.0)); + events.push_back(new ValueAction("Y_forRotZ", "A_invRotZ", 1.0)); + events.push_back(new ValueAction("right_gClose", "left_gOpen", 1.0)); + // events.push_back(new ValueAction("X_velDown", "B_velUp", 1.0)); + + cmdPanel = new UnitreeJoystick(events, emptyAction); +#endif + +#ifdef RUN_ROS + ros::init(argc, argv, "z1_controller"); +#endif // RUN_ROS + + CtrlComponents *ctrlComp = new CtrlComponents(); + ctrlComp->dof = 6; + ctrlComp->armConfigPath = "../config/"; + ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv"); + ctrlComp->running = &running; + +#ifdef UDP + ctrlComp->ioInter = new IOUDP(cmdPanel); + ctrlComp->dt = 0.004; +#endif +#ifdef ROS + ctrlComp->ioInter = new IOROS(cmdPanel); + ctrlComp->dt = 0.004; +#endif + +#ifdef COMPILE_DEBUG +std::cout << "add plot" << std::endl; + + ctrlComp->plot = new PyPlot(); + /* for general debugging */ + // ctrlComp->plot->addPlot("qCmd", 6); + // ctrlComp->plot->addPlot("qdCmd", 6); + // ctrlComp->plot->addPlot("tauCmd", 6); + + // ctrlComp->plot->addPlot("qReal", 6); + // ctrlComp->plot->addPlot("qdReal", 6); + // ctrlComp->plot->addPlot("realTau", 6); + + // ctrlComp->plot->addPlot("errorTau", 6); + + // ctrlComp->plot->addPlot("error", 1); + // ctrlComp->plot->addPlot("costTime", 1); + + /* for MoveJ, MoveL, MoveC*/ + // ctrlComp->plot->addPlot("moveLPostureDes", 6); + // ctrlComp->plot->addPlot("moveLTwistDes", 6); + // ctrlComp->plot->addPlot("moveLPostureAct", 6); + // ctrlComp->plot->addPlot("moveLTwistAct", 6); + // ctrlComp->plot->addPlot("moveLPostureError", 6); + // ctrlComp->plot->addPlot("moveLTwistError", 6); + + // /* for cartesian */ + // ctrlComp->plot->addPlot("cartesianPostureDes", 6); + // ctrlComp->plot->addPlot("cartesianTwistDes", 6); + // ctrlComp->plot->addPlot("cartesianPostureAct", 6); + // ctrlComp->plot->addPlot("cartesianTwistAct", 6); + // ctrlComp->plot->addPlot("cartesianPostureError", 6); + // ctrlComp->plot->addPlot("cartesianTwistError", 6); + + /* for trajectory */ + // plot->addPlot("s", 1); + // plot->addPlot("qPath", 6); + // plot->addPlot("qdPath", 6); + // plot->addPlot("qdPathDiff", 6); + // ctrlComp->plot->addPlot("qState", 6); + // ctrlComp->plot->addPlot("qError", 6); + + /* for gripper */ + // ctrlComp->plot->addPlot("gripper q error", 1); + // ctrlComp->plot->addPlot("gripper qd error", 1); + // ctrlComp->plot->addPlot("gripper tau", 1); + +#endif // COMPILE_DEBUG + ctrlComp->geneObj(); + + std::vector states; + states.push_back(new State_Passive(ctrlComp)); // First state in states is the begining state for FSM + states.push_back(new State_BackToStart(ctrlComp)); + states.push_back(new State_JointSpace(ctrlComp)); + states.push_back(new State_Cartesian(ctrlComp)); + states.push_back(new State_MoveJ(ctrlComp)); + states.push_back(new State_MoveL(ctrlComp)); + states.push_back(new State_MoveC(ctrlComp)); + states.push_back(new State_LowCmd(ctrlComp)); + states.push_back(new State_SaveState(ctrlComp)); + states.push_back(new State_Teach(ctrlComp)); + states.push_back(new State_TeachRepeat(ctrlComp)); + states.push_back(new State_ToState(ctrlComp)); + states.push_back(new State_Trajectory(ctrlComp)); + states.push_back(new State_Calibration(ctrlComp)); + + std::vector _trajMag; + std::vector _jointTraj; + + /* start to forward */ + _trajMag.push_back(new TrajectoryManager(ctrlComp)); + _jointTraj.push_back(new JointSpaceTraj(ctrlComp)); + _jointTraj.at(_jointTraj.size()-1)->setJointTraj("startFlat", "forward", 0.5); + _trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1)); + + /* forward to start */ + _trajMag.push_back(new TrajectoryManager(ctrlComp)); + _jointTraj.push_back(new JointSpaceTraj(ctrlComp)); + _jointTraj.at(_jointTraj.size()-1)->setJointTraj("forward", "startFlat", 0.5); + _trajMag.at(_trajMag.size()-1)->addTrajectory(_jointTraj.at(_jointTraj.size()-1)); + + states.push_back(new State_Dance(ctrlComp, _trajMag.at(0), ArmFSMStateName::DANCE00, ArmFSMStateName::DANCE01, "dance 00")); + states.push_back(new State_Dance(ctrlComp, _trajMag.at(1), ArmFSMStateName::DANCE01, ArmFSMStateName::TRAJECTORY, "dance 01")); +// #endif + + FiniteStateMachine fsm(states, cmdPanel, 0, ctrlComp->dt); + + signal(SIGINT, ShutDown); + + // std::vector values; + while(running){ + // values = cmdPanel->getValues(); + // std::cout << "value: " << values.at(0) << std::endl; + usleep(100000); + } + +#ifdef COMPILE_DEBUG +std::cout << "show plot" << std::endl; + ctrlComp->plot->showPlotAll(); +#endif // COMPILE_DEBUG + delete ctrlComp; + + return 0; +} diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..7d654fb --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + z1_controller + 0.0.0 + The z1_controller package + + unitree + + TODO + + catkin + roscp + rospy + urdf + xacro + roscp + rospy + urdf + xacro + roscp + rospy + urdf + xacro + + diff --git a/thirdparty/eigen-3.3.9.zip b/thirdparty/eigen-3.3.9.zip new file mode 100644 index 0000000..ecdd007 Binary files /dev/null and b/thirdparty/eigen-3.3.9.zip differ diff --git a/thirdparty/quadProgpp/include/Array.hh b/thirdparty/quadProgpp/include/Array.hh new file mode 100755 index 0000000..7d7244b --- /dev/null +++ b/thirdparty/quadProgpp/include/Array.hh @@ -0,0 +1,2532 @@ +// $Id: Array.hh 249 2008-11-20 09:58:23Z schaerf $ +// This file is part of EasyLocalpp: a C++ Object-Oriented framework +// aimed at easing the development of Local Search algorithms. +// Copyright (C) 2001--2008 Andrea Schaerf, Luca Di Gaspero. +// +// This software may be modified and distributed under the terms +// of the MIT license. See the LICENSE file for details. + +#if !defined(_ARRAY_HH) +#define _ARRAY_HH + +#include +#include +#include +#include +#include +#include + +namespace quadprogpp { + +enum MType { DIAG }; + +template +class Vector +{ +public: + Vector(); + Vector(const unsigned int n); + Vector(const T& a, const unsigned int n); //initialize to constant value + Vector(const T* a, const unsigned int n); // Initialize to array + Vector(const Vector &rhs); // copy constructor + ~Vector(); // destructor + + inline void set(const T* a, const unsigned int n); + Vector extract(const std::set& indexes) const; + inline T& operator[](const unsigned int& i); //i-th element + inline const T& operator[](const unsigned int& i) const; + + inline unsigned int size() const; + inline void resize(const unsigned int n); + inline void resize(const T& a, const unsigned int n); + + Vector& operator=(const Vector& rhs); //assignment + Vector& operator=(const T& a); //assign a to every element + inline Vector& operator+=(const Vector& rhs); + inline Vector& operator-=(const Vector& rhs); + inline Vector& operator*=(const Vector& rhs); + inline Vector& operator/=(const Vector& rhs); + inline Vector& operator^=(const Vector& rhs); + inline Vector& operator+=(const T& a); + inline Vector& operator-=(const T& a); + inline Vector& operator*=(const T& a); + inline Vector& operator/=(const T& a); + inline Vector& operator^=(const T& a); +private: + unsigned int n; // size of array. upper index is n-1 + T* v; // storage for data +}; + +template +Vector::Vector() + : n(0), v(0) +{} + +template +Vector::Vector(const unsigned int n) + : v(new T[n]) +{ + this->n = n; +} + +template +Vector::Vector(const T& a, const unsigned int n) + : v(new T[n]) +{ + this->n = n; + for (unsigned int i = 0; i < n; i++) + v[i] = a; +} + +template +Vector::Vector(const T* a, const unsigned int n) + : v(new T[n]) +{ + this->n = n; + for (unsigned int i = 0; i < n; i++) + v[i] = *a++; +} + +template +Vector::Vector(const Vector& rhs) + : v(new T[rhs.n]) +{ + this->n = rhs.n; + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; +} + +template +Vector::~Vector() +{ + if (v != 0) + delete[] (v); +} + +template +void Vector::resize(const unsigned int n) +{ + if (n == this->n) + return; + if (v != 0) + delete[] (v); + v = new T[n]; + this->n = n; +} + +template +void Vector::resize(const T& a, const unsigned int n) +{ + resize(n); + for (unsigned int i = 0; i < n; i++) + v[i] = a; +} + + +template +inline Vector& Vector::operator=(const Vector& rhs) +// postcondition: normal assignment via copying has been performed; +// if vector and rhs were different sizes, vector +// has been resized to match the size of rhs +{ + if (this != &rhs) + { + resize(rhs.n); + for (unsigned int i = 0; i < n; i++) + v[i] = rhs[i]; + } + return *this; +} + +template +inline Vector & Vector::operator=(const T& a) //assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + v[i] = a; + return *this; +} + +template +inline T & Vector::operator[](const unsigned int& i) //subscripting +{ + return v[i]; +} + +template +inline const T& Vector::operator[](const unsigned int& i) const //subscripting +{ + return v[i]; +} + +template +inline unsigned int Vector::size() const +{ + return n; +} + +template +inline void Vector::set(const T* a, unsigned int n) +{ + resize(n); + for (unsigned int i = 0; i < n; i++) + v[i] = a[i]; +} + +template +inline Vector Vector::extract(const std::set& indexes) const +{ + Vector tmp(indexes.size()); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + if (*el >= n) + throw std::logic_error("Error extracting subvector: the indexes are out of vector bounds"); + tmp[i++] = v[*el]; + } + + return tmp; +} + +template +inline Vector& Vector::operator+=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator+=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] += rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator+=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] += a; + + return *this; +} + +template +inline Vector operator+(const Vector& rhs) +{ + return rhs; +} + +template +inline Vector operator+(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator+: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] + rhs[i]; + + return tmp; +} + +template +inline Vector operator+(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] + a; + + return tmp; +} + +template +inline Vector operator+(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a + rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator-=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator-=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] -= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator-=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] -= a; + + return *this; +} + +template +inline Vector operator-(const Vector& rhs) +{ + return (T)(-1) * rhs; +} + +template +inline Vector operator-(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator-: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] - rhs[i]; + + return tmp; +} + +template +inline Vector operator-(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] - a; + + return tmp; +} + +template +inline Vector operator-(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a - rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator*=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator*=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] *= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator*=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] *= a; + + return *this; +} + +template +inline Vector operator*(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator*: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] * rhs[i]; + + return tmp; +} + +template +inline Vector operator*(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] * a; + + return tmp; +} + +template +inline Vector operator*(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a * rhs[i]; + + return tmp; +} + +template +inline Vector& Vector::operator/=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator/=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] /= rhs[i]; + + return *this; +} + + +template +inline Vector& Vector::operator/=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] /= a; + + return *this; +} + +template +inline Vector operator/(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator/: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] / rhs[i]; + + return tmp; +} + +template +inline Vector operator/(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = lhs[i] / a; + + return tmp; +} + +template +inline Vector operator/(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = a / rhs[i]; + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const Vector& rhs) +{ + if (lhs.size() != rhs.size()) + throw std::logic_error("Operator^: vectors have different sizes"); + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = pow(lhs[i], rhs[i]); + + return tmp; +} + +template +inline Vector operator^(const Vector& lhs, const T& a) +{ + Vector tmp(lhs.size()); + for (unsigned int i = 0; i < lhs.size(); i++) + tmp[i] = pow(lhs[i], a); + + return tmp; +} + +template +inline Vector operator^(const T& a, const Vector& rhs) +{ + Vector tmp(rhs.size()); + for (unsigned int i = 0; i < rhs.size(); i++) + tmp[i] = pow(a, rhs[i]); + + return tmp; +} + +template +inline Vector& Vector::operator^=(const Vector& rhs) +{ + if (this->size() != rhs.size()) + throw std::logic_error("Operator^=: vectors have different sizes"); + for (unsigned int i = 0; i < n; i++) + v[i] = pow(v[i], rhs[i]); + + return *this; +} + +template +inline Vector& Vector::operator^=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + v[i] = pow(v[i], a); + + return *this; +} + +template +inline bool operator==(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) + return false; + return true; +} + +template +inline bool operator!=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] != w[i]) + return true; + return false; +} + +template +inline bool operator<(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] >= w[i]) + return false; + return true; +} + +template +inline bool operator<=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] > w[i]) + return false; + return true; +} + +template +inline bool operator>(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] <= w[i]) + return false; + return true; +} + +template +inline bool operator>=(const Vector& v, const Vector& w) +{ + if (v.size() != w.size()) + throw std::logic_error("Vectors of different size are not confrontable"); + for (unsigned i = 0; i < v.size(); i++) + if (v[i] < w[i]) + return false; + return true; +} + +/** + Input/Output +*/ +template +inline std::ostream& operator<<(std::ostream& os, const Vector& v) +{ + os << std::endl << v.size() << std::endl; + for (unsigned int i = 0; i < v.size() - 1; i++) + os << std::setw(20) << std::setprecision(16) << v[i] << ", "; + os << std::setw(20) << std::setprecision(16) << v[v.size() - 1] << std::endl; + + return os; +} + +template +std::istream& operator>>(std::istream& is, Vector& v) +{ + int elements; + char comma; + is >> elements; + v.resize(elements); + for (unsigned int i = 0; i < elements; i++) + is >> v[i] >> comma; + + return is; +} + +/** + Index utilities +*/ + +std::set seq(unsigned int s, unsigned int e); + +std::set singleton(unsigned int i); + +template +class CanonicalBaseVector : public Vector +{ +public: + CanonicalBaseVector(unsigned int i, unsigned int n); + inline void reset(unsigned int i); +private: + unsigned int e; +}; + +template +CanonicalBaseVector::CanonicalBaseVector(unsigned int i, unsigned int n) + : Vector((T)0, n), e(i) +{ (*this)[e] = (T)1; } + +template +inline void CanonicalBaseVector::reset(unsigned int i) +{ + (*this)[e] = (T)0; + e = i; + (*this)[e] = (T)1; +} + +#include + +template +inline T sum(const Vector& v) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < v.size(); i++) + tmp += v[i]; + + return tmp; +} + +template +inline T prod(const Vector& v) +{ + T tmp = (T)1; + for (unsigned int i = 0; i < v.size(); i++) + tmp *= v[i]; + + return tmp; +} + +template +inline T mean(const Vector& v) +{ + T sum = (T)0; + for (unsigned int i = 0; i < v.size(); i++) + sum += v[i]; + return sum / v.size(); +} + +template +inline T median(const Vector& v) +{ + Vector tmp = sort(v); + if (v.size() % 2 == 1) // it is an odd-sized vector + return tmp[v.size() / 2]; + else + return 0.5 * (tmp[v.size() / 2 - 1] + tmp[v.size() / 2]); +} + +template +inline T stdev(const Vector& v, bool sample_correction = false) +{ + return sqrt(var(v, sample_correction)); +} + +template +inline T var(const Vector& v, bool sample_correction = false) +{ + T sum = (T)0, ssum = (T)0; + unsigned int n = v.size(); + for (unsigned int i = 0; i < n; i++) + { + sum += v[i]; + ssum += (v[i] * v[i]); + } + if (!sample_correction) + return (ssum / n) - (sum / n) * (sum / n); + else + return n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); +} + +template +inline T max(const Vector& v) +{ + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) + value = std::max(v[i], value); + + return value; +} + +template +inline T min(const Vector& v) +{ + T value = v[0]; + for (unsigned int i = 1; i < v.size(); i++) + value = std::min(v[i], value); + + return value; +} + +template +inline unsigned int index_max(const Vector& v) +{ + unsigned int max = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] > v[max]) + max = i; + + return max; +} + +template +inline unsigned int index_min(const Vector& v) +{ + unsigned int min = 0; + for (unsigned int i = 1; i < v.size(); i++) + if (v[i] < v[min]) + min = i; + + return min; +} + + +template +inline T dot_prod(const Vector& a, const Vector& b) +{ + T sum = (T)0; + if (a.size() != b.size()) + throw std::logic_error("Dotprod error: the vectors are not the same size"); + for (unsigned int i = 0; i < a.size(); i++) + sum += a[i] * b[i]; + + return sum; +} + +/** + Single element mathematical functions +*/ + +template +inline Vector exp(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = exp(v[i]); + + return tmp; +} + +template +inline Vector log(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = log(v[i]); + + return tmp; +} + +template +inline Vector vec_sqrt(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = sqrt(v[i]); + + return tmp; +} + +template +inline Vector pow(const Vector& v, double a) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = pow(v[i], a); + + return tmp; +} + +template +inline Vector abs(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = (T)fabs(v[i]); + + return tmp; +} + +template +inline Vector sign(const Vector& v) +{ + Vector tmp(v.size()); + for (unsigned int i = 0; i < v.size(); i++) + tmp[i] = v[i] > 0 ? +1 : v[i] == 0 ? 0 : -1; + + return tmp; +} + +template +inline unsigned int partition(Vector& v, unsigned int begin, unsigned int end) +{ + unsigned int i = begin + 1, j = begin + 1; + T pivot = v[begin]; + while (j <= end) + { + if (v[j] < pivot) { + std::swap(v[i], v[j]); + i++; + } + j++; + } + v[begin] = v[i - 1]; + v[i - 1] = pivot; + return i - 2; +} + + +template +inline void quicksort(Vector& v, unsigned int begin, unsigned int end) +{ + if (end > begin) + { + unsigned int index = partition(v, begin, end); + quicksort(v, begin, index); + quicksort(v, index + 2, end); + } +} + +template +inline Vector sort(const Vector& v) +{ + Vector tmp(v); + + quicksort(tmp, 0, tmp.size() - 1); + + return tmp; +} + +template +inline Vector rank(const Vector& v) +{ + Vector tmp(v); + Vector tmp_rank(0.0, v.size()); + + for (unsigned int i = 0; i < tmp.size(); i++) + { + unsigned int smaller = 0, equal = 0; + for (unsigned int j = 0; j < tmp.size(); j++) + if (i == j) + continue; + else + if (tmp[j] < tmp[i]) + smaller++; + else if (tmp[j] == tmp[i]) + equal++; + tmp_rank[i] = smaller + 1; + if (equal > 0) + { + for (unsigned int j = 1; j <= equal; j++) + tmp_rank[i] += smaller + 1 + j; + tmp_rank[i] /= (double)(equal + 1); + } + } + + return tmp_rank; +} + +//enum MType { DIAG }; + +template +class Matrix +{ +public: + Matrix(); // Default constructor + Matrix(const unsigned int n, const unsigned int m); // Construct a n x m matrix + Matrix(const T& a, const unsigned int n, const unsigned int m); // Initialize the content to constant a + Matrix(MType t, const T& a, const T& o, const unsigned int n, const unsigned int m); + Matrix(MType t, const Vector& v, const T& o, const unsigned int n, const unsigned int m); + Matrix(const T* a, const unsigned int n, const unsigned int m); // Initialize to array + Matrix(const Matrix& rhs); // Copy constructor + ~Matrix(); // destructor + + inline T* operator[](const unsigned int& i) { return v[i]; } // Subscripting: row i + inline const T* operator[](const unsigned int& i) const { return v[i]; }; // const subsctipting + + inline void resize(const unsigned int n, const unsigned int m); + inline void resize(const T& a, const unsigned int n, const unsigned int m); + + + inline Vector extractRow(const unsigned int i) const; + inline Vector extractColumn(const unsigned int j) const; + inline Vector extractDiag() const; + inline Matrix extractRows(const std::set& indexes) const; + inline Matrix extractColumns(const std::set& indexes) const; + inline Matrix extract(const std::set& r_indexes, const std::set& c_indexes) const; + + inline void set(const T* a, unsigned int n, unsigned int m); + inline void set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& m); + inline void setRow(const unsigned int index, const Vector& v); + inline void setRow(const unsigned int index, const Matrix& v); + inline void setRows(const std::set& indexes, const Matrix& m); + inline void setColumn(const unsigned int index, const Vector& v); + inline void setColumn(const unsigned int index, const Matrix& v); + inline void setColumns(const std::set& indexes, const Matrix& m); + + + inline unsigned int nrows() const { return n; } // number of rows + inline unsigned int ncols() const { return m; } // number of columns + + inline Matrix& operator=(const Matrix& rhs); // Assignment operator + inline Matrix& operator=(const T& a); // Assign to every element value a + inline Matrix& operator+=(const Matrix& rhs); + inline Matrix& operator-=(const Matrix& rhs); + inline Matrix& operator*=(const Matrix& rhs); + inline Matrix& operator/=(const Matrix& rhs); + inline Matrix& operator^=(const Matrix& rhs); + inline Matrix& operator+=(const T& a); + inline Matrix& operator-=(const T& a); + inline Matrix& operator*=(const T& a); + inline Matrix& operator/=(const T& a); + inline Matrix& operator^=(const T& a); + inline operator Vector(); +private: + unsigned int n; // number of rows + unsigned int m; // number of columns + T **v; // storage for data +}; + +template +Matrix::Matrix() + : n(0), m(0), v(0) +{} + +template +Matrix::Matrix(unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; +} + +template +Matrix::Matrix(const T& a, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; +} + +template +Matrix::Matrix(const T* a, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = *a++; +} + +template +Matrix::Matrix(MType t, const T& a, const T& o, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + switch (t) + { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(MType t, const Vector& a, const T& o, unsigned int n, unsigned int m) + : v(new T*[n]) +{ + this->n = n; this->m = m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + switch (t) + { + case DIAG: + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + if (i != j) + v[i][j] = o; + else + v[i][j] = a[i]; + break; + default: + throw std::logic_error("Matrix type not supported"); + } +} + +template +Matrix::Matrix(const Matrix& rhs) + : v(new T*[rhs.n]) +{ + n = rhs.n; m = rhs.m; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = rhs[i][j]; +} + +template +Matrix::~Matrix() +{ + if (v != 0) { + delete[] (v[0]); + delete[] (v); + } +} + +template +inline Matrix& Matrix::operator=(const Matrix &rhs) +// postcondition: normal assignment via copying has been performed; +// if matrix and rhs were different sizes, matrix +// has been resized to match the size of rhs +{ + if (this != &rhs) + { + resize(rhs.n, rhs.m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = rhs[i][j]; + } + return *this; +} + +template +inline Matrix& Matrix::operator=(const T& a) // assign a to every element +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; + return *this; +} + + +template +inline void Matrix::resize(const unsigned int n, const unsigned int m) +{ + if (n == this->n && m == this->m) + return; + if (v != 0) + { + delete[] (v[0]); + delete[] (v); + } + this->n = n; this->m = m; + v = new T*[n]; + v[0] = new T[m * n]; + for (unsigned int i = 1; i < n; i++) + v[i] = v[i - 1] + m; +} + +template +inline void Matrix::resize(const T& a, const unsigned int n, const unsigned int m) +{ + resize(n, m); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a; +} + + + +template +inline Vector Matrix::extractRow(const unsigned int i) const +{ + if (i >= n) + throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(v[i], m); + + return tmp; +} + +template +inline Vector Matrix::extractColumn(const unsigned int j) const +{ + if (j >= m) + throw std::logic_error("Error in extractRow: trying to extract a row out of matrix bounds"); + Vector tmp(n); + + for (unsigned int i = 0; i < n; i++) + tmp[i] = v[i][j]; + + return tmp; +} + +template +inline Vector Matrix::extractDiag() const +{ + unsigned int d = std::min(n, m); + + Vector tmp(d); + + for (unsigned int i = 0; i < d; i++) + tmp[i] = v[i][i]; + + return tmp; + +} + +template +inline Matrix Matrix::extractRows(const std::set& indexes) const +{ + Matrix tmp(indexes.size(), m); + unsigned int i = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int j = 0; j < m; j++) + { + if (*el >= n) + throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*el][j]; + } + i++; + } + + return tmp; +} + +template +inline Matrix Matrix::extractColumns(const std::set& indexes) const +{ + Matrix tmp(n, indexes.size()); + unsigned int j = 0; + + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int i = 0; i < n; i++) + { + if (*el >= m) + throw std::logic_error("Error extracting columns: the indexes are out of matrix bounds"); + tmp[i][j] = v[i][*el]; + } + j++; + } + + return tmp; +} + +template +inline Matrix Matrix::extract(const std::set& r_indexes, const std::set& c_indexes) const +{ + Matrix tmp(r_indexes.size(), c_indexes.size()); + unsigned int i = 0, j; + + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) + { + if (*r_el >= n) + throw std::logic_error("Error extracting submatrix: the indexes are out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++) + { + if (*c_el >= m) + throw std::logic_error("Error extracting rows: the indexes are out of matrix bounds"); + tmp[i][j] = v[*r_el][*c_el]; + j++; + } + i++; + } + + return tmp; +} + +template +inline void Matrix::setRow(unsigned int i, const Vector& a) +{ + if (i >= n) + throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.size()) + throw std::logic_error("Error setting matrix row: ranges are not compatible"); + for (unsigned int j = 0; j < ncols(); j++) + v[i][j] = a[j]; +} + +template +inline void Matrix::setRow(unsigned int i, const Matrix& a) +{ + if (i >= n) + throw std::logic_error("Error in setRow: trying to set a row out of matrix bounds"); + if (this->m != a.ncols()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + if (a.nrows() != 1) + throw std::logic_error("Error setting matrix column with a non-row matrix"); + for (unsigned int j = 0; j < ncols(); j++) + v[i][j] = a[0][j]; +} + +template +inline void Matrix::setRows(const std::set& indexes, const Matrix& m) +{ + unsigned int i = 0; + + if (indexes.size() != m.nrows() || this->m != m.ncols()) + throw std::logic_error("Error setting matrix rows: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int j = 0; j < ncols(); j++) + { + if (*el >= n) + throw std::logic_error("Error in setRows: trying to set a row out of matrix bounds"); + v[*el][j] = m[i][j]; + } + i++; + } +} + +template +inline void Matrix::setColumn(unsigned int j, const Vector& a) +{ + if (j >= m) + throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.size()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + for (unsigned int i = 0; i < nrows(); i++) + v[i][j] = a[i]; +} + +template +inline void Matrix::setColumn(unsigned int j, const Matrix& a) +{ + if (j >= m) + throw std::logic_error("Error in setColumn: trying to set a column out of matrix bounds"); + if (this->n != a.nrows()) + throw std::logic_error("Error setting matrix column: ranges are not compatible"); + if (a.ncols() != 1) + throw std::logic_error("Error setting matrix column with a non-column matrix"); + for (unsigned int i = 0; i < nrows(); i++) + v[i][j] = a[i][0]; +} + + +template +inline void Matrix::setColumns(const std::set& indexes, const Matrix& a) +{ + unsigned int j = 0; + + if (indexes.size() != a.ncols() || this->n != a.nrows()) + throw std::logic_error("Error setting matrix columns: ranges are not compatible"); + for (std::set::const_iterator el = indexes.begin(); el != indexes.end(); el++) + { + for (unsigned int i = 0; i < nrows(); i++) + { + if (*el >= m) + throw std::logic_error("Error in setColumns: trying to set a column out of matrix bounds"); + v[i][*el] = a[i][j]; + } + j++; + } +} + +template +inline void Matrix::set(const std::set& r_indexes, const std::set& c_indexes, const Matrix& a) +{ + unsigned int i = 0, j; + if (c_indexes.size() != a.ncols() || r_indexes.size() != a.nrows()) + throw std::logic_error("Error setting matrix elements: ranges are not compatible"); + + for (std::set::const_iterator r_el = r_indexes.begin(); r_el != r_indexes.end(); r_el++) + { + if (*r_el >= n) + throw std::logic_error("Error in set: trying to set a row out of matrix bounds"); + j = 0; + for (std::set::const_iterator c_el = c_indexes.begin(); c_el != c_indexes.end(); c_el++) + { + if (*c_el >= m) + throw std::logic_error("Error in set: trying to set a column out of matrix bounds"); + v[*r_el][*c_el] = a[i][j]; + j++; + } + i++; + } +} + +template +inline void Matrix::set(const T* a, unsigned int n, unsigned int m) +{ + if (this->n != n || this->m != m) + resize(n, m); + unsigned int k = 0; + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = a[k++]; +} + + +template +Matrix operator+(const Matrix& rhs) +{ + return rhs; +} + +template +Matrix operator+(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] + rhs[i][j]; + + return tmp; +} + +template +Matrix operator+(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] + a; + + return tmp; +} + +template +Matrix operator+(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a + rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator+=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] += rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator+=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] += a; + + return *this; +} + +template +Matrix operator-(const Matrix& rhs) +{ + return (T)(-1) * rhs; +} + +template +Matrix operator-(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator-: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] - rhs[i][j]; + + return tmp; +} + +template +Matrix operator-(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] - a; + + return tmp; +} + +template +Matrix operator-(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a - rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator-=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator-=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] -= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator-=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] -= a; + + return *this; +} + +template +Matrix operator*(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator*: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] * rhs[i][j]; + + return tmp; +} + +template +Matrix operator*(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] * a; + + return tmp; +} + +template +Matrix operator*(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a * rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator*=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator*=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] *= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator*=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] *= a; + + return *this; +} + +template +Matrix operator/(const Matrix& lhs, const Matrix& rhs) +{ + if (lhs.ncols() != rhs.ncols() || lhs.nrows() != rhs.nrows()) + throw std::logic_error("Operator+: matrices have different sizes"); + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] / rhs[i][j]; + + return tmp; +} + +template +Matrix operator/(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = lhs[i][j] / a; + + return tmp; +} + +template +Matrix operator/(const T& a, const Matrix& rhs) +{ + Matrix tmp(rhs.nrows(), rhs.ncols()); + for (unsigned int i = 0; i < rhs.nrows(); i++) + for (unsigned int j = 0; j < rhs.ncols(); j++) + tmp[i][j] = a / rhs[i][j]; + + return tmp; +} + +template +inline Matrix& Matrix::operator/=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator+=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] /= rhs[i][j]; + + return *this; +} + +template +inline Matrix& Matrix::operator/=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] /= a; + + return *this; +} + +template +Matrix operator^(const Matrix& lhs, const T& a) +{ + Matrix tmp(lhs.nrows(), lhs.ncols()); + for (unsigned int i = 0; i < lhs.nrows(); i++) + for (unsigned int j = 0; j < lhs.ncols(); j++) + tmp[i][j] = pow(lhs[i][j], a); + + return tmp; +} + +template +inline Matrix& Matrix::operator^=(const Matrix& rhs) +{ + if (m != rhs.ncols() || n != rhs.nrows()) + throw std::logic_error("Operator^=: matrices have different sizes"); + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = pow(v[i][j], rhs[i][j]); + + return *this; +} + + +template +inline Matrix& Matrix::operator^=(const T& a) +{ + for (unsigned int i = 0; i < n; i++) + for (unsigned int j = 0; j < m; j++) + v[i][j] = pow(v[i][j], a); + + return *this; +} + +template +inline Matrix::operator Vector() +{ + if (n > 1 && m > 1) + throw std::logic_error("Error matrix cast to vector: trying to cast a multi-dimensional matrix"); + if (n == 1) + return extractRow(0); + else + return extractColumn(0); +} + +template +inline bool operator==(const Matrix& a, const Matrix& b) +{ + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) + return false; + return true; +} + +template +inline bool operator!=(const Matrix& a, const Matrix& b) +{ + if (a.nrows() != b.nrows() || a.ncols() != b.ncols()) + throw std::logic_error("Matrices of different size are not confrontable"); + for (unsigned i = 0; i < a.nrows(); i++) + for (unsigned j = 0; j < a.ncols(); j++) + if (a[i][j] != b[i][j]) + return true; + return false; +} + + + +/** + Input/Output +*/ +template +std::ostream& operator<<(std::ostream& os, const Matrix& m) +{ + os << std::endl << m.nrows() << " " << m.ncols() << std::endl; + for (unsigned int i = 0; i < m.nrows(); i++) + { + for (unsigned int j = 0; j < m.ncols() - 1; j++) + os << std::setw(20) << std::setprecision(16) << m[i][j] << ", "; + os << std::setw(20) << std::setprecision(16) << m[i][m.ncols() - 1] << std::endl; + } + + return os; +} + +template +std::istream& operator>>(std::istream& is, Matrix& m) +{ + int rows, cols; + char comma; + is >> rows >> cols; + m.resize(rows, cols); + for (unsigned int i = 0; i < rows; i++) + for (unsigned int j = 0; j < cols; j++) + is >> m[i][j] >> comma; + + return is; +} + +template +T sign(const T& v) +{ + if (v >= (T)0.0) + return (T)1.0; + else + return (T)-1.0; +} + +template +T dist(const T& a, const T& b) +{ + T abs_a = (T)fabs(a), abs_b = (T)fabs(b); + if (abs_a > abs_b) + return abs_a * sqrt((T)1.0 + (abs_b / abs_a) * (abs_b / abs_a)); + else + return (abs_b == (T)0.0 ? (T)0.0 : abs_b * sqrt((T)1.0 + (abs_a / abs_b) * (abs_a / abs_b))); +} + +template +void svd(const Matrix& A, Matrix& U, Vector& W, Matrix& V) +{ + int m = A.nrows(), n = A.ncols(), i, j, k, l, jj, nm; + const unsigned int max_its = 30; + bool flag; + Vector rv1(n); + U = A; + W.resize(n); + V.resize(n, n); + T anorm, c, f, g, h, s, scale, x, y, z; + g = scale = anorm = (T)0.0; + + // Householder reduction to bidiagonal form + for (i = 0; i < n; i++) + { + l = i + 1; + rv1[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m) + { + for (k = i; k < m; k++) + scale += fabs(U[k][i]); + if (scale != (T)0.0) + { + for (k = i; k < m; k++) + { + U[k][i] /= scale; + s += U[k][i] * U[k][i]; + } + f = U[i][i]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][i] = f - g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = i; k < m; k++) + s += U[k][i] * U[k][j]; + f = s / h; + for (k = i; k < m; k++) + U[k][j] += f * U[k][i]; + } + for (k = i; k < m; k++) + U[k][i] *= scale; + } + } + W[i] = scale * g; + g = s = scale = (T)0.0; + if (i < m && i != n - 1) + { + for (k = l; k < n; k++) + scale += fabs(U[i][k]); + if (scale != (T)0.0) + { + for (k = l; k < n; k++) + { + U[i][k] /= scale; + s += U[i][k] * U[i][k]; + } + f = U[i][l]; + g = -sign(f) * sqrt(s); + h = f * g - s; + U[i][l] = f - g; + for (k = l; k = 0; i--) + { + if (i < n - 1) + { + if (g != (T)0.0) + { + for (j = l; j < n; j++) + V[j][i] = (U[i][j] / U[i][l]) / g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = l; k < n; k++) + s += U[i][k] * V[k][j]; + for (k = l; k < n; k++) + V[k][j] += s * V[k][i]; + } + } + for (j = l; j < n; j++) + V[i][j] = V[j][i] = (T)0.0; + } + V[i][i] = (T)1.0; + g = rv1[i]; + l = i; + } + // Accumulation of left-hand transformations + for (i = std::min(m, n) - 1; i >= 0; i--) + { + l = i + 1; + g = W[i]; + for (j = l; j < n; j++) + U[i][j] = (T)0.0; + if (g != (T)0.0) + { + g = (T)1.0 / g; + for (j = l; j < n; j++) + { + s = (T)0.0; + for (k = l; k < m; k++) + s += U[k][i] * U[k][j]; + f = (s / U[i][i]) * g; + for (k = i; k < m; k++) + U[k][j] += f * U[k][i]; + } + for (j = i; j < m; j++) + U[j][i] *= g; + } + else + for (j = i; j < m; j++) + U[j][i] = (T)0.0; + U[i][i]++; + } + // Diagonalization of the bidiagonal form: loop over singular values, and over allowed iterations. + for (k = n - 1; k >= 0; k--) + { + 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 + { // Test for splitting + nm = l - 1; // Note that rV[0] is always zero + if ((T)(fabs(rv1[l]) + anorm) == anorm) + { + flag = false; + break; + } + if ((T)(fabs(W[nm]) + anorm) == anorm) + break; + } + if (flag) + { + // Cancellation of rv1[l], if l > 0 FIXME: it was l > 1 in NR + c = (T)0.0; + s = (T)1.0; + for (i = l; i <= k; i++) + { + f = s * rv1[i]; + rv1[i] *= c; + if ((T)(fabs(f) + anorm) == anorm) + break; + g = W[i]; + h = dist(f, g); + W[i] = h; + h = (T)1.0 / h; + c = g * h; + s = -f * h; + for (j = 0; j < m; j++) + { + y = U[j][nm]; + z = U[j][i]; + U[j][nm] = y * c + z * s; + U[j][i] = z * c - y * s; + } + } + } + z = W[k]; + if (l == k) + { // Convergence + if (z < (T)0.0) + { // Singular value is made nonnegative + W[k] = -z; + for (j = 0; j < n; j++) + V[j][k] = -V[j][k]; + } + break; + } + if (its == max_its) + throw std::logic_error("Error svd: no convergence in the maximum number of iterations"); + x = W[l]; + nm = k - 1; + y = W[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); + g = dist(f, (T)1.0); + f = ((x - z) * (x + z) + h * ((y / (f + sign(f)*fabs(g))) - h)) / x; + c = s = (T)1.0; // Next QR transformation + for (j = l; j <= nm; j++) + { + i = j + 1; + g = rv1[i]; + y = W[i]; + h = s * g; + g *= c; + z = dist(f, h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < n; jj++) + { + x = V[jj][j]; + z = V[jj][i]; + V[jj][j] = x * c + z * s; + V[jj][i] = z * c - x * s; + } + z = dist(f, h); + W[j] = z; + if (z != 0) // Rotation can be arbitrary if z = 0 + { + z = (T)1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < m; jj++) + { + y = U[jj][j]; + z = U[jj][i]; + U[jj][j] = y * c + z * s; + U[jj][i] = z * c - y * s; + } + } + rv1[l] = (T)0.0; + rv1[k] = f; + W[k] = x; + } + } +} + +template +Matrix pinv(const Matrix& A) +{ + Matrix U, V, x, tmp(A.ncols(), A.nrows()); + Vector W; + CanonicalBaseVector e(0, A.nrows()); + svd(A, U, W, V); + for (unsigned int i = 0; i < A.nrows(); i++) + { + e.reset(i); + tmp.setColumn(i, dot_prod(dot_prod(dot_prod(V, Matrix(DIAG, 1.0 / W, 0.0, W.size(), W.size())), t(U)), e)); + } + + return tmp; +} + +template +int lu(const Matrix& A, Matrix& LU, Vector& index) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU decomposition: matrix must be squared"); + int i, p, j, k, n = A.ncols(), ex; + T val, tmp; + Vector d(n); + LU = A; + index.resize(n); + + ex = 1; + for (i = 0; i < n; i++) + { + index[i] = i; + val = (T)0.0; + for (j = 0; j < n; j++) + val = std::max(val, (T)fabs(LU[i][j])); + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + d[i] = val; + } + + for (k = 0; k < n - 1; k++) + { + p = k; + val = fabs(LU[k][k]) / d[k]; + for (i = k + 1; i < n; i++) + { + tmp = fabs(LU[i][k]) / d[i]; + if (tmp > val) + { + val = tmp; + p = i; + } + } + if (val == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + if (p > k) + { + ex = -ex; + std::swap(index[k], index[p]); + std::swap(d[k], d[p]); + for (j = 0; j < n; j++) + std::swap(LU[k][j], LU[p][j]); + } + + for (i = k + 1; i < n; i++) + { + LU[i][k] /= LU[k][k]; + for (j = k + 1; j < n; j++) + LU[i][j] -= LU[i][k] * LU[k][j]; + } + } + if (LU[n - 1][n - 1] == (T)0.0) + std::logic_error("Error in LU decomposition: matrix was singular"); + + return ex; +} + +template +Vector lu_solve(const Matrix& LU, const Vector& b, Vector& index) +{ + if (LU.ncols() != LU.nrows()) + throw std::logic_error("Error in LU solve: LU matrix should be squared"); + unsigned int n = LU.ncols(); + if (b.size() != n) + throw std::logic_error("Error in LU solve: b vector must be of the same dimensions of LU matrix"); + Vector x((T)0.0, n); + int i, j, p; + T sum; + + p = index[0]; + x[0] = b[p]; + + for (i = 1; i < n; i++) + { + sum = (T)0.0; + for (j = 0; j < i; j++) + sum += LU[i][j] * x[j]; + p = index[i]; + x[i] = b[p] - sum; + } + x[n - 1] /= LU[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) + { + sum = (T)0.0; + for (j = i + 1; j < n; j++) + sum += LU[i][j] * x[j]; + x[i] = (x[i] - sum) / LU[i][i]; + } + return x; +} + +template +void lu_solve(const Matrix& LU, Vector& x, const Vector& b, Vector& index) +{ + x = lu_solve(LU, b, index); +} + +template +Matrix lu_inverse(const Matrix& A) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU invert: matrix must be squared"); + unsigned int n = A.ncols(); + Matrix A1(n, n), LU; + Vector index; + + lu(A, LU, index); + CanonicalBaseVector e(0, n); + for (unsigned i = 0; i < n; i++) + { + e.reset(i); + A1.setColumn(i, lu_solve(LU, e, index)); + } + + return A1; +} + +template +T lu_det(const Matrix& A) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in LU determinant: matrix must be squared"); + unsigned int d; + Matrix LU; + Vector index; + + d = lu(A, LU, index); + + return d * prod(LU.extractDiag()); +} + +template +void cholesky(const Matrix A, Matrix& LL) +{ + if (A.ncols() != A.nrows()) + throw std::logic_error("Error in Cholesky decomposition: matrix must be squared"); + int n = A.ncols(); + double sum; + LL = A; + + for (unsigned int i = 0; i < n; i++) + { + for (unsigned int j = i; j < n; j++) + { + sum = LL[i][j]; + for (int k = i - 1; k >= 0; k--) + sum -= LL[i][k] * LL[j][k]; + if (i == j) + { + if (sum <= 0.0) + throw std::logic_error("Error in Cholesky decomposition: matrix is not postive definite"); + LL[i][i] = sqrt(sum); + } + else + LL[j][i] = sum / LL[i][i]; + } + for (unsigned int k = i + 1; k < n; k++) + LL[i][k] = LL[k][i]; + } +} + +template +Matrix cholesky(const Matrix A) +{ + Matrix LL; + cholesky(A, LL); + + return LL; +} + +template +Vector cholesky_solve(const Matrix& LL, const Vector& b) +{ + if (LL.ncols() != LL.nrows()) + throw std::logic_error("Error in Cholesky solve: matrix must be squared"); + unsigned int n = LL.ncols(); + if (b.size() != n) + throw std::logic_error("Error in Cholesky decomposition: b vector must be of the same dimensions of LU matrix"); + Vector x, y; + + /* Solve L * y = b */ + forward_elimination(LL, y, b); + /* Solve L^T * x = y */ + backward_elimination(LL, x, y); + + return x; +} + +template +void cholesky_solve(const Matrix& LL, Vector& x, const Vector& b) +{ + x = cholesky_solve(LL, b); +} + +template +void forward_elimination(const Matrix& L, Vector& y, const Vector b) +{ + if (L.ncols() != L.nrows()) + throw std::logic_error("Error in Forward elimination: matrix must be squared (lower triangular)"); + if (b.size() != L.nrows()) + throw std::logic_error("Error in Forward elimination: b vector must be of the same dimensions of L matrix"); + unsigned int n = b.size(); + y.resize(n); + + y[0] = b[0] / L[0][0]; + for (unsigned int i = 1; i < n; i++) + { + y[i] = b[i]; + for (unsigned int j = 0; j < i; j++) + y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +template +Vector forward_elimination(const Matrix& L, const Vector b) +{ + Vector y; + forward_elimination(L, y, b); + + return y; +} + +template +void backward_elimination(const Matrix& U, Vector& x, const Vector& y) +{ + if (U.ncols() != U.nrows()) + throw std::logic_error("Error in Backward elimination: matrix must be squared (upper triangular)"); + if (y.size() != U.nrows()) + throw std::logic_error("Error in Backward elimination: b vector must be of the same dimensions of U matrix"); + int n = y.size(); + x.resize(n); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (int i = n - 2; i >= 0; i--) + { + x[i] = y[i]; + for (int j = i + 1; j < n; j++) + x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +template +Vector backward_elimination(const Matrix& U, const Vector y) +{ + Vector x; + forward_elimination(U, x, y); + + return x; +} + +/* Setting default linear systems machinery */ + +// #define det lu_det +// #define inverse lu_inverse +// #define solve lu_solve + +/* Random */ + +template +void random(Matrix& m) +{ + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + m[i][j] = (T)(rand() / double(RAND_MAX)); +} + +/** + Aggregate functions +*/ + +template +Vector sum(const Matrix& m) +{ + Vector tmp((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) + tmp[j] += m[i][j]; + return tmp; +} + +template +Vector r_sum(const Matrix& m) +{ + Vector tmp((T)0, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i] += m[i][j]; + return tmp; +} + +template +T all_sum(const Matrix& m) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp += m[i][j]; + return tmp; +} + +template +Vector prod(const Matrix& m) +{ + Vector tmp((T)1, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + for (unsigned int i = 0; i < m.nrows(); i++) + tmp[j] *= m[i][j]; + return tmp; +} + +template +Vector r_prod(const Matrix& m) +{ + Vector tmp((T)1, m.nrows()); + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i] *= m[i][j]; + return tmp; +} + +template +T all_prod(const Matrix& m) +{ + T tmp = (T)1; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp *= m[i][j]; + return tmp; +} + +template +Vector mean(const Matrix& m) +{ + Vector res((T)0, m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + { + for (unsigned int i = 0; i < m.nrows(); i++) + res[j] += m[i][j]; + res[j] /= m.nrows(); + } + + return res; +} + +template +Vector r_mean(const Matrix& m) +{ + Vector res((T)0, m.rows()); + for (unsigned int i = 0; i < m.nrows(); i++) + { + for (unsigned int j = 0; j < m.ncols(); j++) + res[i] += m[i][j]; + res[i] /= m.nrows(); + } + + return res; +} + +template +T all_mean(const Matrix& m) +{ + T tmp = (T)0; + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp += m[i][j]; + return tmp / (m.nrows() * m.ncols()); +} + +template +Vector var(const Matrix& m, bool sample_correction = false) +{ + Vector res((T)0, m.ncols()); + unsigned int n = m.nrows(); + double sum, ssum; + for (unsigned int j = 0; j < m.ncols(); j++) + { + sum = (T)0.0; ssum = (T)0.0; + for (unsigned int i = 0; i < m.nrows(); i++) + { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[j] = (ssum / n) - (sum / n) * (sum / n); + else + res[j] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector stdev(const Matrix& m, bool sample_correction = false) +{ + return vec_sqrt(var(m, sample_correction)); +} + +template +Vector r_var(const Matrix& m, bool sample_correction = false) +{ + Vector res((T)0, m.nrows()); + double sum, ssum; + unsigned int n = m.ncols(); + for (unsigned int i = 0; i < m.nrows(); i++) + { + sum = 0.0; ssum = 0.0; + for (unsigned int j = 0; j < m.ncols(); j++) + { + sum += m[i][j]; + ssum += (m[i][j] * m[i][j]); + } + if (!sample_correction) + res[i] = (ssum / n) - (sum / n) * (sum / n); + else + res[i] = n * ((ssum / n) - (sum / n) * (sum / n)) / (n - 1); + } + + return res; +} + +template +Vector r_stdev(const Matrix& m, bool sample_correction = false) +{ + return vec_sqrt(r_var(m, sample_correction)); +} + +template +Vector max(const Matrix& m) +{ + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) + { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::max(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_max(const Matrix& m) +{ + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) + { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::max(m[i][j], value); + res[i] = value; + } + + return res; +} + +template +Vector min(const Matrix& m) +{ + Vector res(m.ncols()); + double value; + for (unsigned int j = 0; j < m.ncols(); j++) + { + value = m[0][j]; + for (unsigned int i = 1; i < m.nrows(); i++) + value = std::min(m[i][j], value); + res[j] = value; + } + + return res; +} + +template +Vector r_min(const Matrix& m) +{ + Vector res(m.nrows()); + double value; + for (unsigned int i = 0; i < m.nrows(); i++) + { + value = m[i][0]; + for (unsigned int j = 1; j < m.ncols(); j++) + value = std::min(m[i][j], value); + res[i] = value; + } + + return res; +} + + + +/** + Single element mathematical functions +*/ + +template +Matrix exp(const Matrix&m) +{ + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i][j] = exp(m[i][j]); + + return tmp; +} + +template +Matrix mat_sqrt(const Matrix&m) +{ + Matrix tmp(m.nrows(), m.ncols()); + + for (unsigned int i = 0; i < m.nrows(); i++) + for (unsigned int j = 0; j < m.ncols(); j++) + tmp[i][j] = sqrt(m[i][j]); + + return tmp; +} + +/** + Matrix operators +*/ + +template +Matrix kron(const Vector& b, const Vector& a) +{ + Matrix tmp(b.size(), a.size()); + for (unsigned int i = 0; i < b.size(); i++) + for (unsigned int j = 0; j < a.size(); j++) + tmp[i][j] = a[j] * b[i]; + + return tmp; +} + +template +Matrix t(const Matrix& a) +{ + Matrix tmp(a.ncols(), a.nrows()); + for (unsigned int i = 0; i < a.nrows(); i++) + for (unsigned int j = 0; j < a.ncols(); j++) + tmp[j][i] = a[i][j]; + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Matrix& b) +{ + if (a.ncols() != b.nrows()) + throw std::logic_error("Error matrix dot product: dimensions of the matrices are not compatible"); + Matrix tmp(a.nrows(), b.ncols()); + for (unsigned int i = 0; i < tmp.nrows(); i++) + for (unsigned int j = 0; j < tmp.ncols(); j++) + { + tmp[i][j] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) + tmp[i][j] += a[i][k] * b[k][j]; + } + + return tmp; +} + +template +Matrix dot_prod(const Matrix& a, const Vector& b) +{ + if (a.ncols() != b.size()) + throw std::logic_error("Error matrix dot product: dimensions of the matrix and the vector are not compatible"); + Matrix tmp(a.nrows(), 1); + for (unsigned int i = 0; i < tmp.nrows(); i++) + { + tmp[i][0] = (T)0; + for (unsigned int k = 0; k < a.ncols(); k++) + tmp[i][0] += a[i][k] * b[k]; + } + + return tmp; +} + +template +Matrix dot_prod(const Vector& a, const Matrix& b) +{ + if (a.size() != b.nrows()) + throw std::logic_error("Error matrix dot product: dimensions of the vector and matrix are not compatible"); + Matrix tmp(1, b.ncols()); + for (unsigned int j = 0; j < tmp.ncols(); j++) + { + tmp[0][j] = (T)0; + for (unsigned int k = 0; k < a.size(); k++) + tmp[0][j] += a[k] * b[k][j]; + } + + return tmp; +} + +template +inline Matrix rank(const Matrix m) +{ + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int j = 0; j < m.ncols(); j++) + tmp.setColumn(j, rank(m.extractColumn(j))); + + return tmp; +} + +template +inline Matrix r_rank(const Matrix m) +{ + Matrix tmp(m.nrows(), m.ncols()); + for (unsigned int i = 0; i < m.nrows(); i++) + tmp.setRow(i, rank(m.extractRow(i))); + + return tmp; +} + +} // namespace quadprogpp + +#endif // define _ARRAY_HH_ diff --git a/thirdparty/quadProgpp/include/QuadProg++.hh b/thirdparty/quadProgpp/include/QuadProg++.hh new file mode 100755 index 0000000..891d104 --- /dev/null +++ b/thirdparty/quadProgpp/include/QuadProg++.hh @@ -0,0 +1,77 @@ +/* + File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + for the solution of a (convex) Quadratic Programming problem + by means of an active-set dual method. + +The problem is in the form: + +min 0.5 * x G x + g0 x +s.t. + CE^T x + ce0 = 0 + CI^T x + ci0 >= 0 + + The matrix and vectors dimensions are as follows: + G: n * n + g0: n + + CE: n * p + ce0: p + + CI: n * m + ci0: m + + x: n + + The function will return the cost of the solution written in the x vector or + std::numeric_limits::infinity() if the problem is infeasible. In the latter case + the value of the x vector is not correct. + + References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving + strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. + + Notes: + 1. pay attention in setting up the vectors ce0 and ci0. + If the constraints of your problem are specified in the form + A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. + 2. The matrices have column dimension equal to MATRIX_DIM, + a constant set to 20 in this file (by means of a #define macro). + If the matrices are bigger than 20 x 20 the limit could be + increased by means of a -DMATRIX_DIM=n on the compiler command line. + 3. The matrix G is modified within the function since it is used to compute + the G = L^T L cholesky factorization for further computations inside the function. + If you need the original matrix G you should make a copy of it and pass the copy + to the function. + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + The author will be grateful if the researchers using this software will + acknowledge the contribution of this function in their research papers. + + Copyright (c) 2007-2016 Luca Di Gaspero + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. +*/ + + +#ifndef _QUADPROGPP +#define _QUADPROGPP + +#include "quadProgpp/include/Array.hh" +#include + +namespace quadprogpp { + +double solve_quadprog(Matrix& G, Vector& g0, + const Matrix& CE, const Vector& ce0, + const Matrix& CI, const Vector& ci0, + Vector& x); + +} // namespace quadprogpp + +#endif // #define _QUADPROGPP diff --git a/thirdparty/quadProgpp/src/Array.cc b/thirdparty/quadProgpp/src/Array.cc new file mode 100755 index 0000000..2b6eb7a --- /dev/null +++ b/thirdparty/quadProgpp/src/Array.cc @@ -0,0 +1,33 @@ +// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ +// This file is part of QuadProg++: +// Copyright (C) 2006--2009 Luca Di Gaspero. +// +// This software may be modified and distributed under the terms +// of the MIT license. See the LICENSE file for details. + +#include "quadProgpp/include/Array.hh" + +/** + Index utilities + */ + +namespace quadprogpp { + +std::set seq(unsigned int s, unsigned int e) +{ + std::set tmp; + for (unsigned int i = s; i <= e; i++) + tmp.insert(i); + + return tmp; +} + +std::set singleton(unsigned int i) +{ + std::set tmp; + tmp.insert(i); + + return tmp; +} + +} // namespace quadprogpp diff --git a/thirdparty/quadProgpp/src/QuadProg++.cc b/thirdparty/quadProgpp/src/QuadProg++.cc new file mode 100755 index 0000000..dc78e5e --- /dev/null +++ b/thirdparty/quadProgpp/src/QuadProg++.cc @@ -0,0 +1,791 @@ +/* +File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $ + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. + + */ + +#include +#include +#include +#include +#include +#include +#include "quadProgpp/include/QuadProg++.hh" +//#define TRACE_SOLVER + +namespace quadprogpp { + +// Utility functions for updating some data needed by the solution method +void compute_d(Vector& d, const Matrix& J, const Vector& np); +void update_z(Vector& z, const Matrix& J, const Vector& d, int iq); +void update_r(const Matrix& R, Vector& r, const Vector& d, int iq); +bool add_constraint(Matrix& R, Matrix& J, Vector& d, unsigned int& iq, double& rnorm); +void delete_constraint(Matrix& R, Matrix& J, Vector& A, Vector& u, unsigned int n, int p, unsigned int& iq, int l); + +// Utility functions for computing the Cholesky decomposition and solving +// linear systems +void cholesky_decomposition(Matrix& A); +void cholesky_solve(const Matrix& L, Vector& x, const Vector& b); +void forward_elimination(const Matrix& L, Vector& y, const Vector& b); +void backward_elimination(const Matrix& U, Vector& x, const Vector& y); + +// Utility functions for computing the scalar product and the euclidean +// distance between two numbers +double scalar_product(const Vector& x, const Vector& y); +double distance(double a, double b); + +// Utility functions for printing vectors and matrices +void print_matrix(const char* name, const Matrix& A, int n = -1, int m = -1); + +template +void print_vector(const char* name, const Vector& v, int n = -1); + +// The Solving function, implementing the Goldfarb-Idnani method +double solve_quadprog(Matrix& G, Vector& g0, + const Matrix& CE, const Vector& ce0, + const Matrix& CI, const Vector& ci0, + Vector& x) +{ + std::ostringstream msg; + unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols(); + if (G.nrows() != n) + { + msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")"; + throw std::logic_error(msg.str()); + } + if (CE.nrows() != n) + { + msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ce0.size() != p) + { + msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")"; + throw std::logic_error(msg.str()); + } + if (CI.nrows() != n) + { + msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ci0.size() != m) + { + msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")"; + throw std::logic_error(msg.str()); + } + x.resize(n); + register unsigned int i, j, k, l; /* indices */ + int ip; // this is the index of the constraint to be added to the active set + Matrix R(n, n), J(n, n); + Vector s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p); + double f_value, psi, c1, c2, sum, ss, R_norm; + double inf; + if (std::numeric_limits::has_infinity) + inf = std::numeric_limits::infinity(); + else + inf = 1.0E300; + double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1 + * and the full step length t2 */ + Vector A(m + p), A_old(m + p), iai(m + p); + unsigned int iq, iter = 0; + Vector iaexcl(m + p); + + /* p is the number of equality constraints */ + /* m is the number of inequality constraints */ +#ifdef TRACE_SOLVER + std::cout << std::endl << "Starting solve_quadprog" << std::endl; + print_matrix("G", G); + print_vector("g0", g0); + print_matrix("CE", CE); + print_vector("ce0", ce0); + print_matrix("CI", CI); + print_vector("ci0", ci0); +#endif + + /* + * Preprocessing phase + */ + + /* compute the trace of the original matrix G */ + c1 = 0.0; + for (i = 0; i < n; i++) + { + c1 += G[i][i]; + } + /* decompose the matrix G in the form L^T L */ + cholesky_decomposition(G); +#ifdef TRACE_SOLVER + print_matrix("G", G); +#endif + /* initialize the matrix R */ + for (i = 0; i < n; i++) + { + d[i] = 0.0; + for (j = 0; j < n; j++) + R[i][j] = 0.0; + } + R_norm = 1.0; /* this variable will hold the norm of the matrix R */ + + /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */ + c2 = 0.0; + for (i = 0; i < n; i++) + { + d[i] = 1.0; + forward_elimination(G, z, d); + for (j = 0; j < n; j++) + J[i][j] = z[j]; + c2 += z[i]; + d[i] = 0.0; + } +#ifdef TRACE_SOLVER + print_matrix("J", J); +#endif + + /* c1 * c2 is an estimate for cond(G) */ + + /* + * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x + * this is a feasible point in the dual space + * x = G^-1 * g0 + */ + cholesky_solve(G, x, g0); + for (i = 0; i < n; i++) + x[i] = -x[i]; + /* and compute the current solution value */ + f_value = 0.5 * scalar_product(g0, x); +#ifdef TRACE_SOLVER + std::cout << "Unconstrained solution: " << f_value << std::endl; + print_vector("x", x); +#endif + + /* Add equality constraints to the working set A */ + iq = 0; + for (i = 0; i < p; i++) + { + for (j = 0; j < n; j++) + np[j] = CE[j][i]; + compute_d(d, J, np); + update_z(z, J, d, iq); + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, iq); + print_vector("z", z); + print_vector("r", r, iq); + print_vector("d", d); +#endif + + /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint + becomes feasible */ + t2 = 0.0; + if (fabs(scalar_product(z, z)) > std::numeric_limits::epsilon()) // i.e. z != 0 + t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np); + + /* set x = x + t2 * z */ + for (k = 0; k < n; k++) + x[k] += t2 * z[k]; + + /* set u = u+ */ + u[iq] = t2; + for (k = 0; k < iq; k++) + u[k] -= t2 * r[k]; + + /* compute the new solution value */ + f_value += 0.5 * (t2 * t2) * scalar_product(z, np); + A[i] = -i - 1; + + if (!add_constraint(R, J, d, iq, R_norm)) + { + // Equality constraints are linearly dependent + throw std::runtime_error("Constraints are linearly dependent"); + return f_value; + } + } + + /* set iai = K \ A */ + for (i = 0; i < m; i++) + iai[i] = i; + +l1: iter++; +#ifdef TRACE_SOLVER + print_vector("x", x); +#endif + /* step 1: choose a violated constraint */ + for (i = p; i < iq; i++) + { + ip = A[i]; + iai[ip] = -1; + } + + /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ + ss = 0.0; + psi = 0.0; /* this value will contain the sum of all infeasibilities */ + ip = 0; /* ip will be the index of the chosen violated constraint */ + for (i = 0; i < m; i++) + { + iaexcl[i] = true; + sum = 0.0; + for (j = 0; j < n; j++) + sum += CI[j][i] * x[j]; + sum += ci0[i]; + s[i] = sum; + psi += std::min(0.0, sum); + } +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + + + if (fabs(psi) <= m * std::numeric_limits::epsilon() * c1 * c2* 100.0) + { + /* numerically there are not infeasibilities anymore */ + return f_value; + } + + /* save old values for u and A */ + for (i = 0; i < iq; i++) + { + u_old[i] = u[i]; + A_old[i] = A[i]; + } + /* and for x */ + for (i = 0; i < n; i++) + x_old[i] = x[i]; + +l2: /* Step 2: check for feasibility and determine a new S-pair */ + for (i = 0; i < m; i++) + { + if (s[i] < ss && iai[i] != -1 && iaexcl[i]) + { + ss = s[i]; + ip = i; + } + } + if (ss >= 0.0) + { + return f_value; + } + + /* set np = n[ip] */ + for (i = 0; i < n; i++) + np[i] = CI[i][ip]; + /* set u = [u 0]^T */ + u[iq] = 0.0; + /* add ip to the active set A */ + A[iq] = ip; + +#ifdef TRACE_SOLVER + std::cout << "Trying with constraint " << ip << std::endl; + print_vector("np", np); +#endif + +l2a:/* Step 2a: determine step direction */ + /* compute z = H np: the step direction in the primal space (through J, see the paper) */ + compute_d(d, J, np); + update_z(z, J, d, iq); + /* compute N* np (if q > 0): the negative of the step direction in the dual space */ + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + std::cout << "Step direction z" << std::endl; + print_vector("z", z); + print_vector("r", r, iq + 1); + print_vector("u", u, iq + 1); + print_vector("d", d); + print_vector("A", A, iq + 1); +#endif + + /* Step 2b: compute step length */ + l = 0; + /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */ + t1 = inf; /* +inf */ + /* find the index l s.t. it reaches the minimum of u+[x] / r */ + for (k = p; k < iq; k++) + { + if (r[k] > 0.0) + { + if (u[k] / r[k] < t1) + { + t1 = u[k] / r[k]; + l = A[k]; + } + } + } + /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */ + if (fabs(scalar_product(z, z)) > std::numeric_limits::epsilon()) // i.e. z != 0 + { + t2 = -s[ip] / scalar_product(z, np); + if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies + t2 = inf; + } + else + t2 = inf; /* +inf */ + + /* the step is chosen as the minimum of t1 and t2 */ + t = std::min(t1, t2); +#ifdef TRACE_SOLVER + std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; +#endif + + /* Step 2c: determine new S-pair and take step: */ + + /* case (i): no step in primal or dual space */ + if (t >= inf) + { + /* QPP is infeasible */ + // FIXME: unbounded to raise + return inf; + } + /* case (ii): step in dual space */ + if (t2 >= inf) + { + /* set u = u + t * [-r 1] and drop constraint l from the active set A */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + std::cout << " in dual space: " + << f_value << std::endl; + print_vector("x", x); + print_vector("z", z); + print_vector("A", A, iq + 1); +#endif + goto l2a; + } + + /* case (iii): step in primal and dual space */ + + /* set x = x + t * z */ + for (k = 0; k < n; k++) + x[k] += t * z[k]; + /* update the solution value */ + f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]); + /* u = u + t * [-r 1] */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; +#ifdef TRACE_SOLVER + std::cout << " in both spaces: " + << f_value << std::endl; + print_vector("x", x); + print_vector("u", u, iq + 1); + print_vector("r", r, iq + 1); + print_vector("A", A, iq + 1); +#endif + + if (fabs(t - t2) < std::numeric_limits::epsilon()) + { +#ifdef TRACE_SOLVER + std::cout << "Full step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* full step has taken */ + /* add constraint ip to the active set*/ + if (!add_constraint(R, J, d, iq, R_norm)) + { + iaexcl[ip] = false; + delete_constraint(R, J, A, u, n, p, iq, ip); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + for (i = 0; i < m; i++) + iai[i] = i; + for (i = p; i < iq; i++) + { + A[i] = A_old[i]; + u[i] = u_old[i]; + iai[A[i]] = -1; + } + for (i = 0; i < n; i++) + x[i] = x_old[i]; + goto l2; /* go to step 2 */ + } + else + iai[ip] = -1; +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + goto l1; + } + + /* a patial step has taken */ +#ifdef TRACE_SOLVER + std::cout << "Partial step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* drop constraint l */ + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); +#endif + + /* update s[ip] = CI * x + ci0 */ + sum = 0.0; + for (k = 0; k < n; k++) + sum += CI[k][ip] * x[k]; + s[ip] = sum + ci0[ip]; + +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + goto l2a; +} + +inline void compute_d(Vector& d, const Matrix& J, const Vector& np) +{ + register int i, j, n = d.size(); + register double sum; + + /* compute d = H^T * np */ + for (i = 0; i < n; i++) + { + sum = 0.0; + for (j = 0; j < n; j++) + sum += J[j][i] * np[j]; + d[i] = sum; + } +} + +inline void update_z(Vector& z, const Matrix& J, const Vector& d, int iq) +{ + register int i, j, n = z.size(); + + /* setting of z = H * d */ + for (i = 0; i < n; i++) + { + z[i] = 0.0; + for (j = iq; j < n; j++) + z[i] += J[i][j] * d[j]; + } +} + +inline void update_r(const Matrix& R, Vector& r, const Vector& d, int iq) +{ + register int i, j; + register double sum; + + /* setting of r = R^-1 d */ + for (i = iq - 1; i >= 0; i--) + { + sum = 0.0; + for (j = i + 1; j < iq; j++) + sum += R[i][j] * r[j]; + r[i] = (d[i] - sum) / R[i][i]; + } +} + +bool add_constraint(Matrix& R, Matrix& J, Vector& d, unsigned int& iq, double& R_norm) +{ + unsigned int n = d.size(); +#ifdef TRACE_SOLVER + std::cout << "Add constraint " << iq << '/'; +#endif + register unsigned int i, j, k; + double cc, ss, h, t1, t2, xny; + + /* we have to find the Givens rotation which will reduce the element + d[j] to zero. + if it is already zero we don't have to do anything, except of + decreasing j */ + for (j = n - 1; j >= iq + 1; j--) + { + /* The Givens rotation is done with the matrix (cc cs, cs -cc). + If cc is one, then element (j) of d is zero compared with element + (j - 1). Hence we don't have to do anything. + If cc is zero, then we just have to switch column (j) and column (j - 1) + of J. Since we only switch columns in J, we have to be careful how we + update d depending on the sign of gs. + Otherwise we have to apply the Givens rotation to these columns. + The i - 1 element of d has to be updated to h. */ + cc = d[j - 1]; + ss = d[j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + d[j] = 0.0; + ss = ss / h; + cc = cc / h; + if (cc < 0.0) + { + cc = -cc; + ss = -ss; + d[j - 1] = -h; + } + else + d[j - 1] = h; + xny = ss / (1.0 + cc); + for (k = 0; k < n; k++) + { + t1 = J[k][j - 1]; + t2 = J[k][j]; + J[k][j - 1] = t1 * cc + t2 * ss; + J[k][j] = xny * (t1 + J[k][j - 1]) - t2; + } + } + /* update the number of constraints added*/ + iq++; + /* To update R we have to put the iq components of the d vector + into column iq - 1 of R + */ + for (i = 0; i < iq; i++) + R[i][iq - 1] = d[i]; +#ifdef TRACE_SOLVER + std::cout << iq << std::endl; + print_matrix("R", R, iq, iq); + print_matrix("J", J); + print_vector("d", d, iq); +#endif + + if (fabs(d[iq - 1]) <= std::numeric_limits::epsilon() * R_norm) + { + // problem degenerate + return false; + } + R_norm = std::max(R_norm, fabs(d[iq - 1])); + return true; +} + +void delete_constraint(Matrix& R, Matrix& J, Vector& A, Vector& u, unsigned int n, int p, unsigned int& iq, int l) +{ +#ifdef TRACE_SOLVER + std::cout << "Delete constraint " << l << ' ' << iq; +#endif + register unsigned int i, j, k, qq = 0; // just to prevent warnings from smart compilers + double cc, ss, h, xny, t1, t2; + + bool found = false; + /* Find the index qq for active constraint l to be removed */ + for (i = p; i < iq; i++) + if (A[i] == l) + { + qq = i; + found = true; + break; + } + + if(!found) + { + std::ostringstream os; + os << "Attempt to delete non existing constraint, constraint: " << l; + throw std::invalid_argument(os.str()); + } + /* remove the constraint from the active set and the duals */ + for (i = qq; i < iq - 1; i++) + { + A[i] = A[i + 1]; + u[i] = u[i + 1]; + for (j = 0; j < n; j++) + R[j][i] = R[j][i + 1]; + } + + A[iq - 1] = A[iq]; + u[iq - 1] = u[iq]; + A[iq] = 0; + u[iq] = 0.0; + for (j = 0; j < iq; j++) + R[j][iq - 1] = 0.0; + /* constraint has been fully removed */ + iq--; +#ifdef TRACE_SOLVER + std::cout << '/' << iq << std::endl; +#endif + + if (iq == 0) + return; + + for (j = qq; j < iq; j++) + { + cc = R[j][j]; + ss = R[j + 1][j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + cc = cc / h; + ss = ss / h; + R[j + 1][j] = 0.0; + if (cc < 0.0) + { + R[j][j] = -h; + cc = -cc; + ss = -ss; + } + else + R[j][j] = h; + + xny = ss / (1.0 + cc); + for (k = j + 1; k < iq; k++) + { + t1 = R[j][k]; + t2 = R[j + 1][k]; + R[j][k] = t1 * cc + t2 * ss; + R[j + 1][k] = xny * (t1 + R[j][k]) - t2; + } + for (k = 0; k < n; k++) + { + t1 = J[k][j]; + t2 = J[k][j + 1]; + J[k][j] = t1 * cc + t2 * ss; + J[k][j + 1] = xny * (J[k][j] + t1) - t2; + } + } +} + +inline double distance(double a, double b) +{ + register double a1, b1, t; + a1 = fabs(a); + b1 = fabs(b); + if (a1 > b1) + { + t = (b1 / a1); + return a1 * sqrt(1.0 + t * t); + } + else + if (b1 > a1) + { + t = (a1 / b1); + return b1 * sqrt(1.0 + t * t); + } + return a1 * sqrt(2.0); +} + + +inline double scalar_product(const Vector& x, const Vector& y) +{ + register int i, n = x.size(); + register double sum; + + sum = 0.0; + for (i = 0; i < n; i++) + sum += x[i] * y[i]; + return sum; +} + +void cholesky_decomposition(Matrix& A) +{ + register int i, j, k, n = A.nrows(); + register double sum; + + for (i = 0; i < n; i++) + { + for (j = i; j < n; j++) + { + sum = A[i][j]; + for (k = i - 1; k >= 0; k--) + sum -= A[i][k]*A[j][k]; + if (i == j) + { + if (sum <= 0.0) + { + std::ostringstream os; + // raise error + print_matrix("A", A); + os << "Error in cholesky decomposition, sum: " << sum; + throw std::logic_error(os.str()); + exit(-1); + } + A[i][i] = sqrt(sum); + } + else + A[j][i] = sum / A[i][i]; + } + for (k = i + 1; k < n; k++) + A[i][k] = A[k][i]; + } +} + +void cholesky_solve(const Matrix& L, Vector& x, const Vector& b) +{ + int n = L.nrows(); + Vector y(n); + + /* Solve L * y = b */ + forward_elimination(L, y, b); + /* Solve L^T * x = y */ + backward_elimination(L, x, y); +} + +inline void forward_elimination(const Matrix& L, Vector& y, const Vector& b) +{ + register int i, j, n = L.nrows(); + + y[0] = b[0] / L[0][0]; + for (i = 1; i < n; i++) + { + y[i] = b[i]; + for (j = 0; j < i; j++) + y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +inline void backward_elimination(const Matrix& U, Vector& x, const Vector& y) +{ + register int i, j, n = U.nrows(); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) + { + x[i] = y[i]; + for (j = i + 1; j < n; j++) + x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +void print_matrix(const char* name, const Matrix& A, int n, int m) +{ + std::ostringstream s; + std::string t; + if (n == -1) + n = A.nrows(); + if (m == -1) + m = A.ncols(); + + s << name << ": " << std::endl; + for (int i = 0; i < n; i++) + { + s << " "; + for (int j = 0; j < m; j++) + s << A[i][j] << ", "; + s << std::endl; + } + t = s.str(); + t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline + + std::cout << t << std::endl; +} + +template +void print_vector(const char* name, const Vector& v, int n) +{ + std::ostringstream s; + std::string t; + if (n == -1) + n = v.size(); + + s << name << ": " << std::endl << " "; + for (int i = 0; i < n; i++) + { + s << v[i] << ", "; + } + t = s.str(); + t = t.substr(0, t.size() - 2); // To remove the trailing space and comma + + std::cout << t << std::endl; +} + +} // namespace quadprogpp diff --git a/thirdparty/rbdl-2.6.0.zip b/thirdparty/rbdl-2.6.0.zip new file mode 100644 index 0000000..6208944 Binary files /dev/null and b/thirdparty/rbdl-2.6.0.zip differ