├── unitree_legged_msgs ├── msg │ ├── LED.msg │ ├── Cartesian.msg │ ├── IMU.msg │ ├── MotorCmd.msg │ ├── MotorState.msg │ ├── LowCmd.msg │ ├── HighCmd.msg │ ├── LowState.msg │ └── HighState.msg ├── package.xml └── CMakeLists.txt ├── song_msgs ├── msg │ ├── MotorCmd.msg │ └── MotorState.msg ├── .idea │ ├── song_msgs.iml │ ├── misc.xml │ ├── libraries │ │ ├── workspace.xml │ │ └── ROS.xml │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── ros.xml ├── package.xml └── CMakeLists.txt ├── song_wbc ├── .idea │ ├── song_wbc.iml │ ├── misc.xml │ ├── libraries │ │ ├── workspace.xml │ │ └── ROS.xml │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── ros.xml ├── CMakeLists.txt ├── include │ └── song_wbc │ │ ├── Utils.h │ │ └── controller │ │ └── OscStandController.h ├── package.xml └── src │ └── WbcStand.cpp ├── song_gazebo ├── .idea │ ├── song_gazebo.iml │ ├── misc.xml │ ├── libraries │ │ ├── workspace.xml │ │ └── ROS.xml │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── ros.xml ├── include │ └── InitPosition.h ├── InitPosition.cpp ├── package.xml └── CMakeLists.txt ├── unitree_gazebo ├── .idea │ ├── unitree_gazebo.iml │ ├── misc.xml │ ├── libraries │ │ ├── workspace.xml │ │ └── ROS.xml │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── ros.xml ├── worlds │ ├── building_editor_models │ │ └── stairs │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── earth.world │ ├── space.world │ └── stairs.world ├── CMakeLists.txt ├── package.xml ├── launch │ ├── a1.launch │ └── normal.launch └── plugin │ ├── draw_force_plugin.cc │ └── foot_contact_plugin.cc ├── robots ├── a1_description │ ├── .idea │ │ ├── a1_description.iml │ │ ├── misc.xml │ │ ├── libraries │ │ │ ├── workspace.xml │ │ │ └── ROS.xml │ │ ├── vcs.xml │ │ ├── .gitignore │ │ ├── modules.xml │ │ └── ros.xml │ ├── meshes │ │ └── trunk_A1.png │ ├── CMakeLists.txt │ ├── package.xml │ ├── config │ │ ├── a1_controller.yaml │ │ └── robot_control.yaml │ ├── launch │ │ └── a1_rviz.launch │ └── xacro │ │ ├── materials.xacro │ │ ├── stairs.xacro │ │ ├── transmission.xacro │ │ ├── const.xacro │ │ └── robot.xacro ├── aliengo_description │ ├── meshes │ │ └── trunk_uv_base_final.png │ ├── CMakeLists.txt │ ├── package.xml │ ├── launch │ │ ├── aliengo_rviz.launch │ │ └── check_joint.rviz │ ├── xacro │ │ ├── materials.xacro │ │ ├── stairs.xacro │ │ ├── transmission.xacro │ │ ├── const.xacro │ │ └── robot.xacro │ └── config │ │ └── robot_control.yaml ├── .idea │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── robots.iml └── laikago_description │ ├── CMakeLists.txt │ ├── package.xml │ ├── launch │ └── laikago_rviz.launch │ ├── xacro │ ├── materials.xacro │ ├── transmission.xacro │ ├── const.xacro │ └── robot.xacro │ └── config │ └── robot_control.yaml ├── song_ros_control ├── .idea │ ├── song_ros_control.iml │ ├── misc.xml │ ├── libraries │ │ ├── workspace.xml │ │ └── ROS.xml │ ├── vcs.xml │ ├── .gitignore │ ├── modules.xml │ └── ros.xml ├── song_controller_plugins.xml ├── song_wbc_stand_plugins.xml ├── include │ └── song_ros_control │ │ ├── A1Controller.h │ │ ├── wbc │ │ ├── OSC │ │ │ └── OscTrackingDataRaw.h │ │ ├── Utils.h │ │ └── controller │ │ │ └── OscStandController.h │ │ └── A1StandController.h ├── src │ ├── A1Controller.cpp │ ├── OscTrackingDataRaw.cpp │ └── A1StandController.cpp └── package.xml ├── unitree_legged_control ├── .idea │ ├── unitree_legged_control.iml │ ├── misc.xml │ ├── vcs.xml │ ├── .gitignore │ └── modules.xml ├── lib │ ├── libunitree_joint_control_tool.so │ └── libunitree_joint_control_tool_arm64.so ├── unitree_controller_plugins.xml ├── CMakeLists.txt ├── include │ ├── unitree_joint_control_tool.h │ └── joint_controller.h └── package.xml ├── .gitignore └── README.md /unitree_legged_msgs/msg/LED.msg: -------------------------------------------------------------------------------- 1 | uint8 r 2 | uint8 g 3 | uint8 b -------------------------------------------------------------------------------- /song_msgs/msg/MotorCmd.msg: -------------------------------------------------------------------------------- 1 | string[12] joint_name 2 | float64[12] tau -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/Cartesian.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z -------------------------------------------------------------------------------- /song_msgs/msg/MotorState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string[12] joint_name 3 | float64[12] q 4 | float64[12] dq 5 | float64[12] tau -------------------------------------------------------------------------------- /song_wbc/.idea/song_wbc.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/IMU.msg: -------------------------------------------------------------------------------- 1 | float32[4] quaternion 2 | float32[3] gyroscope 3 | float32[3] accelerometer 4 | int8 temperature -------------------------------------------------------------------------------- /song_msgs/.idea/song_msgs.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /song_gazebo/.idea/song_gazebo.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/unitree_gazebo.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/a1_description.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /robots/a1_description/meshes/trunk_A1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanliumaozhi/unitree-a1-sim/HEAD/robots/a1_description/meshes/trunk_A1.png -------------------------------------------------------------------------------- /song_ros_control/.idea/song_ros_control.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /unitree_legged_control/.idea/unitree_legged_control.iml: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /song_msgs/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /song_wbc/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /robots/aliengo_description/meshes/trunk_uv_base_final.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanliumaozhi/unitree-a1-sim/HEAD/robots/aliengo_description/meshes/trunk_uv_base_final.png -------------------------------------------------------------------------------- /song_gazebo/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /song_ros_control/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /unitree_legged_control/lib/libunitree_joint_control_tool.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanliumaozhi/unitree-a1-sim/HEAD/unitree_legged_control/lib/libunitree_joint_control_tool.so -------------------------------------------------------------------------------- /robots/a1_description/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /unitree_legged_control/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /unitree_legged_control/lib/libunitree_joint_control_tool_arm64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanliumaozhi/unitree-a1-sim/HEAD/unitree_legged_control/lib/libunitree_joint_control_tool_arm64.so -------------------------------------------------------------------------------- /song_msgs/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /song_wbc/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /song_gazebo/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /song_ros_control/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /robots/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_gazebo/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_msgs/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_wbc/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /robots/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /song_ros_control/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_wbc/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_gazebo/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /song_msgs/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /song_ros_control/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /unitree_legged_control/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /unitree_legged_control/.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /shelf/ 3 | /workspace.xml 4 | # Datasource local storage ignored files 5 | /dataSources/ 6 | /dataSources.local.xml 7 | # Editor-based HTTP Client requests 8 | /httpRequests/ 9 | -------------------------------------------------------------------------------- /song_gazebo/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /song_msgs/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /robots/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_wbc/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_msgs/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /robots/.idea/robots.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_gazebo/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_ros_control/song_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The a1 controller. 7 | 8 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_ros_control/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /song_ros_control/song_wbc_stand_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The a1 stand controller. 7 | 8 | -------------------------------------------------------------------------------- /unitree_gazebo/worlds/building_editor_models/stairs/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | stairs 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /unitree_legged_control/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/MotorCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 mode # motor target mode 2 | float32 q # motor target position 3 | float32 dq # motor target velocity 4 | float32 tau # motor target torque 5 | float32 Kp # motor spring stiffness coefficient 6 | float32 Kd # motor damper coefficient 7 | uint32[3] reserve # motor target torque -------------------------------------------------------------------------------- /robots/a1_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(a1_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | genmsg 6 | roscpp 7 | std_msgs 8 | tf 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS 13 | ) 14 | 15 | include_directories( 16 | # include 17 | ${Boost_INCLUDE_DIR} 18 | ${catkin_INCLUDE_DIRS} 19 | ) 20 | -------------------------------------------------------------------------------- /robots/aliengo_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aliengo_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | genmsg 6 | roscpp 7 | std_msgs 8 | tf 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS 13 | ) 14 | 15 | include_directories( 16 | # include 17 | ${Boost_INCLUDE_DIR} 18 | ${catkin_INCLUDE_DIRS} 19 | ) 20 | -------------------------------------------------------------------------------- /robots/laikago_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laikago_description) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | genmsg 6 | roscpp 7 | std_msgs 8 | tf 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS 13 | ) 14 | 15 | include_directories( 16 | # include 17 | ${Boost_INCLUDE_DIR} 18 | ${catkin_INCLUDE_DIRS} 19 | ) 20 | -------------------------------------------------------------------------------- /unitree_legged_control/unitree_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The unitree joint controller. 7 | 8 | 9 | -------------------------------------------------------------------------------- /robots/a1_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | a1_description 4 | 0.0.0 5 | The a1_description package 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | 14 | 15 | -------------------------------------------------------------------------------- /robots/laikago_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laikago_description 4 | 0.0.0 5 | The laikago_description package 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | -------------------------------------------------------------------------------- /robots/aliengo_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aliengo_description 4 | 0.0.0 5 | The aliengo_description package 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | 14 | 15 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/MotorState.msg: -------------------------------------------------------------------------------- 1 | uint8 mode # motor current mode 2 | float32 q # motor current position（rad） 3 | float32 dq # motor current speed（rad/s） 4 | float32 ddq # motor current speed（rad/s） 5 | float32 tauEst # current estimated output torque（N*m） 6 | float32 q_raw # motor current position（rad） 7 | float32 dq_raw # motor current speed（rad/s） 8 | float32 ddq_raw # motor current speed（rad/s） 9 | int8 temperature # motor temperature（slow conduction of temperature leads to lag） 10 | uint32[2] reserve -------------------------------------------------------------------------------- /unitree_gazebo/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /unitree_legged_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | unitree_legged_msgs 5 | 0.0.0 6 | 7 | The test messgaes package. 8 | 9 | unitree 10 | TODO 11 | 12 | catkin 13 | message_runtime 14 | message_generation 15 | std_msgs 16 | geometry_msgs 17 | sensor_msgs 18 | 19 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/LowCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 levelFlag 2 | uint16 commVersion # Old version Aliengo does not have 3 | uint16 robotID # Old version Aliengo does not have 4 | uint32 SN # Old version Aliengo does not have 5 | uint8 bandWidth # Old version Aliengo does not have 6 | MotorCmd[20] motorCmd 7 | LED[4] led 8 | uint8[40] wirelessRemote 9 | uint32 reserve # Old version Aliengo does not have 10 | uint32 crc 11 | 12 | Cartesian[4] ff # will delete # Old version Aliengo does not have -------------------------------------------------------------------------------- /song_gazebo/include/InitPosition.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/21. 3 | // 4 | 5 | #ifndef SONG_GAZEBO_INITPOSITION_H 6 | #define SONG_GAZEBO_INITPOSITION_H 7 | 8 | 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace gazebo { 18 | class InitPosition : public ModelPlugin { 19 | public: 20 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf_elem); 21 | 22 | private: 23 | physics::ModelPtr model_; 24 | std::map name_to_joint_map_; 25 | }; 26 | } 27 | 28 | #endif //SONG_GAZEBO_INITPOSITION_H 29 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/HighCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 levelFlag 2 | uint16 commVersion # Old version Aliengo does not have 3 | uint16 robotID # Old version Aliengo does not have 4 | uint32 SN # Old version Aliengo does not have 5 | uint8 bandWidth # Old version Aliengo does not have 6 | uint8 mode 7 | float32 forwardSpeed 8 | float32 sideSpeed 9 | float32 rotateSpeed 10 | float32 bodyHeight 11 | float32 footRaiseHeight 12 | float32 yaw 13 | float32 pitch 14 | float32 roll 15 | LED[4] led 16 | uint8[40] wirelessRemote 17 | uint8[40] AppRemote # Old version Aliengo does not have 18 | uint32 reserve # Old version Aliengo does not have 19 | int32 crc -------------------------------------------------------------------------------- /song_wbc/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /song_gazebo/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /song_ros_control/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /song_msgs/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /unitree_gazebo/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 12 | -------------------------------------------------------------------------------- /song_wbc/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /unitree_legged_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_legged_control) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | controller_interface 6 | hardware_interface 7 | pluginlib 8 | roscpp 9 | realtime_tools 10 | unitree_legged_msgs 11 | ) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS 15 | unitree_legged_msgs 16 | controller_interface 17 | hardware_interface 18 | pluginlib 19 | roscpp 20 | INCLUDE_DIRS include 21 | LIBRARIES ${PROJECT_NAME} 22 | ) 23 | 24 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) 25 | 26 | link_directories($(catkin_LIB_DIRS) lib) 27 | 28 | add_library( unitree_legged_control 29 | src/joint_controller.cpp 30 | ) 31 | add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp) 32 | target_link_libraries(unitree_legged_control ${catkin_LIBRARIES} unitree_joint_control_tool) 33 | -------------------------------------------------------------------------------- /unitree_legged_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_legged_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | MotorCmd.msg 14 | MotorState.msg 15 | Cartesian.msg 16 | IMU.msg 17 | LED.msg 18 | LowCmd.msg 19 | LowState.msg 20 | HighCmd.msg 21 | HighState.msg 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | geometry_msgs 28 | sensor_msgs 29 | ) 30 | 31 | catkin_package( 32 | CATKIN_DEPENDS 33 | message_runtime 34 | std_msgs 35 | geometry_msgs 36 | sensor_msgs 37 | ) 38 | 39 | ############# 40 | ## Install ## 41 | ############# 42 | 43 | # Mark topic names header files for installation 44 | install( 45 | DIRECTORY include/${PROJECT_NAME}/ 46 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 47 | FILES_MATCHING PATTERN "*.h" 48 | ) -------------------------------------------------------------------------------- /robots/a1_description/config/a1_controller.yaml: -------------------------------------------------------------------------------- 1 | a1_gazebo: 2 | A1_controller: 3 | type: song_ros_control/A1StandController 4 | joint_name_list: 5 | ["FR_hip_joint", "FL_hip_joint", "RR_hip_joint", "RL_hip_joint", "FR_thigh_joint", "FL_thigh_joint", "RR_thigh_joint", "RL_thigh_joint", "FR_calf_joint", "FL_calf_joint", "RR_calf_joint", "RL_calf_joint"] 6 | W_com: [2000, 0, 0, 7 | 0, 2000, 0, 8 | 0, 0, 500] 9 | W_pelvis: [200, 0, 0, 10 | 0, 200, 0, 11 | 0, 0, 200] 12 | K_p_com: [22, 0, 0, 13 | 0, 20, 0, 14 | 0, 0, 20] 15 | K_d_com: [0.5, 0, 0, 16 | 0, 0.4, 0, 17 | 0, 0, 1] 18 | K_p_pelvis: [10, 0, 0, 19 | 0, 100, 0, 20 | 0, 0, 10] 21 | K_d_pelvis: [0.5, 0, 0, 22 | 0, 10, 0, 23 | 0, 0, 10] -------------------------------------------------------------------------------- /robots/a1_description/launch/a1_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | -------------------------------------------------------------------------------- /robots/laikago_description/launch/laikago_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /robots/aliengo_description/launch/aliengo_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /robots/a1_description/xacro/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /robots/aliengo_description/xacro/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /robots/laikago_description/xacro/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /robots/a1_description/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/LowState.msg: -------------------------------------------------------------------------------- 1 | uint8 levelFlag 2 | uint16 commVersion # Old version Aliengo does not have 3 | uint16 robotID # Old version Aliengo does not have 4 | uint32 SN # Old version Aliengo does not have 5 | uint8 bandWidth # Old version Aliengo does not have 6 | IMU imu 7 | MotorState[20] motorState 8 | int16[4] footForce # force sensors # Old version Aliengo is different 9 | int16[4] footForceEst # force sensors # Old version Aliengo does not have 10 | uint32 tick # reference real-time from motion controller (unit: us) 11 | uint8[40] wirelessRemote # wireless commands 12 | uint32 reserve # Old version Aliengo does not have 13 | uint32 crc 14 | 15 | # Old version Aliengo does not have: 16 | Cartesian[4] eeForceRaw 17 | Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization 18 | Cartesian position # will delete 19 | Cartesian velocity # will delete 20 | Cartesian velocity_w # will delete 21 | -------------------------------------------------------------------------------- /song_ros_control/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /unitree_legged_control/include/unitree_joint_control_tool.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | // #ifndef _UNITREE_JOINT_CONTROL_TOOL_H_ 7 | // #define _UNITREE_JOINT_CONTROL_TOOL_H_ 8 | #ifndef _LAIKAGO_CONTROL_TOOL_H_ 9 | #define _LAIKAGO_CONTROL_TOOL_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define posStopF (2.146E+9f) // stop position control mode 17 | #define velStopF (16000.0f) // stop velocity control mode 18 | 19 | typedef struct 20 | { 21 | uint8_t mode; 22 | double pos; 23 | double posStiffness; 24 | double vel; 25 | double velStiffness; 26 | double torque; 27 | } ServoCmd; 28 | 29 | double clamp(double&, double, double); // eg. clamp(1.5, -1, 1) = 1 30 | double computeVel(double current_position, double last_position, double last_velocity, double duration); // get current velocity 31 | double computeTorque(double current_position, double current_velocity, ServoCmd&); // get torque 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/HighState.msg: -------------------------------------------------------------------------------- 1 | uint8 levelFlag 2 | uint16 commVersion # Old version Aliengo does not have 3 | uint16 robotID # Old version Aliengo does not have 4 | uint32 SN # Old version Aliengo does not have 5 | uint8 bandWidth # Old version Aliengo does not have 6 | uint8 mode 7 | IMU imu 8 | float32 forwardSpeed 9 | float32 sideSpeed 10 | float32 rotateSpeed 11 | float32 bodyHeight 12 | float32 updownSpeed 13 | float32 forwardPosition # (will be float type next version) # Old version Aliengo is different 14 | float32 sidePosition # (will be float type next version) # Old version Aliengo is different 15 | Cartesian[4] footPosition2Body 16 | Cartesian[4] footSpeed2Body 17 | int16[4] footForce # Old version Aliengo is different 18 | int16[4] footForceEst # Old version Aliengo does not have 19 | uint32 tick 20 | uint8[40] wirelessRemote 21 | uint32 reserve # Old version Aliengo does not have 22 | uint32 crc 23 | 24 | # Under are not defined in SDK yet. # Old version Aliengo does not have 25 | Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization. 26 | float32[12] jointP # for visualization -------------------------------------------------------------------------------- /unitree_legged_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_legged_control 4 | 0.0.0 5 | The unitree_legged_control package 6 | unitree 7 | TODO 8 | catkin 9 | controller_interface 10 | hardware_interface 11 | pluginlib 12 | roscpp 13 | controller_interface 14 | hardware_interface 15 | pluginlib 16 | roscpp 17 | controller_interface 18 | hardware_interface 19 | pluginlib 20 | roscpp 21 | unitree_legged_msgs 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /unitree_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_gazebo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | controller_manager 6 | genmsg 7 | joint_state_controller 8 | robot_state_publisher 9 | roscpp 10 | gazebo_ros 11 | std_msgs 12 | tf 13 | geometry_msgs 14 | unitree_legged_msgs 15 | ) 16 | 17 | find_package(gazebo REQUIRED) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS 21 | unitree_legged_msgs 22 | ) 23 | 24 | include_directories( 25 | # include 26 | ${Boost_INCLUDE_DIR} 27 | ${catkin_INCLUDE_DIRS} 28 | ${GAZEBO_INCLUDE_DIRS} 29 | ) 30 | 31 | link_directories(${GAZEBO_LIBRARY_DIRS}) 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 33 | 34 | # # Declare a C++ library 35 | # add_library(${PROJECT_NAME} 36 | # src/body.cpp 37 | # ) 38 | 39 | # add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp) 40 | 41 | # target_link_libraries(${PROJECT_NAME} 42 | # ${catkin_LIBRARIES} ${EXTRA_LIBS} 43 | # ) 44 | 45 | add_library(unitreeFootContactPlugin SHARED plugin/foot_contact_plugin.cc) 46 | target_link_libraries(unitreeFootContactPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 47 | 48 | add_library(unitreeDrawForcePlugin SHARED plugin/draw_force_plugin.cc) 49 | target_link_libraries(unitreeDrawForcePlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 50 | 51 | # add_executable(unitree_external_force src/exe/external_force.cpp) 52 | # target_link_libraries(unitree_external_force ${catkin_LIBRARIES}) 53 | 54 | # add_executable(unitree_servo src/exe/servo.cpp) 55 | # target_link_libraries(unitree_servo ${PROJECT_NAME} ${catkin_LIBRARIES}) 56 | 57 | 58 | -------------------------------------------------------------------------------- /unitree_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_gazebo 4 | 0.0.0 5 | The unitree_gazebo package. It can start a gazebo simulation 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | genmsg 12 | controller_manager 13 | joint_state_controller 14 | robot_state_publisher 15 | roscpp 16 | std_msgs 17 | controller_manager 18 | joint_state_controller 19 | robot_state_publisher 20 | roscpp 21 | std_msgs 22 | controller_manager 23 | joint_state_controller 24 | robot_state_publisher 25 | roscpp 26 | std_msgs 27 | gazebo_ros 28 | unitree_legged_msgs 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /song_ros_control/include/song_ros_control/A1Controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/20. 3 | // 4 | 5 | #ifndef SONG_ROS_CONTROL_A1CONTROLLER_H 6 | #define SONG_ROS_CONTROL_A1CONTROLLER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | class A1Controller: public controller_interface::Controller 23 | { 24 | public: 25 | A1Controller(); 26 | ~A1Controller() override; 27 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) override; 28 | void starting(const ros::Time& time) override; 29 | void update(const ros::Time& time, const ros::Duration& period) override; 30 | virtual void stopping(){}; 31 | 32 | void cmd_cb(const song_msgs::MotorCmdConstPtr& msg); 33 | 34 | private: 35 | std::string name_space; 36 | 37 | song_msgs::MotorCmd last_cmd_; 38 | sensor_msgs::JointState last_state_; 39 | 40 | ros::Subscriber sub_cmd_; 41 | 42 | std::unique_ptr> robot_status_pub_; 43 | std::vector joint_name_list_; 44 | std::vector joint_list_; 45 | std::map joint_name_to_index_; 46 | 47 | realtime_tools::RealtimeBuffer command_; 48 | 49 | }; 50 | 51 | #endif //SONG_ROS_CONTROL_A1CONTROLLER_H 52 | -------------------------------------------------------------------------------- /robots/a1_description/xacro/stairs.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /robots/aliengo_description/xacro/stairs.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 2 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 3 | 4 | # User-specific stuff 5 | .idea/**/workspace.xml 6 | .idea/**/tasks.xml 7 | .idea/**/usage.statistics.xml 8 | .idea/**/dictionaries 9 | .idea/**/shelf 10 | 11 | # Generated files 12 | .idea/**/contentModel.xml 13 | 14 | # Sensitive or high-churn files 15 | .idea/**/dataSources/ 16 | .idea/**/dataSources.ids 17 | .idea/**/dataSources.local.xml 18 | .idea/**/sqlDataSources.xml 19 | .idea/**/dynamic.xml 20 | .idea/**/uiDesigner.xml 21 | .idea/**/dbnavigator.xml 22 | 23 | # Gradle 24 | .idea/**/gradle.xml 25 | .idea/**/libraries 26 | 27 | # Gradle and Maven with auto-import 28 | # When using Gradle or Maven with auto-import, you should exclude module files, 29 | # since they will be recreated, and may cause churn. Uncomment if using 30 | # auto-import. 31 | # .idea/artifacts 32 | # .idea/compiler.xml 33 | # .idea/jarRepositories.xml 34 | # .idea/modules.xml 35 | # .idea/*.iml 36 | # .idea/modules 37 | # *.iml 38 | # *.ipr 39 | 40 | # CMake 41 | cmake-build-*/ 42 | 43 | # Mongo Explorer plugin 44 | .idea/**/mongoSettings.xml 45 | 46 | # File-based project format 47 | *.iws 48 | 49 | # IntelliJ 50 | out/ 51 | 52 | # mpeltonen/sbt-idea plugin 53 | .idea_modules/ 54 | 55 | # JIRA plugin 56 | atlassian-ide-plugin.xml 57 | 58 | # Cursive Clojure plugin 59 | .idea/replstate.xml 60 | 61 | # Crashlytics plugin (for Android Studio and IntelliJ) 62 | com_crashlytics_export_strings.xml 63 | crashlytics.properties 64 | crashlytics-build.properties 65 | fabric.properties 66 | 67 | # Editor-based Rest Client 68 | .idea/httpRequests 69 | 70 | # Android studio 3.1+ serialized cache file 71 | .idea/caches/build_file_checksums.ser 72 | -------------------------------------------------------------------------------- /song_wbc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(song_wbc) 3 | 4 | # for drake 5 | option(RUN_X11_TESTS "Run tests that require X11" OFF) 6 | 7 | if(APPLE) 8 | set(FIND_PYTHON_EXECUTABLE_PATHS /usr/local/opt/python@3.8/bin) 9 | set(FIND_PYTHON_INTERP_VERSION 3.8) 10 | else() 11 | set(FIND_PYTHON_EXECUTABLE_PATHS /usr/bin) 12 | set(FIND_PYTHON_INTERP_VERSION 3.8) 13 | endif() 14 | find_program(PYTHON_EXECUTABLE NAMES python3 15 | PATHS "${FIND_PYTHON_EXECUTABLE_PATHS}" 16 | NO_DEFAULT_PATH 17 | ) 18 | 19 | find_package(PythonInterp ${FIND_PYTHON_INTERP_VERSION} MODULE REQUIRED) 20 | 21 | execute_process(COMMAND ${PYTHON_EXECUTABLE}-config --exec-prefix 22 | OUTPUT_VARIABLE PYTHON_EXEC_PREFIX 23 | OUTPUT_STRIP_TRAILING_WHITESPACE 24 | ) 25 | set(CMAKE_PREFIX_PATH "/opt/drake") 26 | list(APPEND CMAKE_PREFIX_PATH "${PYTHON_EXEC_PREFIX}") 27 | find_package(PythonLibs ${FIND_PYTHON_INTERP_VERSION} MODULE REQUIRED) 28 | 29 | find_package(drake CONFIG REQUIRED) 30 | 31 | get_filename_component(PYTHONPATH 32 | "${drake_DIR}/../../python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages" 33 | REALPATH 34 | ) 35 | 36 | find_package(catkin REQUIRED COMPONENTS 37 | roscpp 38 | song_msgs 39 | message_filters 40 | roslib 41 | ) 42 | 43 | set(CMAKE_CXX_EXTENSIONS OFF) 44 | set(CMAKE_CXX_STANDARD 17) 45 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 46 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 47 | 48 | 49 | catkin_package() 50 | 51 | include_directories( 52 | include 53 | ${catkin_INCLUDE_DIRS} 54 | ) 55 | 56 | add_executable(wbc_stand_node src/WbcStand.cpp src/Utils.cpp src/OscTrackingData.cpp src/OscStandController.cpp) 57 | 58 | target_link_libraries(wbc_stand_node ${catkin_LIBRARIES} drake::drake) 59 | -------------------------------------------------------------------------------- /song_ros_control/include/song_ros_control/wbc/OSC/OscTrackingDataRaw.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/25. 3 | // 4 | 5 | #ifndef SONG_ROS_CONTROL_OSCTRACKINGDATARAW_H 6 | #define SONG_ROS_CONTROL_OSCTRACKINGDATARAW_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | class ComTrackingDataRaw 14 | { 15 | public: 16 | ComTrackingDataRaw(const std::string& name, const Eigen::MatrixXd& K_p, 17 | const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, 18 | const drake::multibody::MultibodyPlant& plant); 19 | 20 | 21 | bool Update(const Eigen::VectorXd& x, 22 | const drake::systems::Context& context, 23 | const Eigen::Vector3d& position_d); 24 | 25 | void PrintFeedbackAndDesiredValues(const Eigen::VectorXd& dv); 26 | 27 | void SaveYddotCommandSol(const Eigen::VectorXd& dv); 28 | std::string name_; 29 | 30 | Eigen::MatrixXd K_p_; 31 | Eigen::MatrixXd K_d_; 32 | Eigen::MatrixXd W_; 33 | 34 | Eigen::MatrixXd J_; 35 | Eigen::VectorXd JdotV_; 36 | Eigen::Vector3d yddot_command_; 37 | 38 | Eigen::Vector3d error_y_; 39 | Eigen::Vector3d error_ydot_; 40 | Eigen::Vector3d y_; 41 | Eigen::Vector3d ydot_; 42 | 43 | // Desired output 44 | Eigen::Vector3d y_des_; 45 | Eigen::Vector3d ydot_des_; 46 | Eigen::Vector3d yddot_des_; 47 | Eigen::Vector3d yddot_des_converted_; 48 | 49 | // Commanded acceleration after feedback terms 50 | 51 | // Osc solution 52 | Eigen::VectorXd yddot_command_sol_; 53 | 54 | const drake::multibody::MultibodyPlant& plant_; 55 | 56 | // World frames 57 | const drake::multibody::BodyFrame& world_; 58 | }; 59 | 60 | 61 | #endif //SONG_ROS_CONTROL_OSCTRACKINGDATARAW_H 62 | -------------------------------------------------------------------------------- /robots/a1_description/xacro/transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | transmission_interface/SimpleTransmission 9 | 10 | hardware_interface/EffortJointInterface 11 | 12 | 13 | hardware_interface/EffortJointInterface 14 | 1 15 | 16 | 17 | 18 | 19 | transmission_interface/SimpleTransmission 20 | 21 | hardware_interface/EffortJointInterface 22 | 23 | 24 | hardware_interface/EffortJointInterface 25 | 1 26 | 27 | 28 | 29 | 30 | transmission_interface/SimpleTransmission 31 | 32 | hardware_interface/EffortJointInterface 33 | 34 | 35 | hardware_interface/EffortJointInterface 36 | 1 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robots/aliengo_description/xacro/transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | transmission_interface/SimpleTransmission 9 | 10 | hardware_interface/EffortJointInterface 11 | 12 | 13 | hardware_interface/EffortJointInterface 14 | 1 15 | 16 | 17 | 18 | 19 | transmission_interface/SimpleTransmission 20 | 21 | hardware_interface/EffortJointInterface 22 | 23 | 24 | hardware_interface/EffortJointInterface 25 | 1 26 | 27 | 28 | 29 | 30 | transmission_interface/SimpleTransmission 31 | 32 | hardware_interface/EffortJointInterface 33 | 34 | 35 | hardware_interface/EffortJointInterface 36 | 1 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robots/laikago_description/xacro/transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | transmission_interface/SimpleTransmission 9 | 10 | hardware_interface/EffortJointInterface 11 | 12 | 13 | hardware_interface/EffortJointInterface 14 | 1 15 | 16 | 17 | 18 | 19 | transmission_interface/SimpleTransmission 20 | 21 | hardware_interface/EffortJointInterface 22 | 23 | 24 | hardware_interface/EffortJointInterface 25 | 1 26 | 27 | 28 | 29 | 30 | transmission_interface/SimpleTransmission 31 | 32 | hardware_interface/EffortJointInterface 33 | 34 | 35 | hardware_interface/EffortJointInterface 36 | 1 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /song_gazebo/InitPosition.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/21. 3 | // 4 | 5 | #include "InitPosition.h" 6 | 7 | #include 8 | 9 | using ignition::math::Vector3d; 10 | using ignition::math::Quaterniond; 11 | 12 | 13 | namespace gazebo { 14 | 15 | void InitPosition::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf_elem) { 16 | model_ = _parent; 17 | 18 | Vector3d position = Vector3d(0, 0, 0.3); 19 | Quaterniond quat = Quaterniond(1, 0, 0, 0); 20 | model_->SetLinkWorldPose(ignition::math::Pose3d(position, quat),"base"); 21 | 22 | std::array joint_list; 23 | joint_list = { 24 | model_->GetJoint("FR_hip_joint"), 25 | model_->GetJoint("FL_hip_joint"), 26 | model_->GetJoint("RR_hip_joint"), 27 | model_->GetJoint("RL_hip_joint"), 28 | model_->GetJoint("FR_thigh_joint"), 29 | model_->GetJoint("FL_thigh_joint"), 30 | model_->GetJoint("RR_thigh_joint"), 31 | model_->GetJoint("RL_thigh_joint"), 32 | model_->GetJoint("FR_calf_joint"), 33 | model_->GetJoint("FL_calf_joint"), 34 | model_->GetJoint("RR_calf_joint"), 35 | model_->GetJoint("RL_calf_joint") 36 | }; 37 | 38 | std::array joint_positions = { 39 | -0.0263951, 40 | 0.0263951, 41 | -0.0263951, 42 | 0.0263951, 43 | 0.814243, 44 | 0.814243, 45 | 0.814243, 46 | 0.814243, 47 | -1.53652, 48 | -1.53652, 49 | -1.53652, 50 | -1.53652 51 | 52 | }; 53 | 54 | for (int i = 0; i < 12; ++i) { 55 | joint_list[i]->SetPosition(0, joint_positions[i]); 56 | joint_list[i]->SetVelocity(0, 0); 57 | } 58 | } 59 | 60 | GZ_REGISTER_MODEL_PLUGIN(InitPosition) 61 | } 62 | -------------------------------------------------------------------------------- /unitree_gazebo/launch/a1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | -------------------------------------------------------------------------------- /unitree_gazebo/worlds/earth.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.001 7 | 1 8 | 1000 9 | 0 0 -9.81 10 | 11 | 12 | quick 13 | 50 14 | 1.3 15 | 16 | 17 | 0.0 18 | 0.2 19 | 10.0 20 | 0.001 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 12 29 | 30 | 31 | 32 | 33 | 34 | model://sun 35 | 36 | 37 | 38 | model://ground_plane 39 | 40 | 41 | 42 | true 43 | 44 | -2 2 0.5 0 0 0 45 | 46 | 47 | 48 | 1 1 1 49 | 50 | 51 | 52 | 53 | 54 | 55 | 1 1 1 56 | 57 | 58 | 59 | 0.2 0.2 0.2 1.0 60 | .421 0.225 0.0 1.0 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /unitree_gazebo/worlds/space.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.001 7 | 1 8 | 1000 9 | 0 0 0 10 | 11 | 12 | quick 13 | 50 14 | 1.3 15 | 16 | 17 | 0.0 18 | 0.2 19 | 10.0 20 | 0.001 21 | 22 | 23 | 24 | 25 | 26 | 31 | 32 | 33 | 34 | model://sun 35 | 36 | 37 | 38 | model://ground_plane 39 | 40 | 41 | 42 | true 43 | 44 | -2 2 0.5 0 0 0 45 | 46 | 47 | 48 | 1 1 1 49 | 50 | 51 | 52 | 53 | 54 | 55 | 1 1 1 56 | 57 | 58 | 59 | 0.2 0.2 0.2 1.0 60 | .421 0.225 0.0 1.0 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /robots/a1_description/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | a1_gazebo: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 1000 6 | 7 | # FL Controllers --------------------------------------- 8 | FL_hip_controller: 9 | type: unitree_legged_control/UnitreeJointController 10 | joint: FL_hip_joint 11 | pid: {p: 100.0, i: 0.0, d: 5.0} 12 | 13 | FL_thigh_controller: 14 | type: unitree_legged_control/UnitreeJointController 15 | joint: FL_thigh_joint 16 | pid: {p: 300.0, i: 0.0, d: 8.0} 17 | 18 | FL_calf_controller: 19 | type: unitree_legged_control/UnitreeJointController 20 | joint: FL_calf_joint 21 | pid: {p: 300.0, i: 0.0, d: 8.0} 22 | 23 | # FR Controllers --------------------------------------- 24 | FR_hip_controller: 25 | type: unitree_legged_control/UnitreeJointController 26 | joint: FR_hip_joint 27 | pid: {p: 100.0, i: 0.0, d: 5.0} 28 | 29 | FR_thigh_controller: 30 | type: unitree_legged_control/UnitreeJointController 31 | joint: FR_thigh_joint 32 | pid: {p: 300.0, i: 0.0, d: 8.0} 33 | 34 | FR_calf_controller: 35 | type: unitree_legged_control/UnitreeJointController 36 | joint: FR_calf_joint 37 | pid: {p: 300.0, i: 0.0, d: 8.0} 38 | 39 | # RL Controllers --------------------------------------- 40 | RL_hip_controller: 41 | type: unitree_legged_control/UnitreeJointController 42 | joint: RL_hip_joint 43 | pid: {p: 100.0, i: 0.0, d: 5.0} 44 | 45 | RL_thigh_controller: 46 | type: unitree_legged_control/UnitreeJointController 47 | joint: RL_thigh_joint 48 | pid: {p: 300.0, i: 0.0, d: 8.0} 49 | 50 | RL_calf_controller: 51 | type: unitree_legged_control/UnitreeJointController 52 | joint: RL_calf_joint 53 | pid: {p: 300.0, i: 0.0, d: 8.0} 54 | 55 | # RR Controllers --------------------------------------- 56 | RR_hip_controller: 57 | type: unitree_legged_control/UnitreeJointController 58 | joint: RR_hip_joint 59 | pid: {p: 100.0, i: 0.0, d: 5.0} 60 | 61 | RR_thigh_controller: 62 | type: unitree_legged_control/UnitreeJointController 63 | joint: RR_thigh_joint 64 | pid: {p: 300.0, i: 0.0, d: 8.0} 65 | 66 | RR_calf_controller: 67 | type: unitree_legged_control/UnitreeJointController 68 | joint: RR_calf_joint 69 | pid: {p: 300.0, i: 0.0, d: 8.0} 70 | 71 | -------------------------------------------------------------------------------- /robots/aliengo_description/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | aliengo_gazebo: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 1000 6 | 7 | # FL Controllers --------------------------------------- 8 | FL_hip_controller: 9 | type: unitree_legged_control/UnitreeJointController 10 | joint: FL_hip_joint 11 | pid: {p: 100.0, i: 0.0, d: 5.0} 12 | 13 | FL_thigh_controller: 14 | type: unitree_legged_control/UnitreeJointController 15 | joint: FL_thigh_joint 16 | pid: {p: 300.0, i: 0.0, d: 8.0} 17 | 18 | FL_calf_controller: 19 | type: unitree_legged_control/UnitreeJointController 20 | joint: FL_calf_joint 21 | pid: {p: 300.0, i: 0.0, d: 8.0} 22 | 23 | # FR Controllers --------------------------------------- 24 | FR_hip_controller: 25 | type: unitree_legged_control/UnitreeJointController 26 | joint: FR_hip_joint 27 | pid: {p: 100.0, i: 0.0, d: 5.0} 28 | 29 | FR_thigh_controller: 30 | type: unitree_legged_control/UnitreeJointController 31 | joint: FR_thigh_joint 32 | pid: {p: 300.0, i: 0.0, d: 8.0} 33 | 34 | FR_calf_controller: 35 | type: unitree_legged_control/UnitreeJointController 36 | joint: FR_calf_joint 37 | pid: {p: 300.0, i: 0.0, d: 8.0} 38 | 39 | # RL Controllers --------------------------------------- 40 | RL_hip_controller: 41 | type: unitree_legged_control/UnitreeJointController 42 | joint: RL_hip_joint 43 | pid: {p: 100.0, i: 0.0, d: 5.0} 44 | 45 | RL_thigh_controller: 46 | type: unitree_legged_control/UnitreeJointController 47 | joint: RL_thigh_joint 48 | pid: {p: 300.0, i: 0.0, d: 8.0} 49 | 50 | RL_calf_controller: 51 | type: unitree_legged_control/UnitreeJointController 52 | joint: RL_calf_joint 53 | pid: {p: 300.0, i: 0.0, d: 8.0} 54 | 55 | # RR Controllers --------------------------------------- 56 | RR_hip_controller: 57 | type: unitree_legged_control/UnitreeJointController 58 | joint: RR_hip_joint 59 | pid: {p: 100.0, i: 0.0, d: 5.0} 60 | 61 | RR_thigh_controller: 62 | type: unitree_legged_control/UnitreeJointController 63 | joint: RR_thigh_joint 64 | pid: {p: 300.0, i: 0.0, d: 8.0} 65 | 66 | RR_calf_controller: 67 | type: unitree_legged_control/UnitreeJointController 68 | joint: RR_calf_joint 69 | pid: {p: 300.0, i: 0.0, d: 8.0} 70 | 71 | -------------------------------------------------------------------------------- /robots/laikago_description/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | laikago_gazebo: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 1000 6 | 7 | # FL Controllers --------------------------------------- 8 | FL_hip_controller: 9 | type: unitree_legged_control/UnitreeJointController 10 | joint: FL_hip_joint 11 | pid: {p: 100.0, i: 0.0, d: 5.0} 12 | 13 | FL_thigh_controller: 14 | type: unitree_legged_control/UnitreeJointController 15 | joint: FL_thigh_joint 16 | pid: {p: 300.0, i: 0.0, d: 8.0} 17 | 18 | FL_calf_controller: 19 | type: unitree_legged_control/UnitreeJointController 20 | joint: FL_calf_joint 21 | pid: {p: 300.0, i: 0.0, d: 8.0} 22 | 23 | # FR Controllers --------------------------------------- 24 | FR_hip_controller: 25 | type: unitree_legged_control/UnitreeJointController 26 | joint: FR_hip_joint 27 | pid: {p: 100.0, i: 0.0, d: 5.0} 28 | 29 | FR_thigh_controller: 30 | type: unitree_legged_control/UnitreeJointController 31 | joint: FR_thigh_joint 32 | pid: {p: 300.0, i: 0.0, d: 8.0} 33 | 34 | FR_calf_controller: 35 | type: unitree_legged_control/UnitreeJointController 36 | joint: FR_calf_joint 37 | pid: {p: 300.0, i: 0.0, d: 8.0} 38 | 39 | # RL Controllers --------------------------------------- 40 | RL_hip_controller: 41 | type: unitree_legged_control/UnitreeJointController 42 | joint: RL_hip_joint 43 | pid: {p: 100.0, i: 0.0, d: 5.0} 44 | 45 | RL_thigh_controller: 46 | type: unitree_legged_control/UnitreeJointController 47 | joint: RL_thigh_joint 48 | pid: {p: 300.0, i: 0.0, d: 8.0} 49 | 50 | RL_calf_controller: 51 | type: unitree_legged_control/UnitreeJointController 52 | joint: RL_calf_joint 53 | pid: {p: 300.0, i: 0.0, d: 8.0} 54 | 55 | # RR Controllers --------------------------------------- 56 | RR_hip_controller: 57 | type: unitree_legged_control/UnitreeJointController 58 | joint: RR_hip_joint 59 | pid: {p: 100.0, i: 0.0, d: 5.0} 60 | 61 | RR_thigh_controller: 62 | type: unitree_legged_control/UnitreeJointController 63 | joint: RR_thigh_joint 64 | pid: {p: 300.0, i: 0.0, d: 8.0} 65 | 66 | RR_calf_controller: 67 | type: unitree_legged_control/UnitreeJointController 68 | joint: RR_calf_joint 69 | pid: {p: 300.0, i: 0.0, d: 8.0} 70 | 71 | -------------------------------------------------------------------------------- /unitree_gazebo/launch/normal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 44 | 45 | 46 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /song_wbc/include/song_wbc/Utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/22. 3 | // 4 | 5 | #ifndef SONG_WBC_UTILS_H 6 | #define SONG_WBC_UTILS_H 7 | 8 | #include 9 | #include 10 | 11 | std::pair&> 12 | LeftToeFront(const drake::multibody::MultibodyPlant& plant); 13 | 14 | std::pair&> 15 | RightToeFront(const drake::multibody::MultibodyPlant& plant); 16 | 17 | std::pair&> 18 | LeftToeRear(const drake::multibody::MultibodyPlant& plant); 19 | 20 | std::pair&> 21 | RightToeRear(const drake::multibody::MultibodyPlant& plant); 22 | 23 | void addA1Multibody( 24 | drake::multibody::MultibodyPlant* plant, 25 | std::string filename, 26 | bool floating_base = true); 27 | 28 | using ContactData = std::pair&>; 29 | 30 | void SetPositionsIfNew(const drake::multibody::MultibodyPlant& plant, 31 | const Eigen::Ref>& q, 32 | drake::systems::Context* context); 33 | 34 | /// Update an existing MultibodyPlant context, setting corresponding velocities. 35 | /// Will only set if value if changed from current value. 36 | void SetVelocitiesIfNew(const drake::multibody::MultibodyPlant& plant, 37 | const Eigen::Ref>& f, 38 | drake::systems::Context* context); 39 | 40 | 41 | /// Given a MultiBodyTree, builds a map from position name to position index 42 | std::map makeNameToPositionsMap( 43 | const drake::multibody::MultibodyPlant& plant); 44 | 45 | /// Given a MultiBodyTree, builds a map from velocity name to velocity index 46 | std::map makeNameToVelocitiesMap( 47 | const drake::multibody::MultibodyPlant& plant); 48 | 49 | /// Given a MultiBodyTree, builds a map from actuator name to actuator index 50 | std::map makeNameToActuatorsMap( 51 | const drake::multibody::MultibodyPlant& plant); 52 | 53 | Eigen::MatrixXd get_contact_jacobin(ContactData contact, const drake::multibody::MultibodyPlant& plant, const drake::systems::Context& context); 54 | 55 | Eigen::MatrixXd get_contact_jacobinDotTimesV(ContactData contact, const drake::multibody::MultibodyPlant& plant, const drake::systems::Context& context); 56 | #endif //SONG_WBC_UTILS_H 57 | -------------------------------------------------------------------------------- /song_ros_control/include/song_ros_control/A1StandController.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/24. 3 | // 4 | 5 | #ifndef SONG_ROS_CONTROL_A1STANDCONTROLLER_H 6 | #define SONG_ROS_CONTROL_A1STANDCONTROLLER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | //#include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | #include "song_ros_control/wbc/controller/OscStandController.h" 24 | 25 | #include "drake/multibody/plant/multibody_plant.h" 26 | 27 | 28 | 29 | class A1StandController: public controller_interface::Controller 30 | { 31 | public: 32 | A1StandController(); 33 | ~A1StandController() override; 34 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) override; 35 | void starting(const ros::Time& time) override; 36 | void update(const ros::Time& time, const ros::Duration& period) override; 37 | virtual void stopping(){}; 38 | 39 | void inertial_cb(const nav_msgs::OdometryConstPtr& msg); 40 | 41 | private: 42 | std::string name_space; 43 | 44 | nav_msgs::Odometry global_state_; 45 | song_msgs::MotorState last_state_; 46 | 47 | nav_msgs::Odometry global_state__; 48 | 49 | ros::Subscriber sub_cmd_; 50 | 51 | std::unique_ptr> robot_status_pub_; 52 | std::vector joint_name_list_; 53 | std::vector joint_list_; 54 | std::map joint_name_to_index_; 55 | 56 | std::vector output_order_; 57 | 58 | realtime_tools::RealtimeBuffer command_; 59 | 60 | std::vector w_com_; 61 | Eigen::Matrix3d W_com_; 62 | 63 | std::vector k_p_com_; 64 | Eigen::Matrix3d K_p_com_; 65 | 66 | std::vector k_d_com_; 67 | Eigen::Matrix3d K_d_com_; 68 | 69 | std::vector w_pelvis_; 70 | Eigen::Matrix3d W_pelvis_; 71 | 72 | std::vector k_p_pelvis_; 73 | Eigen::Matrix3d K_p_pelvis_; 74 | 75 | std::vector k_d_pelvis_; 76 | Eigen::Matrix3d K_d_pelvis_; 77 | 78 | std::shared_ptr osc_; 79 | std::shared_ptr> plant_; 80 | std::unique_ptr> context_; 81 | }; 82 | 83 | 84 | #endif //SONG_ROS_CONTROL_A1STANDCONTROLLER_H 85 | -------------------------------------------------------------------------------- /song_ros_control/include/song_ros_control/wbc/Utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/22. 3 | // 4 | 5 | #ifndef SONG_WBC_UTILS_H 6 | #define SONG_WBC_UTILS_H 7 | 8 | #include 9 | #include 10 | 11 | std::pair&> 12 | LeftToeFront(const drake::multibody::MultibodyPlant& plant); 13 | 14 | std::pair&> 15 | RightToeFront(const drake::multibody::MultibodyPlant& plant); 16 | 17 | std::pair&> 18 | LeftToeRear(const drake::multibody::MultibodyPlant& plant); 19 | 20 | std::pair&> 21 | RightToeRear(const drake::multibody::MultibodyPlant& plant); 22 | 23 | void addA1Multibody( 24 | drake::multibody::MultibodyPlant* plant, 25 | std::string filename, 26 | bool floating_base = true); 27 | 28 | using ContactData = std::pair&>; 29 | 30 | void SetPositionsIfNew(const drake::multibody::MultibodyPlant& plant, 31 | const Eigen::Ref>& q, 32 | drake::systems::Context* context); 33 | 34 | /// Update an existing MultibodyPlant context, setting corresponding velocities. 35 | /// Will only set if value if changed from current value. 36 | void SetVelocitiesIfNew(const drake::multibody::MultibodyPlant& plant, 37 | const Eigen::Ref>& f, 38 | drake::systems::Context* context); 39 | 40 | 41 | /// Given a MultiBodyTree, builds a map from position name to position index 42 | std::map makeNameToPositionsMap( 43 | const drake::multibody::MultibodyPlant& plant); 44 | 45 | /// Given a MultiBodyTree, builds a map from velocity name to velocity index 46 | std::map makeNameToVelocitiesMap( 47 | const drake::multibody::MultibodyPlant& plant); 48 | 49 | /// Given a MultiBodyTree, builds a map from actuator name to actuator index 50 | std::map makeNameToActuatorsMap( 51 | const drake::multibody::MultibodyPlant& plant); 52 | 53 | Eigen::MatrixXd get_contact_jacobin(ContactData contact, const drake::multibody::MultibodyPlant& plant, const drake::systems::Context& context); 54 | 55 | Eigen::MatrixXd get_contact_jacobinDotTimesV(ContactData contact, const drake::multibody::MultibodyPlant& plant, const drake::systems::Context& context); 56 | #endif //SONG_WBC_UTILS_H 57 | -------------------------------------------------------------------------------- /song_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | song_gazebo 4 | 0.0.0 5 | The song_gazebo package 6 | 7 | 8 | 9 | 10 | han 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | gazebo_dev 53 | gazebo_dev 54 | gazebo_dev 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /song_wbc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | song_wbc 4 | 0.0.1 5 | The song_wbc package 6 | 7 | 8 | 9 | 10 | Song Liu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | song_msgs 54 | message_filters 55 | roslib 56 | roscpp 57 | song_msgs 58 | message_filters 59 | roslib 60 | roscpp 61 | song_msgs 62 | message_filters 63 | roslib 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /unitree_legged_control/include/joint_controller.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_ROS_JOINT_CONTROLLER_H_ 7 | #define _UNITREE_ROS_JOINT_CONTROLLER_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "unitree_legged_msgs/MotorCmd.h" 22 | #include "unitree_legged_msgs/MotorState.h" 23 | #include 24 | #include "unitree_joint_control_tool.h" 25 | 26 | #define PMSM (0x0A) 27 | #define BRAKE (0x00) 28 | #define PosStopF (2.146E+9f) 29 | #define VelStopF (16000.0f) 30 | 31 | namespace unitree_legged_control 32 | { 33 | class UnitreeJointController: public controller_interface::Controller 34 | { 35 | private: 36 | hardware_interface::JointHandle joint; 37 | ros::Subscriber sub_cmd, sub_ft; 38 | // ros::Publisher pub_state; 39 | control_toolbox::Pid pid_controller_; 40 | boost::scoped_ptr > controller_state_publisher_ ; 41 | 42 | public: 43 | // bool start_up; 44 | std::string name_space; 45 | std::string joint_name; 46 | float sensor_torque; 47 | bool isHip, isThigh, isCalf, rqtTune; 48 | urdf::JointConstSharedPtr joint_urdf; 49 | realtime_tools::RealtimeBuffer command; 50 | unitree_legged_msgs::MotorCmd lastCmd; 51 | unitree_legged_msgs::MotorState lastState; 52 | ServoCmd servoCmd; 53 | 54 | UnitreeJointController(); 55 | ~UnitreeJointController(); 56 | virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 57 | virtual void starting(const ros::Time& time); 58 | virtual void update(const ros::Time& time, const ros::Duration& period); 59 | virtual void stopping(); 60 | void setTorqueCB(const geometry_msgs::WrenchStampedConstPtr& msg); 61 | void setCommandCB(const unitree_legged_msgs::MotorCmdConstPtr& msg); 62 | void positionLimits(double &position); 63 | void velocityLimits(double &velocity); 64 | void effortLimits(double &effort); 65 | 66 | void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false); 67 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); 68 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 69 | 70 | }; 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /song_ros_control/src/A1Controller.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/20. 3 | // 4 | 5 | #include "song_ros_control/A1Controller.h" 6 | 7 | #include 8 | A1Controller::A1Controller() 9 | { 10 | memset(&last_cmd_, 0, sizeof(song_msgs::MotorCmd)); 11 | memset(&last_state_, 0, sizeof(sensor_msgs::JointState)); 12 | } 13 | 14 | A1Controller::~A1Controller() 15 | { 16 | sub_cmd_.shutdown(); 17 | } 18 | 19 | bool A1Controller::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) 20 | { 21 | name_space = n.getNamespace(); 22 | 23 | urdf::Model urdf; // Get URDF info about joint 24 | if (!urdf.initParamWithNodeHandle("robot_description", n)){ 25 | ROS_ERROR("Failed to parse urdf file"); 26 | return false; 27 | } 28 | 29 | n.getParam("joint_name_list", joint_name_list_); 30 | 31 | for (int i = 0; i < joint_name_list_.size(); ++i) { 32 | ROS_INFO_STREAM(joint_name_list_[i] << " \n "); 33 | joint_name_to_index_[joint_name_list_[i]] = i; 34 | 35 | urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_name_list_[i]); 36 | if (!joint_urdf) { 37 | ROS_ERROR("Could not find joint '%s' in urdf", joint_name_list_[i].c_str()); 38 | return false; 39 | } 40 | hardware_interface::JointHandle joint = robot->getHandle(joint_name_list_[i]); 41 | joint_list_.push_back(joint); 42 | } 43 | 44 | sub_cmd_ = n.subscribe("command", 20, &A1Controller::cmd_cb, this); 45 | 46 | robot_status_pub_ = std::make_unique>( 47 | n, name_space + "/state", 1); 48 | return true; 49 | } 50 | 51 | void A1Controller::starting(const ros::Time& time) 52 | { 53 | for (int i = 0; i < joint_name_list_.size(); ++i) { 54 | last_cmd_.tau[i] = 0.0; 55 | last_cmd_.joint_name[i] = joint_name_list_[i]; 56 | } 57 | command_.initRT(last_cmd_); 58 | } 59 | 60 | void A1Controller::cmd_cb(const song_msgs::MotorCmdConstPtr& msg) 61 | { 62 | last_cmd_.tau = msg->tau; 63 | last_cmd_.joint_name = msg->joint_name; 64 | 65 | command_.writeFromNonRT(last_cmd_); 66 | } 67 | 68 | void A1Controller::update(const ros::Time& time, const ros::Duration& period) 69 | { 70 | last_cmd_ = *(command_.readFromRT()); 71 | 72 | for (int i = 0; i != 12; ++i){ 73 | joint_list_[joint_name_to_index_[last_cmd_.joint_name[i]]].setCommand(last_cmd_.tau[i]); 74 | } 75 | 76 | if(robot_status_pub_ && robot_status_pub_->trylock()){ 77 | last_state_.name.resize(12); 78 | last_state_.position.resize(12); 79 | last_state_.velocity.resize(12); 80 | last_state_.effort.resize(12); 81 | for (int i = 0; i != 12; ++i){ 82 | last_state_.name[i] = joint_name_list_[i]; 83 | last_state_.position[i] = joint_list_[i].getPosition(); 84 | last_state_.velocity[i] = joint_list_[i].getVelocity(); 85 | last_state_.effort[i] = joint_list_[i].getEffort(); 86 | } 87 | robot_status_pub_->msg_.position = last_state_.position; 88 | robot_status_pub_->msg_.velocity = last_state_.velocity; 89 | robot_status_pub_->msg_.effort = last_state_.effort; 90 | //robot_status_pub_->msg_ = time; 91 | robot_status_pub_->msg_.header.stamp = time; 92 | robot_status_pub_->unlockAndPublish(); 93 | } 94 | } 95 | 96 | PLUGINLIB_EXPORT_CLASS(A1Controller, controller_interface::ControllerBase); 97 | 98 | -------------------------------------------------------------------------------- /song_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | song_msgs 4 | 0.0.1 5 | The song_msgs package 6 | 7 | 8 | 9 | 10 | Song Liu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | sensor_msgs 55 | std_msgs 56 | nav_msgs 57 | geometry_msgs 58 | roscpp 59 | sensor_msgs 60 | std_msgs 61 | nav_msgs 62 | geometry_msgs 63 | roscpp 64 | sensor_msgs 65 | std_msgs 66 | nav_msgs 67 | message_generation 68 | message_runtime 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /song_wbc/src/WbcStand.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/22. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "song_wbc/controller/OscStandController.h" 10 | 11 | #include 12 | #include 13 | 14 | std::shared_ptr osc; 15 | ros::Publisher motor_cmd; 16 | 17 | void callback(const sensor_msgs::JointStateConstPtr& motor_state, const nav_msgs::OdometryConstPtr& odo_data) 18 | { 19 | song_msgs::MotorCmd new_cmd; 20 | osc->update(motor_state, odo_data, new_cmd); 21 | motor_cmd.publish(new_cmd); 22 | } 23 | 24 | int main(int argc, char** argv) 25 | { 26 | std::string path = ros::package::getPath("song_wbc"); 27 | std::string urdf_path = path + "/urdf/a1.urdf"; 28 | 29 | drake::multibody::MultibodyPlant plant(0.0); 30 | addA1Multibody(&plant, urdf_path); 31 | plant.Finalize(); 32 | 33 | auto context = plant.CreateDefaultContext(); 34 | 35 | osc = std::make_shared(plant, context.get()); 36 | 37 | double w_contact_relax = 20000; 38 | 39 | osc->SetWeightOfSoftContactConstraint(w_contact_relax); 40 | 41 | double mu = 0.8; 42 | osc->SetContactFriction(mu); 43 | 44 | auto left_toe = LeftToeFront(plant); 45 | auto left_heel = LeftToeRear(plant); 46 | auto right_toe = RightToeFront(plant); 47 | auto right_heel = RightToeRear(plant); 48 | 49 | 50 | osc->AddContactPoint(left_toe); 51 | osc->AddContactPoint(left_heel); 52 | osc->AddContactPoint(right_toe); 53 | osc->AddContactPoint(right_heel); 54 | 55 | int n_v = plant.num_velocities(); 56 | Eigen::MatrixXd Q_accel = 0.01 * Eigen::MatrixXd::Identity(n_v, n_v); 57 | osc->SetAccelerationCostForAllJoints(Q_accel); 58 | 59 | Eigen::Matrix3d W_com; 60 | W_com << 2000, 0, 0, 0, 2000, 0, 0, 0, 1000; 61 | 62 | Eigen::Matrix3d K_p_com; 63 | K_p_com << 22, 0, 0, 0, 20, 0, 0, 0, 20; 64 | Eigen::Matrix3d K_d_com; 65 | K_d_com << 0.5, 0, 0, 0, 0.4, 0, 0, 0, 2; 66 | 67 | ComTrackingData center_of_mass_traj("com_traj", K_p_com, K_d_com, 68 | W_com, 69 | plant); 70 | osc->AddAllLegTrackingData(¢er_of_mass_traj); 71 | 72 | Eigen::Matrix3d W_pelvis; 73 | W_pelvis << 200, 0, 0, 0, 200, 0, 0, 0, 200; 74 | 75 | Eigen::Matrix3d K_p_pelvis; 76 | K_p_pelvis << 10, 0, 0, 0, 100, 0, 0, 0, 10; 77 | 78 | Eigen::Matrix3d K_d_pelvis; 79 | K_d_pelvis << 0.5, 0, 0, 0, 10, 0, 0, 0, 10; 80 | 81 | // Pelvis rotation tracking 82 | RotTaskSpaceTrackingData pelvis_rot_traj( 83 | "base_rot_traj", K_p_pelvis, K_d_pelvis, 84 | W_pelvis, plant); 85 | pelvis_rot_traj.AddFrameToTrack("base"); 86 | osc->AddAllLegTrackingData(&pelvis_rot_traj); 87 | 88 | osc->Build(); 89 | 90 | ros::init(argc, argv, "stand_node"); 91 | 92 | ros::NodeHandle nh; 93 | 94 | motor_cmd = nh.advertise("/a1_gazebo/A1_controller/command", 1); 95 | 96 | message_filters::Subscriber state_sub(nh, "/a1_gazebo/A1_controller/state", 1); 97 | message_filters::Subscriber odo_sub(nh, "/ground_truth/state", 1); 98 | message_filters::TimeSynchronizer sync(state_sub, odo_sub, 2); 99 | 100 | sync.registerCallback(boost::bind(&callback, _1, _2)); 101 | 102 | ros::spin(); 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /song_ros_control/src/OscTrackingDataRaw.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/25. 3 | // 4 | 5 | #include "song_ros_control/wbc/OSC/OscTrackingDataRaw.h" 6 | 7 | using std::cout; 8 | using std::endl; 9 | using drake::multibody::JacobianWrtVariable; 10 | using drake::multibody::MultibodyPlant; 11 | using drake::systems::Context; 12 | using Eigen::Isometry3d; 13 | using Eigen::MatrixXd; 14 | using Eigen::Quaterniond; 15 | using Eigen::Vector3d; 16 | using Eigen::VectorXd; 17 | using std::string; 18 | using std::vector; 19 | 20 | 21 | ComTrackingDataRaw::ComTrackingDataRaw(const std::string& name, const Eigen::MatrixXd& K_p, 22 | const Eigen::MatrixXd& K_d, const Eigen::MatrixXd& W, 23 | const drake::multibody::MultibodyPlant& plant): 24 | name_(name), 25 | K_p_(K_p), 26 | K_d_(K_d), 27 | W_(W), 28 | plant_(plant), 29 | world_(plant.world_frame()){} 30 | 31 | bool ComTrackingDataRaw::Update(const Eigen::VectorXd& x, 32 | const drake::systems::Context& context, 33 | const Eigen::Vector3d& position_d) 34 | { 35 | std::cout<<1< 17 | #include 18 | #include 19 | 20 | namespace gazebo 21 | { 22 | class UnitreeDrawForcePlugin : public VisualPlugin 23 | { 24 | public: 25 | UnitreeDrawForcePlugin():line(NULL){} 26 | ~UnitreeDrawForcePlugin(){ 27 | this->visual->DeleteDynamicLine(this->line); 28 | } 29 | 30 | void Load(rendering::VisualPtr _parent, sdf::ElementPtr _sdf ) 31 | { 32 | this->visual = _parent; 33 | this->visual_namespace = "visual/"; 34 | if (!_sdf->HasElement("topicName")){ 35 | ROS_INFO("Force draw plugin missing , defaults to /default_force_draw"); 36 | this->topic_name = "/default_force_draw"; 37 | } else{ 38 | this->topic_name = _sdf->Get("topicName"); 39 | } 40 | if (!ros::isInitialized()){ 41 | int argc = 0; 42 | char** argv = NULL; 43 | ros::init(argc,argv,"gazebo_visual",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); 44 | } 45 | 46 | this->line = this->visual->CreateDynamicLine(rendering::RENDERING_LINE_STRIP); 47 | this->line->AddPoint(ignition::math::Vector3d(0, 0, 0), ignition::math::Color(0, 1, 0, 1.0)); 48 | this->line->AddPoint(ignition::math::Vector3d(1, 1, 1), ignition::math::Color(0, 1, 0, 1.0)); 49 | this->line->setMaterial("Gazebo/Purple"); 50 | this->line->setVisibilityFlags(GZ_VISIBILITY_GUI); 51 | this->visual->SetVisible(true); 52 | 53 | this->rosnode = new ros::NodeHandle(this->visual_namespace); 54 | this->force_sub = this->rosnode->subscribe(this->topic_name+"/"+"the_force", 30, &UnitreeDrawForcePlugin::GetForceCallback, this); 55 | this->update_connection = event::Events::ConnectPreRender(boost::bind(&UnitreeDrawForcePlugin::OnUpdate, this)); 56 | ROS_INFO("Load %s Draw Force plugin.", this->topic_name.c_str()); 57 | } 58 | 59 | void OnUpdate() 60 | { 61 | this->line->SetPoint(1, ignition::math::Vector3d(Fx, Fy, Fz)); 62 | } 63 | 64 | void GetForceCallback(const geometry_msgs::WrenchStamped & msg) 65 | { 66 | Fx = msg.wrench.force.x/20.0; 67 | Fy = msg.wrench.force.y/20.0; 68 | Fz = msg.wrench.force.z/20.0; 69 | // Fx = msg.wrench.force.x; 70 | // Fy = msg.wrench.force.y; 71 | // Fz = msg.wrench.force.z; 72 | } 73 | 74 | private: 75 | ros::NodeHandle* rosnode; 76 | std::string topic_name; 77 | rendering::VisualPtr visual; 78 | rendering::DynamicLines *line; 79 | std::string visual_namespace; 80 | ros::Subscriber force_sub; 81 | double Fx=0, Fy=0, Fz=0; 82 | event::ConnectionPtr update_connection; 83 | }; 84 | GZ_REGISTER_VISUAL_PLUGIN(UnitreeDrawForcePlugin) 85 | } 86 | -------------------------------------------------------------------------------- /song_ros_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | song_ros_control 4 | 0.0.0 5 | The song_ros_control package 6 | 7 | 8 | 9 | 10 | han 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | controller_interface 53 | hardware_interface 54 | pluginlib 55 | roscpp 56 | song_msgs 57 | realtime_tools 58 | controller_interface 59 | hardware_interface 60 | pluginlib 61 | roscpp 62 | song_msgs 63 | realtime_tools 64 | controller_interface 65 | hardware_interface 66 | pluginlib 67 | roscpp 68 | song_msgs 69 | realtime_tools 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /song_wbc/include/song_wbc/controller/OscStandController.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/23. 3 | // 4 | 5 | #ifndef SONG_WBC_OSCSTANDCONTROLLER_H 6 | #define SONG_WBC_OSCSTANDCONTROLLER_H 7 | 8 | #include "drake/multibody/plant/multibody_plant.h" 9 | #include "drake/solvers/mathematical_program.h" 10 | #include "drake/solvers/solve.h" 11 | #include "drake/common/trajectories/piecewise_polynomial.h" 12 | 13 | #include "song_wbc/Utils.h" 14 | #include "song_wbc/OSC/OscTrackingData.h" 15 | #include "song_msgs/MotorState.h" 16 | #include "song_msgs/MotorCmd.h" 17 | #include "nav_msgs/Odometry.h" 18 | #include "sensor_msgs/JointState.h" 19 | 20 | class OscStandController { 21 | public: 22 | OscStandController(const drake::multibody::MultibodyPlant& plant, 23 | drake::systems::Context* context, 24 | bool print_info=true); 25 | 26 | void Build(); 27 | 28 | void SetWeightOfSoftContactConstraint(double w_soft_constraint) { 29 | w_soft_constraint_ = w_soft_constraint; 30 | } 31 | 32 | void SetContactFriction(double mu) { mu_ = mu; } 33 | 34 | // Cost methods 35 | void SetInputCost(const Eigen::MatrixXd& W) { W_input_ = W; } 36 | 37 | void SetAccelerationCostForAllJoints(const Eigen::MatrixXd& W) { 38 | W_joint_accel_ = W; 39 | } 40 | 41 | void AddContactPoint(const ContactData evaluator); 42 | 43 | void AddAllLegTrackingData(OscTrackingData* tracking_data); 44 | 45 | void update(const sensor_msgs::JointStateConstPtr& motor_state, const nav_msgs::OdometryConstPtr& odo_data, song_msgs::MotorCmd& motor_cmd); 46 | 47 | 48 | private: 49 | const drake::multibody::MultibodyPlant& plant_; 50 | 51 | const drake::multibody::BodyFrame& world_; 52 | 53 | drake::systems::Context* context_; 54 | 55 | int n_q_; 56 | int n_v_; 57 | int n_u_; 58 | 59 | // Size of holonomic constraint and total/active contact constraints 60 | int n_h_; 61 | int n_c_; 62 | int n_c_active_; 63 | 64 | bool is_print_info_; 65 | 66 | // mu_ and w_soft_constraint 67 | double mu_ = -1; // Friction coefficients 68 | double w_soft_constraint_ = -1; 69 | 70 | Eigen::MatrixXd W_input_; // Input cost weight 71 | Eigen::MatrixXd W_joint_accel_; // Joint acceleration cost weight 72 | 73 | 74 | std::map> contact_indices_map_ = {}; 75 | // All contacts (used in contact constraints) 76 | std::vector all_contacts_ = {}; 77 | 78 | // MathematicalProgram 79 | std::unique_ptr prog_; 80 | // Decision variables 81 | drake::solvers::VectorXDecisionVariable dv_; 82 | drake::solvers::VectorXDecisionVariable u_; 83 | drake::solvers::VectorXDecisionVariable lambda_c_; 84 | drake::solvers::VectorXDecisionVariable lambda_h_; 85 | drake::solvers::VectorXDecisionVariable epsilon_; 86 | // Cost and constraints 87 | drake::solvers::LinearEqualityConstraint* dynamics_constraint_; 88 | drake::solvers::LinearEqualityConstraint* holonomic_constraint_; 89 | drake::solvers::LinearEqualityConstraint* contact_constraints_; 90 | std::vector friction_constraints_; 91 | 92 | std::vector all_leg_tracking_cost_; 93 | 94 | // OSC solution 95 | std::unique_ptr dv_sol_; 96 | std::unique_ptr u_sol_; 97 | std::unique_ptr lambda_c_sol_; 98 | std::unique_ptr lambda_h_sol_; 99 | std::unique_ptr epsilon_sol_; 100 | 101 | // OSC constraint members 102 | bool with_input_constraints_ = true; 103 | 104 | // robot input limits 105 | Eigen::VectorXd u_min_; 106 | Eigen::VectorXd u_max_; 107 | 108 | std::unique_ptr> all_leg_data_vec_ = 109 | std::make_unique>(); 110 | 111 | // Osc checkers and constructor-related methods 112 | void CheckCostSettings(); 113 | void CheckConstraintSettings(); 114 | 115 | }; 116 | 117 | 118 | #endif //SONG_WBC_OSCSTANDCONTROLLER_H 119 | -------------------------------------------------------------------------------- /unitree_gazebo/worlds/stairs.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.001 7 | 1 8 | 1000 9 | 0 0 -9.81 10 | 11 | 12 | quick 13 | 50 14 | 1.3 15 | 16 | 17 | 0.0 18 | 0.2 19 | 10.0 20 | 0.001 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 12 29 | 30 | 31 | 32 | 33 | 34 | model://sun 35 | 36 | 37 | 38 | model://ground_plane 39 | 40 | 41 | 42 | true 43 | 44 | 0 0 0.09 0 0 0 45 | 46 | 47 | 48 | 2 2 0.18 49 | 50 | 51 | 52 | 53 | 54 | 55 | 2 2 0.18 56 | 57 | 58 | 59 | 0.2 0.2 0.2 1.0 60 | .421 0.225 0.0 1.0 61 | 62 | 63 | 64 | 65 | 66 | 2.2 0 0.09 0 0 0 67 | 68 | 69 | 70 | 2 2 0.18 71 | 72 | 73 | 74 | 75 | 76 | 77 | 2 2 0.18 78 | 79 | 80 | 81 | 0.2 0.2 0.2 1.0 82 | .421 0.225 0.0 1.0 83 | 84 | 85 | 86 | 87 | 88 | 5.2 0 0.63 0 0 0 89 | 90 | 91 | 92 | 2 2 0.18 93 | 94 | 95 | 96 | 97 | 98 | 99 | 2 2 0.18 100 | 101 | 102 | 103 | 0.2 0.2 0.2 1.0 104 | .421 0.225 0.0 1.0 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | model://unitree_gazebo/worlds/building_editor_models/stairs 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /unitree_gazebo/plugin/foot_contact_plugin.cc: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace gazebo 15 | { 16 | class UnitreeFootContactPlugin : public SensorPlugin 17 | { 18 | public: 19 | UnitreeFootContactPlugin() : SensorPlugin(){} 20 | ~UnitreeFootContactPlugin(){} 21 | 22 | void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) 23 | { 24 | this->parentSensor = std::dynamic_pointer_cast(_sensor); // Make sure the parent sensor is valid. 25 | if (!this->parentSensor){ 26 | gzerr << "UnitreeFootContactPlugin requires a ContactSensor.\n"; 27 | return; 28 | } 29 | this->contact_namespace = "contact/"; 30 | this->rosnode = new ros::NodeHandle(this->contact_namespace); 31 | // add "visual" is for the same name of draw node 32 | this->force_pub = this->rosnode->advertise("/visual/"+_sensor->Name()+"/the_force", 100); 33 | // Connect to the sensor update event. 34 | this->update_connection = this->parentSensor->ConnectUpdated(std::bind(&UnitreeFootContactPlugin::OnUpdate, this)); 35 | this->parentSensor->SetActive(true); // Make sure the parent sensor is active. 36 | count = 0; 37 | Fx = 0; 38 | Fy = 0; 39 | Fz = 0; 40 | ROS_INFO("Load %s plugin.", _sensor->Name().c_str()); 41 | } 42 | 43 | private: 44 | void OnUpdate() 45 | { 46 | msgs::Contacts contacts; 47 | contacts = this->parentSensor->Contacts(); 48 | count = contacts.contact_size(); 49 | // std::cout << count <<"\n"; 50 | for (unsigned int i = 0; i < count; ++i){ 51 | if(contacts.contact(i).position_size() != 1){ 52 | ROS_ERROR("Contact count isn't correct!!!!"); 53 | } 54 | for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j){ 55 | // std::cout << i <<" "<< contacts.contact(i).position_size() <<" Force:" 56 | // << contacts.contact(i).wrench(j).body_1_wrench().force().x() << " " 57 | // << contacts.contact(i).wrench(j).body_1_wrench().force().y() << " " 58 | // << contacts.contact(i).wrench(j).body_1_wrench().force().z() << "\n"; 59 | Fx += contacts.contact(i).wrench(0).body_1_wrench().force().x(); // Notice: the force is in local coordinate, not in world or base coordnate. 60 | Fy += contacts.contact(i).wrench(0).body_1_wrench().force().y(); 61 | Fz += contacts.contact(i).wrench(0).body_1_wrench().force().z(); 62 | } 63 | } 64 | if(count != 0){ 65 | force.wrench.force.x = Fx/double(count); 66 | force.wrench.force.y = Fy/double(count); 67 | force.wrench.force.z = Fz/double(count); 68 | count = 0; 69 | Fx = 0; 70 | Fy = 0; 71 | Fz = 0; 72 | } 73 | else{ 74 | force.wrench.force.x = 0; 75 | force.wrench.force.y = 0; 76 | force.wrench.force.z = 0; 77 | } 78 | this->force_pub.publish(force); 79 | } 80 | 81 | private: 82 | ros::NodeHandle* rosnode; 83 | ros::Publisher force_pub; 84 | event::ConnectionPtr update_connection; 85 | std::string contact_namespace; 86 | sensors::ContactSensorPtr parentSensor; 87 | geometry_msgs::WrenchStamped force; 88 | int count = 0; 89 | double Fx=0, Fy=0, Fz=0; 90 | }; 91 | GZ_REGISTER_SENSOR_PLUGIN(UnitreeFootContactPlugin) 92 | } 93 | 94 | -------------------------------------------------------------------------------- /song_ros_control/include/song_ros_control/wbc/controller/OscStandController.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/23. 3 | // 4 | 5 | #ifndef SONG_WBC_OSCSTANDCONTROLLER_H 6 | #define SONG_WBC_OSCSTANDCONTROLLER_H 7 | 8 | #include "drake/multibody/plant/multibody_plant.h" 9 | #include "drake/solvers/mathematical_program.h" 10 | #include "drake/solvers/solve.h" 11 | #include "drake/common/trajectories/piecewise_polynomial.h" 12 | 13 | #include "song_ros_control/wbc/Utils.h" 14 | #include "song_ros_control/wbc/OSC/OscTrackingData.h" 15 | #include "song_ros_control/wbc/OSC/OscTrackingDataRaw.h" 16 | #include "song_msgs/MotorState.h" 17 | #include "song_msgs/MotorCmd.h" 18 | #include "nav_msgs/Odometry.h" 19 | 20 | class OscStandController { 21 | public: 22 | OscStandController(std::shared_ptr> plant, 23 | drake::systems::Context* context, 24 | bool print_info=false); 25 | 26 | void Build(); 27 | 28 | void SetWeightOfSoftContactConstraint(double w_soft_constraint) { 29 | w_soft_constraint_ = w_soft_constraint; 30 | } 31 | 32 | void SetContactFriction(double mu) { mu_ = mu; } 33 | 34 | // Cost methods 35 | void SetInputCost(const Eigen::MatrixXd& W) { W_input_ = W; } 36 | 37 | void SetAccelerationCostForAllJoints(const Eigen::MatrixXd& W) { 38 | W_joint_accel_ = W; 39 | } 40 | 41 | void AddContactPoint(const ContactData evaluator); 42 | 43 | void AddAllLegTrackingData(ComTrackingDataRaw* tracking_data); 44 | 45 | void update(const song_msgs::MotorStatePtr& motor_state, const nav_msgs::OdometryConstPtr& odo_data, song_msgs::MotorCmd& motor_cmd); 46 | 47 | Eigen::VectorXd update(Eigen::VectorXd x, Eigen::VectorXd dx, Eigen::VectorXd x_, double t); 48 | 49 | 50 | private: 51 | std::shared_ptr> plant_; 52 | 53 | const drake::multibody::BodyFrame& world_; 54 | 55 | drake::systems::Context* context_; 56 | 57 | int n_q_; 58 | int n_v_; 59 | int n_u_; 60 | 61 | // Size of holonomic constraint and total/active contact constraints 62 | int n_h_; 63 | int n_c_; 64 | int n_c_active_; 65 | 66 | bool is_print_info_; 67 | 68 | // mu_ and w_soft_constraint 69 | double mu_ = -1; // Friction coefficients 70 | double w_soft_constraint_ = -1; 71 | 72 | Eigen::MatrixXd W_input_; // Input cost weight 73 | Eigen::MatrixXd W_joint_accel_; // Joint acceleration cost weight 74 | 75 | 76 | std::map> contact_indices_map_ = {}; 77 | // All contacts (used in contact constraints) 78 | std::vector all_contacts_ = {}; 79 | 80 | // MathematicalProgram 81 | std::unique_ptr prog_; 82 | // Decision variables 83 | drake::solvers::VectorXDecisionVariable dv_; 84 | drake::solvers::VectorXDecisionVariable u_; 85 | drake::solvers::VectorXDecisionVariable lambda_c_; 86 | drake::solvers::VectorXDecisionVariable lambda_h_; 87 | drake::solvers::VectorXDecisionVariable epsilon_; 88 | // Cost and constraints 89 | drake::solvers::LinearEqualityConstraint* dynamics_constraint_; 90 | drake::solvers::LinearEqualityConstraint* holonomic_constraint_; 91 | drake::solvers::LinearEqualityConstraint* contact_constraints_; 92 | std::vector friction_constraints_; 93 | 94 | std::vector all_leg_tracking_cost_; 95 | 96 | // OSC solution 97 | std::unique_ptr dv_sol_; 98 | std::unique_ptr u_sol_; 99 | std::unique_ptr lambda_c_sol_; 100 | std::unique_ptr lambda_h_sol_; 101 | std::unique_ptr epsilon_sol_; 102 | 103 | // OSC constraint members 104 | bool with_input_constraints_ = true; 105 | 106 | // robot input limits 107 | Eigen::VectorXd u_min_; 108 | Eigen::VectorXd u_max_; 109 | 110 | std::unique_ptr> all_leg_data_vec_ = 111 | std::make_unique>(); 112 | 113 | // Osc checkers and constructor-related methods 114 | void CheckCostSettings(); 115 | void CheckConstraintSettings(); 116 | 117 | }; 118 | 119 | 120 | #endif //SONG_WBC_OSCSTANDCONTROLLER_H 121 | -------------------------------------------------------------------------------- /robots/aliengo_description/xacro/const.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /robots/a1_description/xacro/const.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /robots/laikago_description/xacro/const.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /robots/laikago_description/xacro/robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /robots/a1_description/xacro/robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | -------------------------------------------------------------------------------- /robots/aliengo_description/xacro/robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 58 | 59 | 60 | 61 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Introduction 2 | Here are the ROS packages of Unitree robots, namely Laikago, Aliengo and A1. You can load robots and joint controllers in Gazebo, so you can do low-level control(control the torque, position and angular velocity) on the robot joints. Please watch out that the Gazebo simulation cannot do high-level control, namely walking. Besides of these simulation functions, you can also control your real robots in ROS by the `unitree_legged_real`. For real robots, you can do high-level and low-level control by our ROS packages. 3 | 4 | ## Packages: 5 | Robot description: `a1_description`, `aliengo_description`, `laikago_description` 6 | 7 | Robot and joints controller: `unitree_controller` 8 | 9 | Basic function: `unitree_legged_msgs` 10 | 11 | Simulation related: `unitree_gazebo`, `unitree_legged_control` 12 | 13 | Real robot control related: `unitree_legged_real` 14 | 15 | # Dependencies 16 | * [ROS](https://www.ros.org/) melodic or ROS kinetic(has not been tested) 17 | * [Gazebo8](http://gazebosim.org/) 18 | * [unitree_legged_sdk](https://github.com/unitreerobotics) 19 | * [aliengo_sdk](https://github.com/unitreerobotics) 20 | 21 | # Configuration 22 | Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case. 23 | ``` 24 | source /opt/ros/melodic/setup.bash 25 | source /usr/share/gazebo-8/setup.sh 26 | source ~/catkin_ws/devel/setup.bash 27 | export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH} 28 | export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH} 29 | export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH} 30 | export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk 31 | export ALIENGO_SDK_PATH=~/aliengo_sdk 32 | #amd64, arm32, arm64 33 | export UNITREE_PLATFORM="amd64" 34 | ``` 35 | 36 | # Build 37 | Please run the following command to install relative packages. 38 | 39 | If your ROS is melodic: 40 | ``` 41 | sudo apt-get install ros-melodic-controller-interface ros-melodic-gazebo-ros-control ros-melodic-joint-state-controller ros-melodic-effort-controllers ros-melodic-joint-trajectory-controller 42 | ``` 43 | Else if your ROS is kinetic: 44 | ``` 45 | sudo apt-get install ros-kinetic-controller-manager ros-kinetic-ros-control ros-kinetic-ros-controllers ros-kinetic-joint-state-controller ros-kinetic-effort-controllers ros-kinetic-velocity-controllers ros-kinetic-position-controllers ros-kinetic-robot-controllers ros-kinetic-robot-state-publisher ros-kinetic-gazebo8-ros ros-kinetic-gazebo8-ros-control ros-kinetic-gazebo8-ros-pkgs ros-kinetic-gazebo8-ros-dev 46 | ``` 47 | 48 | And open the file `unitree_gazebo/worlds/stairs.world`. At the end of the file: 49 | ``` 50 | 51 | model:///home/unitree/catkin_ws/src/unitree_ros/unitree_gazebo/worlds/building_editor_models/stairs 52 | 53 | ``` 54 | Please change the path of `building_editor_models/stairs` to the real path on your PC. 55 | 56 | Then you can use catkin_make to build: 57 | ``` 58 | cd ~/catkin_ws 59 | catkin_make 60 | ``` 61 | 62 | If you face a dependency problem, you can just run `catkin_make` again. 63 | 64 | # Detail of Packages 65 | ## unitree_legged_control: 66 | It contains the joints controllers for Gazebo simulation, which allows user to control joints with position, velocity and torque. 67 | 68 | ## unitree_legged_msgs: 69 | ros-type message, including command and state of high-level and low-level control. 70 | It would be better if it be compiled firstly, otherwise you may have dependency problems (such as that you can't find the header file). 71 | 72 | ## The description of robots: 73 | Namely the description of A1, Aliengo and Laikago. Each package include mesh, urdf and xacro files of robot. Take Laikago as an example, you can check the model in Rviz by: 74 | ``` 75 | roslaunch laikago_description laikago_rviz.launch 76 | ``` 77 | 78 | ## unitree_gazebo & unitree_controller: 79 | You can launch the Gazebo simulation by the following command: 80 | ``` 81 | roslaunch unitree_gazebo normal.launch rname:=a1 wname:=stairs 82 | ``` 83 | Where the `rname` means robot name, which can be `laikago`, `aliengo` or `a1`. The `wname` means world name, which can be `earth`, `space` or `stairs`. And the default value of `rname` is `laikago`, while the default value of `wname` is `earth`. In Gazebo, the robot should be lying on the ground with joints not activated. 84 | 85 | ### Stand controller 86 | After launching the gazebo simulation, you can start to control the robot: 87 | ``` 88 | rosrun unitree_controller unitree_servo 89 | ``` 90 | 91 | And you can add external disturbances, like a push or a kick: 92 | ``` 93 | rosrun unitree_controller unitree_external_force 94 | ``` 95 | ### Position and pose publisher 96 | Here we showed how to control the position and pose of robot without a controller, which should be useful in SLAM or visual development. 97 | 98 | Then run the position and pose publisher in another terminal: 99 | ``` 100 | rosrun unitree_controller unitree_move_kinetic 101 | ``` 102 | The robot will turn around the origin, which is the movement under the world coordinate. And inside of the source file `move_publisher`, we also offered the method to move robot under robot coordinate. You can change the value of `def_frame` to `coord::ROBOT` and run the catkin_make again, then the `unitree_move_publisher` will move robot under its own coordinate. 103 | 104 | ## unitree_legged_real 105 | ### Setup the net connection 106 | First, please connect the network cable between your PC and robot. Then run `ifconfig` in a terminal, you will find your port name. For example, `enx000ec6612921`. 107 | 108 | Then, open the `ipconfig.sh` file under the folder `unitree_legged_real`, modify the port name to your own. And run the following commands: 109 | ``` 110 | sudo chmod +x ipconfig.sh 111 | sudo ./ipconfig.sh 112 | ``` 113 | If you run the `ifconfig` again, you will find that port has `inet` and `netmask` now. 114 | In order to set your port automatically, you can modify `interfaces`: 115 | ``` 116 | sudo gedit /etc/network/interfaces 117 | ``` 118 | And add the following 4 lines at the end: 119 | ``` 120 | auto enx000ec6612921 121 | iface enx000ec6612921 inet static 122 | address 192.168.123.162 123 | netmask 255.255.255.0 124 | ``` 125 | Where the port name have to change to your own. 126 | 127 | 128 | ### Run the package 129 | You can control your real robot(only A1 and Aliengo) from ROS by this package. 130 | 131 | First you have to run the `real_launch` under root account: 132 | ``` 133 | sudo su 134 | source /home/yourUserName/catkin_ws/devel/setup.bash 135 | roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=highlevel firmwork:=3_2 136 | ``` 137 | Please watchout that the `/home/yourUserName` means the home directory of yourself. These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And the `ctrl_level` means the control level, which can be `lowlevel` or `highlevel`(case does not matter), and the default value is `highlevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose. The `firmwork` means the firmwork version of the robot. The default value is `3_2` Now all the A1's firmwork version is `3_2`, and most Aliengo's firmwork version is `3_1`.(will update in the future) 138 | 139 | To do so, you need to run the controller in another terminal(also under root account): 140 | ``` 141 | rosrun unitree_legged_real position_lcm 142 | ``` 143 | We offered some examples. When you run the low level controller, please make sure the robot is hanging up. The low level contains: 144 | ``` 145 | position_lcm 146 | velocity_lcm 147 | torque_lcm 148 | ``` 149 | The `velocity_lcm` and `torque_lcm` have to run under root account too. Please use the same method as runing `real_launch`. 150 | 151 | And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`. 152 | -------------------------------------------------------------------------------- /song_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(song_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | sensor_msgs 14 | std_msgs 15 | nav_msgs 16 | message_generation 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | add_message_files( 54 | FILES 55 | MotorState.msg 56 | MotorCmd.msg 57 | # Message2.msg 58 | ) 59 | 60 | ## Generate services in the 'srv' folder 61 | # add_service_files( 62 | # FILES 63 | # Service1.srv 64 | # Service2.srv 65 | # ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | geometry_msgs sensor_msgs std_msgs 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES song_msgs 112 | CATKIN_DEPENDS message_runtime geometry_msgs roscpp sensor_msgs std_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/song_msgs.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/song_msgs_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # catkin_install_python(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables for installation 172 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 173 | # install(TARGETS ${PROJECT_NAME}_node 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark libraries for installation 178 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 179 | # install(TARGETS ${PROJECT_NAME} 180 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark cpp header files for installation 186 | # install(DIRECTORY include/${PROJECT_NAME}/ 187 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 188 | # FILES_MATCHING PATTERN "*.h" 189 | # PATTERN ".svn" EXCLUDE 190 | # ) 191 | 192 | ## Mark other files for installation (e.g. launch and bag files, etc.) 193 | # install(FILES 194 | # # myfile1 195 | # # myfile2 196 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 197 | # ) 198 | 199 | ############# 200 | ## Testing ## 201 | ############# 202 | 203 | ## Add gtest based cpp test target and link libraries 204 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_song_msgs.cpp) 205 | # if(TARGET ${PROJECT_NAME}-test) 206 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 207 | # endif() 208 | 209 | ## Add folders to be run by python nosetests 210 | # catkin_add_nosetests(test) 211 | -------------------------------------------------------------------------------- /song_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(song_gazebo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | gazebo_dev 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES song_gazebo 106 | # CATKIN_DEPENDS gazebo_dev 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/song_gazebo.cpp 124 | # ) 125 | 126 | add_library(a1_init_position InitPosition.cpp) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/song_gazebo_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | target_link_libraries(a1_init_position 154 | ${catkin_LIBRARIES} 155 | ) 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # catkin_install_python(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_song_gazebo.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /song_ros_control/src/A1StandController.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by han on 2021/2/24. 3 | // 4 | 5 | #include "song_ros_control/A1StandController.h" 6 | 7 | A1StandController::A1StandController() 8 | { 9 | memset(&global_state_, 0, sizeof(nav_msgs::Odometry)); 10 | memset(&last_state_, 0, sizeof(song_msgs::MotorState)); 11 | } 12 | 13 | A1StandController::~A1StandController() 14 | { 15 | sub_cmd_.shutdown(); 16 | } 17 | 18 | bool A1StandController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) 19 | { 20 | 21 | std::string path = ros::package::getPath("song_ros_control"); 22 | std::string urdf_path = path + "/urdf/a1.urdf"; 23 | 24 | plant_ = std::make_shared>(0.0); 25 | addA1Multibody(plant_.get(), urdf_path); 26 | plant_->Finalize(); 27 | 28 | ROS_INFO_STREAM(plant_->num_velocities()); 29 | 30 | context_ = plant_->CreateDefaultContext(); 31 | 32 | osc_ = std::make_shared(plant_, context_.get()); 33 | 34 | double w_contact_relax = 20000; 35 | 36 | osc_->SetWeightOfSoftContactConstraint(w_contact_relax); 37 | 38 | double mu = 0.8; 39 | osc_->SetContactFriction(mu); 40 | 41 | auto left_toe = LeftToeFront(*plant_); 42 | auto left_heel = LeftToeRear(*plant_); 43 | auto right_toe = RightToeFront(*plant_); 44 | auto right_heel = RightToeRear(*plant_); 45 | 46 | 47 | osc_->AddContactPoint(left_toe); 48 | osc_->AddContactPoint(left_heel); 49 | osc_->AddContactPoint(right_toe); 50 | osc_->AddContactPoint(right_heel); 51 | 52 | int n_v = plant_->num_velocities(); 53 | Eigen::MatrixXd Q_accel = 0.01 * Eigen::MatrixXd::Identity(n_v, n_v); 54 | osc_->SetAccelerationCostForAllJoints(Q_accel); 55 | 56 | name_space = n.getNamespace(); 57 | 58 | urdf::Model urdf; // Get URDF info about joint 59 | if (!urdf.initParamWithNodeHandle("robot_description", n)){ 60 | ROS_ERROR("Failed to parse urdf file"); 61 | return false; 62 | } 63 | 64 | n.getParam("joint_name_list", joint_name_list_); 65 | n.getParam("W_com", w_com_); 66 | W_com_ = Eigen::Map(w_com_.data()); 67 | 68 | n.getParam("W_pelvis", w_pelvis_); 69 | W_pelvis_ = Eigen::Map(w_pelvis_.data()); 70 | 71 | n.getParam("K_p_com", k_p_com_); 72 | K_p_com_ = Eigen::Map(k_p_com_.data()); 73 | 74 | n.getParam("K_d_com", k_d_com_); 75 | K_d_com_ = Eigen::Map(k_d_com_.data()); 76 | 77 | n.getParam("K_p_pelvis", k_p_pelvis_); 78 | K_p_pelvis_ = Eigen::Map(k_p_pelvis_.data()); 79 | 80 | n.getParam("K_d_pelvis", k_d_pelvis_); 81 | K_d_pelvis_ = Eigen::Map(k_d_pelvis_.data()); 82 | 83 | 84 | ComTrackingDataRaw center_of_mass_traj("com_traj", K_p_com_, K_d_com_, 85 | W_com_, 86 | *plant_); 87 | osc_->AddAllLegTrackingData(¢er_of_mass_traj); 88 | 89 | /*RotTaskSpaceTrackingData pelvis_rot_traj( 90 | "base_rot_traj", K_p_pelvis_, K_d_pelvis_, 91 | W_pelvis_, *plant_); 92 | pelvis_rot_traj.AddFrameToTrack("base"); 93 | osc_->AddAllLegTrackingData(&pelvis_rot_traj);*/ 94 | 95 | osc_->Build(); 96 | 97 | 98 | for (int i = 0; i < joint_name_list_.size(); ++i) { 99 | ROS_INFO_STREAM(joint_name_list_[i] << " \n "); 100 | joint_name_to_index_[joint_name_list_[i]] = i; 101 | 102 | urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_name_list_[i]); 103 | if (!joint_urdf) { 104 | ROS_ERROR("Could not find joint '%s' in urdf", joint_name_list_[i].c_str()); 105 | return false; 106 | } 107 | hardware_interface::JointHandle joint = robot->getHandle(joint_name_list_[i]); 108 | joint_list_.push_back(joint); 109 | } 110 | 111 | sub_cmd_ = n.subscribe("/ground_truth/state", 20, &A1StandController::inertial_cb, this); 112 | 113 | robot_status_pub_ = std::make_unique>( 114 | n, name_space + "/state", 1); 115 | 116 | output_order_.resize(12); 117 | output_order_[0] = "FR_hip_joint"; 118 | output_order_[1] = "FR_thigh_joint"; 119 | output_order_[2] = "FR_calf_joint"; 120 | 121 | output_order_[3] = "FL_hip_joint"; 122 | output_order_[4] = "FL_thigh_joint"; 123 | output_order_[5] = "FL_calf_joint"; 124 | 125 | output_order_[6] = "RR_hip_joint"; 126 | output_order_[7] = "RR_thigh_joint"; 127 | output_order_[8] = "RR_calf_joint"; 128 | 129 | output_order_[9] = "RR_hip_joint"; 130 | output_order_[10] = "RR_thigh_joint"; 131 | output_order_[11] = "RR_calf_joint"; 132 | 133 | return true; 134 | } 135 | 136 | void A1StandController::starting(const ros::Time& time) 137 | { 138 | global_state_.pose.pose.orientation.w = 1; 139 | global_state_.pose.pose.position.x = -0.006337; 140 | global_state_.pose.pose.position.y = 0.00082725; 141 | global_state_.pose.pose.position.z = 0.278652; 142 | for (int i = 0; i < joint_name_list_.size(); ++i) { 143 | last_state_.q[i] = 0.0; 144 | last_state_.dq[i] = 0.0; 145 | last_state_.tau[i] = 0.0; 146 | last_state_.joint_name[i] = joint_name_list_[i]; 147 | } 148 | command_.initRT(global_state_); 149 | } 150 | 151 | void A1StandController::inertial_cb(const nav_msgs::OdometryConstPtr& msg) 152 | { 153 | global_state_.pose.pose.orientation = msg->pose.pose.orientation; 154 | global_state_.pose.pose.position = msg->pose.pose.position; 155 | 156 | global_state_.twist.twist.angular = msg->twist.twist.angular; 157 | global_state_.twist.twist.linear = msg->twist.twist.linear; 158 | 159 | command_.writeFromNonRT(global_state_); 160 | } 161 | 162 | void A1StandController::update(const ros::Time& time, const ros::Duration& period) 163 | { 164 | global_state__ = *(command_.readFromRT()); 165 | 166 | Eigen::VectorXd x(7+12); 167 | Eigen::VectorXd dx(6+12); 168 | Eigen::VectorXd x_(plant_->num_positions() + plant_->num_velocities()); 169 | 170 | // update 171 | x(0) = global_state__.pose.pose.orientation.w; 172 | x(1) = global_state__.pose.pose.orientation.x; 173 | x(2) = global_state__.pose.pose.orientation.y; 174 | x(3) = global_state__.pose.pose.orientation.z; 175 | x(4) = global_state__.pose.pose.position.x; 176 | x(5) = global_state__.pose.pose.position.y; 177 | x(6) = global_state__.pose.pose.position.z; 178 | for (int i = 0; i < 12; ++i) { 179 | x(7+i) = joint_list_[i].getPosition(); 180 | } 181 | 182 | dx(0) = global_state__.twist.twist.angular.x; 183 | dx(1) = global_state__.twist.twist.angular.y; 184 | dx(2) = global_state__.twist.twist.angular.z; 185 | dx(3) = global_state__.twist.twist.linear.x; 186 | dx(4) = global_state__.twist.twist.linear.y; 187 | dx(5) = global_state__.twist.twist.linear.z; 188 | 189 | for (int i = 0; i < 12; ++i) { 190 | dx(6+i) = joint_list_[i].getVelocity(); 191 | } 192 | 193 | x_ << x, dx; 194 | 195 | 196 | auto joint_tau = osc_->update(x, dx, x_, time.toSec()); 197 | 198 | ROS_INFO_STREAM(joint_tau); 199 | 200 | for (int i = 0; i < 12; ++i) { 201 | joint_list_[joint_name_to_index_[output_order_[i]]].setCommand(joint_tau[i]); 202 | } 203 | 204 | if(robot_status_pub_ && robot_status_pub_->trylock()){ 205 | for (int i = 0; i != 12; ++i){ 206 | last_state_.joint_name[i] = joint_name_list_[i]; 207 | last_state_.q[i] = joint_list_[i].getPosition(); 208 | last_state_.dq[i] = joint_list_[i].getVelocity(); 209 | last_state_.tau[i] = joint_list_[i].getEffort(); 210 | } 211 | robot_status_pub_->msg_.q = last_state_.q; 212 | robot_status_pub_->msg_.dq = last_state_.dq; 213 | robot_status_pub_->msg_.tau = last_state_.tau; 214 | //robot_status_pub_->msg_ = time; 215 | robot_status_pub_->msg_.header.stamp = time; 216 | robot_status_pub_->unlockAndPublish(); 217 | } 218 | } 219 | 220 | PLUGINLIB_EXPORT_CLASS(A1StandController, controller_interface::ControllerBase); 221 | -------------------------------------------------------------------------------- /robots/aliengo_description/launch/check_joint.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 420 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Class: rviz/RobotModel 52 | Collision Enabled: false 53 | Enabled: true 54 | Links: 55 | All Links Enabled: true 56 | Expand Joint Details: false 57 | Expand Link Details: false 58 | Expand Tree: false 59 | FL_calf: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Value: true 64 | FL_foot: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | FL_hip: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | FL_thigh: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | FR_calf: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | FR_foot: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | FR_hip: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | FR_thigh: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | Link Tree Style: Links in Alphabetic Order 100 | RL_calf: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | RL_foot: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | RL_hip: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | RL_thigh: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | RR_calf: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | RR_foot: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | RR_hip: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | RR_thigh: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | Value: true 140 | base: 141 | Alpha: 1 142 | Show Axes: false 143 | Show Trail: false 144 | Value: true 145 | imu_link: 146 | Alpha: 1 147 | Show Axes: false 148 | Show Trail: false 149 | Value: true 150 | trunk: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | Name: RobotModel 156 | Robot Description: robot_description 157 | TF Prefix: "" 158 | Update Interval: 0 159 | Value: true 160 | Visual Enabled: true 161 | - Class: rviz/Axes 162 | Enabled: false 163 | Length: 1 164 | Name: Axes 165 | Radius: 0.10000000149011612 166 | Reference Frame: 167 | Value: false 168 | - Class: rviz/TF 169 | Enabled: false 170 | Frame Timeout: 15 171 | Frames: 172 | All Enabled: true 173 | Marker Scale: 1 174 | Name: TF 175 | Show Arrows: true 176 | Show Axes: true 177 | Show Names: true 178 | Tree: 179 | {} 180 | Update Interval: 0 181 | Value: false 182 | Enabled: true 183 | Global Options: 184 | Background Color: 238; 238; 238 185 | Default Light: true 186 | Fixed Frame: base 187 | Frame Rate: 30 188 | Name: root 189 | Tools: 190 | - Class: rviz/Interact 191 | Hide Inactive Objects: true 192 | - Class: rviz/MoveCamera 193 | - Class: rviz/Select 194 | - Class: rviz/FocusCamera 195 | - Class: rviz/Measure 196 | - Class: rviz/SetInitialPose 197 | Topic: /initialpose 198 | - Class: rviz/SetGoal 199 | Topic: /move_base_simple/goal 200 | - Class: rviz/PublishPoint 201 | Single click: true 202 | Topic: /clicked_point 203 | Value: true 204 | Views: 205 | Current: 206 | Class: rviz/Orbit 207 | Distance: 1.4732182025909424 208 | Enable Stereo Rendering: 209 | Stereo Eye Separation: 0.05999999865889549 210 | Stereo Focal Distance: 1 211 | Swap Stereo Eyes: false 212 | Value: false 213 | Focal Point: 214 | X: 0.0760490670800209 215 | Y: 0.11421932280063629 216 | Z: -0.1576911211013794 217 | Focal Shape Fixed Size: false 218 | Focal Shape Size: 0.05000000074505806 219 | Invert Z Axis: false 220 | Name: Current View 221 | Near Clip Distance: 0.009999999776482582 222 | Pitch: 0.49979713559150696 223 | Target Frame: 224 | Value: Orbit (rviz) 225 | Yaw: 5.093583106994629 226 | Saved: ~ 227 | Window Geometry: 228 | Displays: 229 | collapsed: false 230 | Height: 627 231 | Hide Left Dock: false 232 | Hide Right Dock: false 233 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 234 | Selection: 235 | collapsed: false 236 | Time: 237 | collapsed: false 238 | Tool Properties: 239 | collapsed: false 240 | Views: 241 | collapsed: false 242 | Width: 1108 243 | X: 812 244 | Y: 241 245 | --------------------------------------------------------------------------------