├── Hector_ROS_Simulation ├── .vscode │ ├── c_cpp_properties.json │ └── settings.json ├── hector_control │ ├── CMakeLists.txt │ ├── ConvexMPC │ │ ├── Constraints.cpp │ │ ├── Constraints.h │ │ ├── ConvexMPCLocomotion.cpp │ │ ├── ConvexMPCLocomotion.h │ │ ├── GaitGenerator.cpp │ │ ├── GaitGenerator.h │ │ ├── RobotState.cpp │ │ ├── RobotState.h │ │ ├── SolverMPC.cpp │ │ ├── SolverMPC.h │ │ ├── common_types.h │ │ ├── convexMPC_interface.cpp │ │ └── convexMPC_interface.h │ ├── include │ │ ├── FSM │ │ │ ├── FSM.h │ │ │ ├── FSMState.h │ │ │ ├── FSMState_Passive.h │ │ │ └── FSMState_Walking.h │ │ ├── common │ │ │ ├── Biped.h │ │ │ ├── CheaterEstimator.h │ │ │ ├── ControlFSMData.h │ │ │ ├── DesiredCommand.h │ │ │ ├── FootSwingTrajectory.h │ │ │ ├── LegController.h │ │ │ ├── Math │ │ │ │ ├── FirstOrderIIRFilter.h │ │ │ │ ├── Interpolation.h │ │ │ │ ├── MathUtilities.h │ │ │ │ └── orientation_tools.h │ │ │ ├── OrientationEstimator.h │ │ │ ├── PositionVelocityEstimator.h │ │ │ ├── StateEstimatorContainer.h │ │ │ ├── Utilities │ │ │ │ ├── BSplineBasic.h │ │ │ │ ├── BezierCurve.h │ │ │ │ ├── PeriodicTask.h │ │ │ │ ├── SharedMemory.h │ │ │ │ ├── Timer.h │ │ │ │ ├── filters.h │ │ │ │ └── pseudoInverse.h │ │ │ ├── cTypes.h │ │ │ ├── cppTypes.h │ │ │ └── enumClass.h │ │ ├── interface │ │ │ ├── CheatIO.h │ │ │ ├── CmdPanel.h │ │ │ ├── HighLevelIO.h │ │ │ ├── IOInterface.h │ │ │ ├── KeyBoard.h │ │ │ └── MujocoIO.h │ │ ├── messages │ │ │ ├── LowLevelCmd.h │ │ │ ├── LowlevelState.h │ │ │ └── unitree_joystick.h │ │ ├── sdk │ │ │ ├── include │ │ │ │ └── unitree_legged_sdk │ │ │ │ │ ├── a1_const.h │ │ │ │ │ ├── aliengo_const.h │ │ │ │ │ ├── comm.h │ │ │ │ │ ├── control.h │ │ │ │ │ ├── lcm.h │ │ │ │ │ ├── loop.h │ │ │ │ │ ├── quadruped.h │ │ │ │ │ ├── udp.h │ │ │ │ │ └── unitree_legged_sdk.h │ │ │ └── lib │ │ │ │ └── libunitree_legged_sdk.so │ │ └── simulation.h │ ├── package.xml │ ├── src │ │ ├── FSM │ │ │ ├── FSM.cpp │ │ │ ├── FSMState.cpp │ │ │ ├── FSMState_Passive.cpp │ │ │ └── FSMState_Walking.cpp │ │ ├── common │ │ │ ├── DesiredCommand.cpp │ │ │ ├── FootSwingTrajectory.cpp │ │ │ ├── LegController.cpp │ │ │ ├── OrientationEstimator.cpp │ │ │ └── PositionVelocityEstimator.cpp │ │ ├── interface │ │ │ ├── CheatIO.cpp │ │ │ ├── KeyBoard.cpp │ │ │ └── MujocoIO.cpp │ │ ├── main.cpp │ │ ├── mjSim.cpp │ │ └── simulation.cpp │ └── third_party │ │ └── qpOASES │ │ ├── AUTHORS │ │ ├── AUTHORS.txt │ │ ├── CMakeLists.txt │ │ ├── INSTALL │ │ ├── INSTALL.txt │ │ ├── LICENSE │ │ ├── LICENSE.txt │ │ ├── Makefile │ │ ├── README │ │ ├── README.txt │ │ ├── VERSIONS │ │ ├── VERSIONS.txt │ │ ├── include │ │ ├── qpOASES.hpp │ │ └── qpOASES │ │ │ ├── Bounds.hpp │ │ │ ├── Bounds.ipp │ │ │ ├── Constants.hpp │ │ │ ├── ConstraintProduct.hpp │ │ │ ├── Constraints.hpp │ │ │ ├── Constraints.ipp │ │ │ ├── Flipper.hpp │ │ │ ├── Indexlist.hpp │ │ │ ├── Indexlist.ipp │ │ │ ├── LapackBlasReplacement.hpp │ │ │ ├── Matrices.hpp │ │ │ ├── MessageHandling.hpp │ │ │ ├── MessageHandling.ipp │ │ │ ├── Options.hpp │ │ │ ├── QProblem.hpp │ │ │ ├── QProblem.ipp │ │ │ ├── QProblemB.hpp │ │ │ ├── QProblemB.ipp │ │ │ ├── SQProblem.hpp │ │ │ ├── SQProblem.ipp │ │ │ ├── SQProblemSchur.hpp │ │ │ ├── SQProblemSchur.ipp │ │ │ ├── SparseSolver.hpp │ │ │ ├── SubjectTo.hpp │ │ │ ├── SubjectTo.ipp │ │ │ ├── Types.hpp │ │ │ ├── UnitTesting.hpp │ │ │ ├── Utils.hpp │ │ │ ├── Utils.ipp │ │ │ └── extras │ │ │ ├── OQPinterface.hpp │ │ │ ├── SolutionAnalysis.hpp │ │ │ └── SolutionAnalysis.ipp │ │ ├── make.mk │ │ ├── make_cygwin.mk │ │ ├── make_linux.mk │ │ ├── make_osx.mk │ │ ├── make_windows.mk │ │ └── src │ │ ├── BLASReplacement.cpp │ │ ├── Bounds.cpp │ │ ├── Constraints.cpp │ │ ├── Flipper.cpp │ │ ├── Indexlist.cpp │ │ ├── LAPACKReplacement.cpp │ │ ├── Makefile │ │ ├── Matrices.cpp │ │ ├── MessageHandling.cpp │ │ ├── OQPinterface.cpp │ │ ├── Options.cpp │ │ ├── QProblem.cpp │ │ ├── QProblemB.cpp │ │ ├── SQProblem.cpp │ │ ├── SQProblemSchur.cpp │ │ ├── SolutionAnalysis.cpp │ │ ├── SparseSolver.cpp │ │ ├── SubjectTo.cpp │ │ └── Utils.cpp ├── hector_description │ ├── CMakeLists.txt │ ├── config │ │ └── robot_control.yaml │ ├── launch │ │ ├── check_joint.rviz │ │ └── hector_rviz.launch │ ├── meshes │ │ ├── L_calf.STL │ │ ├── L_foot.STL │ │ ├── L_hip1.STL │ │ ├── L_hip2.STL │ │ ├── L_thigh.STL │ │ ├── R_calf.STL │ │ ├── R_foot.STL │ │ ├── R_hip1.STL │ │ ├── R_hip2.STL │ │ ├── R_thigh.STL │ │ ├── body.STL │ │ ├── calf.STL │ │ ├── hip.STL │ │ ├── hip2_L.STL │ │ ├── hip2_R.STL │ │ ├── left leg hip_1.STL │ │ ├── right leg hip_1.STL │ │ ├── thigh.STL │ │ ├── toe.STL │ │ └── trunk.STL │ ├── mjcf │ │ └── hector.xml │ ├── package.xml │ └── xacro │ │ ├── const.xacro │ │ ├── gazebo.xacro │ │ ├── hector.urdf │ │ ├── leg.xacro │ │ ├── materials.xacro │ │ ├── robot.xacro │ │ ├── stairs.xacro │ │ └── transmission.xacro ├── mujoco_sim │ ├── CMakeLists.txt │ ├── package.xml │ └── script │ │ ├── __pycache__ │ │ └── mujoco_base.cpython-38.pyc │ │ ├── hector_sim.py │ │ └── mujoco_base.py ├── pai_description │ ├── CMakeLists.txt │ ├── config │ │ └── robot_control.yaml │ ├── launch │ │ ├── check_joint.rviz │ │ └── hector_rviz.launch │ ├── meshes │ │ ├── L_calf.STL │ │ ├── L_hip.STL │ │ ├── L_hip2.STL │ │ ├── L_thigh.STL │ │ ├── L_toe.STL │ │ ├── R_calf.STL │ │ ├── R_hip.STL │ │ ├── R_hip2.STL │ │ ├── R_thigh.STL │ │ ├── R_toe.STL │ │ └── base_link.STL │ ├── mjcf │ │ └── pai.xml │ ├── package.xml │ └── xacro │ │ ├── const.xacro │ │ ├── gazebo.xacro │ │ ├── gazebo_hector.xacro │ │ ├── leg.xacro │ │ ├── materials.xacro │ │ ├── pai.urdf │ │ ├── pai.xacro │ │ ├── robot.xacro │ │ ├── stairs.xacro │ │ └── transmission.xacro └── unitree_ros │ ├── LICENSE │ ├── README.md │ ├── unitree_controller │ ├── CMakeLists.txt │ ├── include │ │ └── body.h │ ├── launch │ │ └── set_ctrl.launch │ ├── package.xml │ └── src │ │ ├── body.cpp │ │ ├── external_force.cpp │ │ ├── move_publisher.cpp │ │ └── servo.cpp │ ├── unitree_gazebo │ ├── CMakeLists.txt │ ├── launch │ │ └── biped.launch │ ├── package.xml │ ├── plugin │ │ ├── draw_force_plugin.cc │ │ └── foot_contact_plugin.cc │ └── worlds │ │ ├── big_map.world │ │ ├── box.world │ │ ├── building_editor_models │ │ └── stairs │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── earth.world │ │ ├── house_obstacles.world │ │ ├── maze.world │ │ ├── models │ │ ├── big_map │ │ │ ├── meshes │ │ │ │ └── big_map.stl │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── house_obstacles │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── maze │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── simple_house │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── two_storeys │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── normal.world │ │ ├── simple_house.world │ │ ├── space.world │ │ └── stairs.world │ ├── unitree_legged_control │ ├── CMakeLists.txt │ ├── include │ │ ├── joint_controller.h │ │ └── unitree_joint_control_tool.h │ ├── lib │ │ ├── libunitree_joint_control_tool.so │ │ └── libunitree_joint_control_tool_arm64.so │ ├── package.xml │ ├── src │ │ └── joint_controller.cpp │ └── unitree_controller_plugins.xml │ └── unitree_legged_msgs │ ├── CMakeLists.txt │ ├── msg │ ├── BmsCmd.msg │ ├── BmsState.msg │ ├── Cartesian.msg │ ├── HighCmd.msg │ ├── HighState.msg │ ├── IMU.msg │ ├── LED.msg │ ├── LowCmd.msg │ ├── LowState.msg │ ├── MotorCmd.msg │ └── MotorState.msg │ └── package.xml ├── License.md └── Readme.md /Hector_ROS_Simulation/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/home/obot/catkin_ws/devel/include/**", 10 | "/opt/ros/noetic/include/**", 11 | "/opt/openrobots/include/**", 12 | "/opt/openrobots/include/**", 13 | "/home/obot/catkin_ws/src/Hector_Simulation/Hector_ROS_Simulation/hector_control/include/**", 14 | "/home/obot/catkin_ws/src/Hector_Simulation/Hector_ROS_Simulation/unitree_ros/unitree_controller/include/**", 15 | "/home/obot/catkin_ws/src/Hector_Simulation/Hector_ROS_Simulation/unitree_ros/unitree_legged_control/include/**", 16 | "/usr/include/**" 17 | ], 18 | "name": "ROS", 19 | "intelliSenseMode": "gcc-x64", 20 | "compilerPath": "/usr/bin/gcc", 21 | "cStandard": "gnu11", 22 | "cppStandard": "c++14" 23 | } 24 | ], 25 | "version": 4 26 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/obot/catkin_ws/devel/lib/python3/dist-packages", 4 | "/opt/ros/noetic/lib/python3/dist-packages", 5 | "/opt/openrobots/lib/python3.8/site-packages" 6 | ], 7 | "python.analysis.extraPaths": [ 8 | "/home/obot/catkin_ws/devel/lib/python3/dist-packages", 9 | "/opt/ros/noetic/lib/python3/dist-packages", 10 | "/opt/openrobots/lib/python3.8/site-packages" 11 | ], 12 | "files.associations": { 13 | "*.urdf": "xml", 14 | "*.info": "bat", 15 | "cctype": "cpp", 16 | "clocale": "cpp", 17 | "cmath": "cpp", 18 | "cstdarg": "cpp", 19 | "cstddef": "cpp", 20 | "cstdio": "cpp", 21 | "cstdlib": "cpp", 22 | "cstring": "cpp", 23 | "ctime": "cpp", 24 | "cwchar": "cpp", 25 | "cwctype": "cpp", 26 | "array": "cpp", 27 | "atomic": "cpp", 28 | "bit": "cpp", 29 | "*.tcc": "cpp", 30 | "bitset": "cpp", 31 | "chrono": "cpp", 32 | "compare": "cpp", 33 | "concepts": "cpp", 34 | "cstdint": "cpp", 35 | "deque": "cpp", 36 | "list": "cpp", 37 | "map": "cpp", 38 | "set": "cpp", 39 | "unordered_map": "cpp", 40 | "vector": "cpp", 41 | "exception": "cpp", 42 | "algorithm": "cpp", 43 | "functional": "cpp", 44 | "iterator": "cpp", 45 | "memory": "cpp", 46 | "memory_resource": "cpp", 47 | "optional": "cpp", 48 | "random": "cpp", 49 | "string": "cpp", 50 | "string_view": "cpp", 51 | "system_error": "cpp", 52 | "tuple": "cpp", 53 | "type_traits": "cpp", 54 | "utility": "cpp", 55 | "hash_map": "cpp", 56 | "hash_set": "cpp", 57 | "initializer_list": "cpp", 58 | "iosfwd": "cpp", 59 | "iostream": "cpp", 60 | "istream": "cpp", 61 | "limits": "cpp", 62 | "new": "cpp", 63 | "ostream": "cpp", 64 | "sstream": "cpp", 65 | "stdexcept": "cpp", 66 | "streambuf": "cpp", 67 | "typeinfo": "cpp", 68 | "variant": "cpp", 69 | "dense": "cpp" 70 | } 71 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(hector_control) 3 | add_compile_options(-std=c++11) 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 6 | # set(CMAKE_C_FLAGS "-O3 -ggdb -march=native -std=gnu99 -I.") 7 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 8 | # set(CMAKE_CXX_FLAGS "-lboost_system -fext-numeric-literals -O3") 9 | add_definitions(-DEIGEN_STACK_ALLOCATION_LIMIT=0) 10 | # EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. 11 | # For internal temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size 12 | # matrices or arrays, exceeding this threshold raises a compile time assertion. Use 0 to set no limit. 13 | # Default is 128 KB. 14 | find_package(catkin REQUIRED COMPONENTS 15 | controller_manager 16 | genmsg 17 | joint_state_controller 18 | robot_state_publisher 19 | roscpp 20 | rospy 21 | gazebo_ros 22 | gazebo_msgs 23 | std_msgs 24 | tf 25 | geometry_msgs 26 | unitree_legged_msgs 27 | hector_description 28 | ) 29 | find_package(gazebo REQUIRED) 30 | find_package(Boost REQUIRED) 31 | find_package(pinocchio REQUIRED) 32 | #find_package(qpoases) 33 | catkin_package( 34 | CATKIN_DEPENDS 35 | unitree_legged_msgs 36 | ) 37 | find_package(mujoco) 38 | find_package(glfw3 REQUIRED) 39 | 40 | include_directories(${CMAKE_BINARY_DIR}) 41 | include_directories( 42 | include 43 | ${Boost_INCLUDE_DIR} 44 | ${catkin_INCLUDE_DIRS} 45 | ${GAZEBO_INCLUDE_DIRS} 46 | ${pinocchio_INCLUDE_DIRS} 47 | #${GAZEBO_LIBRARY_DIRS} 48 | ) 49 | 50 | link_directories( 51 | include/sdk/lib 52 | ${GAZEBO_LIBRARY_DIRS} 53 | ) 54 | 55 | add_subdirectory(third_party/qpOASES) 56 | 57 | file(GLOB_RECURSE SRC_LIST 58 | "src/*/*.cpp" 59 | "src/*/*.h" 60 | "ConvexMPC/*.cpp" 61 | ) 62 | list(REMOVE_ITEM SRC_LIST "src/interface/MujocoIO.cpp") 63 | #print all source files 64 | # foreach(_source IN ITEMS ${SRC_LIST}) 65 | # message(STATUS "source file: ${_source}") 66 | # endforeach() 67 | 68 | add_executable(hector_ctrl src/main.cpp src/simulation.cpp ${SRC_LIST} ) 69 | target_link_libraries(hector_ctrl ${catkin_LIBRARIES} qpOASES -pthread lcm mujoco glfw 70 | ${pinocchio_LIBRARIES} 71 | ) 72 | 73 | # add_executable(mj_sim_node src/mjSim.cpp src/simulation.cpp 74 | # src/interface/MujocoIO.cpp 75 | # ${SRC_LIST} ) 76 | # target_link_libraries(mj_sim_node ${catkin_LIBRARIES} qpOASES -pthread lcm mujoco glfw 77 | # ) 78 | 79 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/Constraints.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTRAINT_H 2 | #define CONSTRAINT_H 3 | #include 4 | #include "common_types.h" 5 | #include "RobotState.h" 6 | 7 | /** 8 | * @file Constraints.h 9 | * @brief Constraints calculations for force-and-moment based convex MPC 10 | * 11 | * This file provides the definition for the Constraints class, which is responsible for 12 | * calculating the constraints needed for the convex MPC including friction cone constraints, 13 | * force limits, and moment limits. 14 | * 15 | */ 16 | 17 | class Constraints { 18 | public: 19 | Constraints(RobotState& rs, const Eigen::MatrixXf& q, int horizon, int num_constraints, float motorTorqueLimit, float big_number, float f_max, const Eigen::MatrixXf& gait); 20 | 21 | Eigen::VectorXf GetUpperBound() const; 22 | Eigen::VectorXf GetLowerBound() const; 23 | Eigen::MatrixXf GetConstraintMatrix() const; 24 | 25 | private: 26 | RobotState& rs_; 27 | int Num_Variables; 28 | int Num_Constraints; 29 | int horizon_; 30 | float motorTorqueLimit_; 31 | float BIG_NUMBER_; 32 | float f_max_; 33 | Eigen::MatrixXf gait_; 34 | Eigen::MatrixXf q; 35 | Matrix R_foot_L; 36 | Matrix R_foot_R; 37 | 38 | Eigen::VectorXf U_b; // Upper bound 39 | Eigen::VectorXf L_b; // Lower bound 40 | 41 | Eigen::MatrixXf A_c; // Constraint matrix 42 | 43 | void CalculateUpperBound(); 44 | void CalculateLowerBound(); 45 | void CalculateConstarintMatrix(); 46 | void CalculateFootRotationMatrix(); 47 | }; 48 | 49 | #endif //CONSTRAINT_H 50 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/ConvexMPCLocomotion.h: -------------------------------------------------------------------------------- 1 | #ifndef CONVEXMPCLOCOMOTION_H 2 | #define CONVEXMPCLOCOMOTION_H 3 | 4 | #include 5 | #include "../include/common/FootSwingTrajectory.h" 6 | #include "../include/common/ControlFSMData.h" 7 | #include "../include/common/cppTypes.h" 8 | #include "GaitGenerator.h" 9 | #include 10 | 11 | using Eigen::Array4f; 12 | using Eigen::Array4i; 13 | using Eigen::Array4d; 14 | using Eigen::Array2d; 15 | using Eigen::Array2i; 16 | using Eigen::Array2f; 17 | 18 | using namespace std; 19 | 20 | /** 21 | * @file ConvexMPCLocomotion.h 22 | * @brief Convex Model Predictive Control (MPC) for Bipedal Locomotion 23 | * 24 | * This file defines the ConvexMPCLocomotion class, which implements a convex MPC 25 | * approach to generate and control gait patterns for bipedal robots. The class 26 | * offers functionality to set gait patterns, update the MPC as needed, and track 27 | * the state of the robot in terms of position, orientation, and contact with the ground. 28 | */ 29 | 30 | 31 | struct CMPC_Result { 32 | LegControllerCommand commands[2]; 33 | Vec2 contactPhase; 34 | }; 35 | 36 | class ConvexMPCLocomotion { 37 | public: 38 | // Constructors 39 | ConvexMPCLocomotion(double _dt, int _iterations_between_mpc); 40 | 41 | // Main Functionalities 42 | void run(ControlFSMData& data); 43 | void setGaitNum(int gaitNum) { gaitNumber = gaitNum; } 44 | bool firstRun = true; 45 | 46 | 47 | private: 48 | void updateMPCIfNeeded(int* mpcTable, ControlFSMData& data, bool omniMode); 49 | void GenerateTrajectory(int* mpcTable, ControlFSMData& data, bool omniMode); 50 | 51 | // Locomotion and Gait Parameters 52 | int iterationsBetweenMPC; 53 | int horizonLength; 54 | double dt; 55 | double dtMPC; 56 | int iterationCounter = 0; 57 | Vec6 f_ff[2]; 58 | Vec12 Forces_Sol; 59 | Vec2 swingTimes; 60 | FootSwingTrajectory footSwingTrajectories[2]; 61 | Gait walking, standing; 62 | 63 | // Feedback and Control Variables 64 | Mat3 Kp, Kd, Kp_stance, Kd_stance; 65 | bool firstSwing[2] = {true, true}; 66 | double swingTimeRemaining[2]; 67 | double stand_traj[6]; 68 | int current_gait; 69 | int gaitNumber; 70 | Vec3 world_position_desired; 71 | Vec3 rpy_int; 72 | Vec3 rpy_comp; 73 | Vec3 pFoot[2]; 74 | CMPC_Result result; 75 | double trajAll[12*10]; 76 | Mat43 W; 77 | Vec3 a; 78 | Vec4 pz; 79 | double ground_pitch; 80 | 81 | Vec3 pBody_des; 82 | Vec3 vBody_des; 83 | Vec3 aBody_des; 84 | Vec3 pBody_RPY_des; 85 | Vec3 vBody_Ori_des; 86 | Vec3 pFoot_des[2]; 87 | Vec3 vFoot_des[2]; 88 | Vec3 aFoot_des[2]; 89 | Vec3 Fr_des[2]; 90 | Vec2 contact_state; 91 | Vec3 v_des_robot; 92 | bool climb = 0; 93 | ofstream foot_position; 94 | Vec3 ori_des_world; 95 | }; 96 | 97 | 98 | #endif //CONVEXMPCLOCOMOTION_H 99 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/GaitGenerator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "../include/common/cppTypes.h" 5 | 6 | using Eigen::Array4f; 7 | using Eigen::Array4i; 8 | using Eigen::Array4d; 9 | using Eigen::Array2d; 10 | using Eigen::Array2i; 11 | using Eigen::Array2f; 12 | using namespace std; 13 | 14 | 15 | /** 16 | * @file gaitgenerator.h 17 | * @brief Gait Generation for Bipedal Locomotion 18 | * 19 | * This file provides the definition for the Gait class, which is responsible for 20 | * generating and managing various gait patterns for a bipedal robot. 21 | * 22 | */ 23 | 24 | 25 | class Gait 26 | { 27 | public: 28 | Gait(int nMPC_segments, Vec2 offsets, Vec2 durations, const std::string& name=""); 29 | ~Gait(); 30 | Vec2 getContactSubPhase(); 31 | Vec2 getSwingSubPhase(); 32 | int* mpc_gait(); 33 | void setIterations(int iterationsPerMPC, int currentIteration); 34 | int _stance; 35 | int _swing; 36 | 37 | 38 | private: 39 | int _nMPC_segments; 40 | int* _mpc_table; 41 | Array2i _offsets; // offset in mpc segments 42 | Array2i _durations; // duration of step in mpc segments 43 | Array2d _offsetsPhase; // offsets in phase (0 to 1) 44 | Array2d _durationsPhase; // durations in phase (0 to 1) 45 | int _iteration; 46 | int _nIterations; 47 | int currentIteration; 48 | double _phase; 49 | 50 | }; -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/RobotState.cpp: -------------------------------------------------------------------------------- 1 | #include "RobotState.h" 2 | #include 3 | #include 4 | #include 5 | 6 | using std::cout; 7 | using std::endl; 8 | 9 | void RobotState::set(flt* p_, flt* v_, flt* q_, flt* w_, flt* r_,flt yaw_) 10 | { 11 | for(u8 i = 0; i < 3; i++) 12 | { 13 | this->p(i) = p_[i]; 14 | this->v(i) = v_[i]; 15 | this->w(i) = w_[i]; 16 | } 17 | this->q.w() = q_[0]; 18 | this->q.x() = q_[1]; 19 | this->q.y() = q_[2]; 20 | this->q.z() = q_[3]; 21 | this->yaw = yaw; 22 | 23 | //for(u8 i = 0; i < 12; i++) 24 | // this->r_feet(i) = r[i]; 25 | for(u8 rs = 0; rs < 3; rs++) 26 | for(u8 c = 0; c < 2; c++){ 27 | // std::cout<< "r_ : " << rs << ", "<< c << ": " << r_[rs*2 + c] <r_feet(rs,c) = r_[rs*2 + c]; 29 | } 30 | R = this->q.toRotationMatrix(); 31 | fpt yc = cos(yaw_); 32 | fpt ys = sin(yaw_); 33 | 34 | R_yaw << yc, -ys, 0, 35 | ys, yc, 0, 36 | 0, 0, 1; 37 | 38 | 39 | // R_yaw = R; 40 | // R = R_yaw; 41 | Matrix Id; 42 | 43 | // Id << 0.064f, 0.057f, 0.016f; //trunk 44 | // Id << 0.2351, 0.2230, 0.0323; // adding hips/ 45 | Id << 0.5413, 0.5200, 0.0691; //adding thighs 46 | 47 | I_body.diagonal() = Id; 48 | // I_body << 0.5435, 0.0, -0.0321, 49 | // 0.0, 0.5200, 0.00, 50 | // -0.0321, 0.0, 0.0713; 51 | 52 | //TODO: Consider normalizing quaternion?? 53 | } 54 | 55 | void RobotState::print() 56 | { 57 | cout<<"Robot State:"< 5 | #include "common_types.h" 6 | 7 | using Eigen::Matrix; 8 | using Eigen::Quaternionf; 9 | 10 | #include "common_types.h" 11 | class RobotState 12 | { 13 | public: 14 | void set(flt* p, flt* v, flt* q, flt* w, flt* r, flt yaw); 15 | //void compute_rotations(); 16 | void print(); 17 | Matrix p,v,w; 18 | Matrix r_feet; 19 | Matrix R; 20 | Matrix R_yaw; 21 | Matrix I_body; 22 | Quaternionf q; 23 | fpt yaw; 24 | //fpt m = 19; // Aliengo 25 | fpt m = 13; // A1 26 | //private: 27 | }; 28 | #endif 29 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/SolverMPC.h: -------------------------------------------------------------------------------- 1 | #ifndef _solver_mpc 2 | #define _solver_mpc 3 | 4 | 5 | #include 6 | #include "common_types.h" 7 | #include "convexMPC_interface.h" 8 | #include 9 | #include 10 | #include "Constraints.h" 11 | 12 | using Eigen::Matrix; 13 | using Eigen::Quaternionf; 14 | using Eigen::Quaterniond; 15 | 16 | template 17 | void print_array(T* array, u16 rows, u16 cols) 18 | { 19 | for(u16 r = 0; r < rows; r++) 20 | { 21 | for(u16 c = 0; c < cols; c++) 22 | std::cout<<(fpt)array[c+r*cols]<<" "; 23 | printf("\n"); 24 | } 25 | } 26 | 27 | template 28 | void print_named_array(const char* name, T* array, u16 rows, u16 cols) 29 | { 30 | printf("%s:\n",name); 31 | print_array(array,rows,cols); 32 | } 33 | 34 | //print named variable 35 | template 36 | void pnv(const char* name, T v) 37 | { 38 | printf("%s: ",name); 39 | std::cout< 43 | T t_min(T a, T b) 44 | { 45 | if(a 50 | T sq(T a) 51 | { 52 | return a*a; 53 | } 54 | 55 | 56 | void solve_mpc(update_data_t* update, problem_setup* setup); 57 | void quat_to_rpy(Quaternionf q, Matrix& rpy); 58 | void ct_ss_mats(Matrix I_world, fpt m, Matrix r_feet, Matrix R_yaw, Matrix& A, Matrix& B); 59 | void resize_qp_mats(s16 horizon); 60 | void c2qp(Matrix Ac, Matrix Bc,fpt dt,s16 horizon); 61 | void leg_Jv(Matrix R, double q0, double q1, double q2, double q3, double q4,int leg); 62 | void leg_Jw(Matrix R, double q0, double q1, double q2, double q3, double q4,int leg); 63 | mfp* get_q_soln(); 64 | #endif 65 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/common_types.h: -------------------------------------------------------------------------------- 1 | #ifndef _common_types 2 | #define _common_types 3 | #include 4 | #include 5 | 6 | //adding this line adds print statements and sanity checks 7 | //that are too slow for realtime use. 8 | //#define K_DEBUG 9 | 10 | typedef double dbl; 11 | typedef float flt; 12 | 13 | //floating point type used whenever possible 14 | typedef float fpt; 15 | 16 | //floating point type used when interfacing with MATLAB 17 | typedef double mfp; 18 | typedef int mint; 19 | 20 | typedef uint64_t u64; 21 | typedef uint32_t u32; 22 | typedef uint16_t u16; 23 | typedef uint8_t u8; 24 | typedef int8_t s8; 25 | typedef int16_t s16; 26 | typedef int32_t s32; 27 | typedef int64_t s64; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/ConvexMPC/convexMPC_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef _convexmpc_interface 2 | #define _convexmpc_interface 3 | #define K_MAX_GAIT_SEGMENTS 36 4 | 5 | #ifdef __cplusplus 6 | #define EXTERNC extern "C" 7 | #else 8 | #define EXTERNC 9 | #endif 10 | 11 | struct problem_setup 12 | { 13 | float dt; 14 | float mu; 15 | float f_max; 16 | int horizon; 17 | }; 18 | 19 | struct update_data_t 20 | { 21 | float p[3]; 22 | float v[3]; 23 | float q[4]; 24 | float w[3]; 25 | float r[6]; 26 | float joint_angles[10]; 27 | float yaw; 28 | float weights[12]; 29 | float traj[12*K_MAX_GAIT_SEGMENTS]; 30 | 31 | // float alpha; 32 | float Alpha_K[12]; 33 | unsigned char gait[K_MAX_GAIT_SEGMENTS]; 34 | unsigned char hack_pad[1000]; 35 | int max_iterations; 36 | double rho, sigma, solver_alpha, terminate; 37 | }; 38 | 39 | EXTERNC void setup_problem(double dt, int horizon, double mu, double f_max); 40 | EXTERNC double get_solution(int index); 41 | EXTERNC void update_solver_settings(int max_iter, double rho, double sigma, double solver_alpha, double terminate, double use_jcqp); 42 | 43 | EXTERNC void update_problem_data(double* p, double* v, double* q, double* w, double* r, double* joint_angles, double yaw, double* weights, double* state_trajectory, double* Alpha_K, int* gait); 44 | 45 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/FSM/FSM.h: -------------------------------------------------------------------------------- 1 | #ifndef FSM_H 2 | #define FSM_H 3 | 4 | #include 5 | #include "FSMState.h" 6 | #include "FSMState_Passive.h" 7 | #include "FSMState_Walking.h" 8 | #include "../common/enumClass.h" 9 | 10 | struct FSMStateList{ 11 | FSMState *invalid; 12 | FSMState_Passive *passive; 13 | FSMState_Walking *walking; 14 | 15 | 16 | void deletePtr(){ 17 | delete invalid; 18 | delete passive; 19 | delete walking; 20 | } 21 | }; 22 | 23 | class FSM{ 24 | public: 25 | FSM(std::shared_ptr data); 26 | ~FSM(); 27 | void initialize(); 28 | void run(); 29 | private: 30 | FSMState* getNextState(FSMStateName stateName); 31 | bool checkSafty(); 32 | std::shared_ptr _data; 33 | FSMState *_currentState; 34 | FSMState *_nextState; 35 | FSMStateName _nextStateName; 36 | FSMStateList _stateList; 37 | FSMMode _mode; 38 | long long _startTime; 39 | int count; 40 | }; 41 | 42 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/FSM/FSMState.h: -------------------------------------------------------------------------------- 1 | #ifndef FSMSTATE_H 2 | #define FSMSTATE_H 3 | 4 | #include 5 | #include 6 | #include "../common/ControlFSMData.h" 7 | #include "../common/cppTypes.h" 8 | #include "../common/enumClass.h" 9 | #include "../interface/CmdPanel.h" 10 | #include "../messages/LowLevelCmd.h" 11 | #include "../messages/LowlevelState.h" 12 | 13 | class FSMState 14 | { 15 | public: 16 | FSMState(std::shared_ptr data, FSMStateName stateName, std::string stateNameStr); 17 | 18 | virtual void enter() = 0; 19 | virtual void run() = 0; 20 | virtual void exit() = 0; 21 | virtual FSMStateName checkTransition() {return FSMStateName::INVALID;} 22 | 23 | FSMStateName _stateName; 24 | std::string _stateNameStr; 25 | 26 | protected: 27 | std::shared_ptr _data; 28 | FSMStateName _nextStateName; 29 | 30 | std::shared_ptr _lowCmd; 31 | LowlevelState *_lowState; 32 | UserValue _userValue; 33 | }; 34 | 35 | #endif // FSMSTATE_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/FSM/FSMState_Passive.h: -------------------------------------------------------------------------------- 1 | #ifndef PASSIVE_H 2 | #define PASSIVE_H 3 | 4 | #include "FSMState.h" 5 | 6 | class FSMState_Passive: public FSMState 7 | { 8 | public: 9 | FSMState_Passive(std::shared_ptr data); 10 | void enter(); 11 | void run(); 12 | void exit(); 13 | FSMStateName checkTransition(); 14 | }; 15 | 16 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/FSM/FSMState_Walking.h: -------------------------------------------------------------------------------- 1 | #ifndef WALKING_H 2 | #define WALKING_H 3 | 4 | #include "FSMState.h" 5 | #include "../../ConvexMPC/ConvexMPCLocomotion.h" 6 | 7 | class FSMState_Walking: public FSMState 8 | { 9 | public: 10 | FSMState_Walking(std::shared_ptr data); 11 | ~FSMState_Walking(){} 12 | void enter(); 13 | void run(); 14 | void exit(); 15 | FSMStateName checkTransition(); 16 | 17 | private: 18 | ConvexMPCLocomotion Cmpc; 19 | int counter; 20 | Vec3 v_des_body; 21 | double turn_rate = 0; 22 | double pitch, roll; 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Biped.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT_BIPED_H 2 | #define PROJECT_BIPED_H 3 | 4 | #include "cppTypes.h" 5 | #include "pinocchio/algorithm/frames.hpp" 6 | #include "pinocchio/algorithm/jacobian.hpp" 7 | #include "pinocchio/algorithm/joint-configuration.hpp" 8 | #include "pinocchio/algorithm/kinematics.hpp" 9 | #include "pinocchio/parsers/urdf.hpp" 10 | #include 11 | #include 12 | #include 13 | class Biped { 14 | public: 15 | void setBiped() { 16 | 17 | mass = 13.856; 18 | 19 | leg_offset_x = 0.0; 20 | leg_offset_y = 0.047; // 0.057; 21 | leg_offset_z = -0.1360; //-0.125; 22 | 23 | leg_offset_x2 = 0.0; 24 | leg_offset_y2 = 0.047; // 0.057; 25 | leg_offset_z2 = -0.136; 26 | 27 | hipLinkLength = 0.038; // hip offset in const.xacro 28 | thighLinkLength = 0.22; 29 | calfLinkLength = 0.22; 30 | std::string path = ros::package::getPath("hector_description"); 31 | path += "/xacro/hector.urdf"; 32 | pinocchio::urdf::buildModel(path, model_); 33 | data_ = pinocchio::Data(model_); 34 | q_.setZero(10); 35 | } 36 | int robot_index; // 1 for Aliengo, 2 for A1 37 | double hipLinkLength; 38 | double thighLinkLength; 39 | double calfLinkLength; 40 | double leg_offset_x; 41 | double leg_offset_y; 42 | double leg_offset_z; 43 | double leg_offset_x2; 44 | double leg_offset_y2; 45 | double leg_offset_z2; 46 | double mass; 47 | pinocchio::Model model_; 48 | pinocchio::Data data_; 49 | Eigen::VectorXd q_; 50 | Vec3 getHipLocation(int leg) { 51 | assert(leg >= 0 && leg < 2); 52 | Vec3 pHip = Vec3::Zero(); 53 | if (leg == 0) { 54 | pHip(0) = leg_offset_x; 55 | pHip(1) = leg_offset_y; 56 | pHip(2) = leg_offset_z; 57 | } 58 | if (leg == 1) { 59 | pHip(0) = leg_offset_x; 60 | pHip(1) = -leg_offset_y; 61 | pHip(2) = leg_offset_z; 62 | } 63 | return pHip; 64 | }; 65 | 66 | Vec3 getHip2Location(int leg) { 67 | assert(leg >= 0 && leg < 2); 68 | Vec3 pHip2 = Vec3::Zero(); 69 | if (leg == 0) { 70 | pHip2(0) = leg_offset_x2; 71 | pHip2(1) = leg_offset_y2; 72 | pHip2(2) = leg_offset_z2; 73 | } 74 | if (leg == 1) { 75 | pHip2(0) = leg_offset_x2; 76 | pHip2(1) = -leg_offset_y2; 77 | pHip2(2) = leg_offset_z2; 78 | } 79 | return pHip2; 80 | }; 81 | }; 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/CheaterEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef CHEATER_ESTIMATOR 2 | #define CHEATER_ESTIMATOR 3 | 4 | #include "StateEstimatorContainer.h" 5 | #include "Math/orientation_tools.h" 6 | 7 | class CheaterRobotStateEstimator: public GenericEstimator{ 8 | public: 9 | virtual void run() 10 | { 11 | this->_stateEstimatorData.result->orientation[0] = this->_stateEstimatorData.lowState->imu.quaternion[0]; 12 | this->_stateEstimatorData.result->orientation[1] = this->_stateEstimatorData.lowState->imu.quaternion[1]; 13 | this->_stateEstimatorData.result->orientation[2] = this->_stateEstimatorData.lowState->imu.quaternion[2]; 14 | this->_stateEstimatorData.result->orientation[3] = this->_stateEstimatorData.lowState->imu.quaternion[3]; 15 | 16 | this->_stateEstimatorData.result->rBody = ori::quaternionToRotationMatrix( 17 | this->_stateEstimatorData.result->orientation); 18 | // when wrting to this variable, we write the world frame omega into it instead of IMU reading 19 | this->_stateEstimatorData.result->omegaWorld[0] = this->_stateEstimatorData.lowState->imu.gyroscope[0]; 20 | this->_stateEstimatorData.result->omegaWorld[1] = this->_stateEstimatorData.lowState->imu.gyroscope[1]; 21 | this->_stateEstimatorData.result->omegaWorld[2] = this->_stateEstimatorData.lowState->imu.gyroscope[2]; 22 | 23 | this->_stateEstimatorData.result->omegaBody = 24 | this->_stateEstimatorData.result->rBody * this->_stateEstimatorData.result->omegaWorld; 25 | this->_stateEstimatorData.result->rpy = 26 | ori::quatToRPY(this->_stateEstimatorData.result->orientation); 27 | 28 | this->_stateEstimatorData.result->vWorld[0] = this->_stateEstimatorData.lowState->vWorld[0]; 29 | this->_stateEstimatorData.result->vWorld[1] = this->_stateEstimatorData.lowState->vWorld[1]; 30 | this->_stateEstimatorData.result->vWorld[2] = this->_stateEstimatorData.lowState->vWorld[2]; 31 | 32 | this->_stateEstimatorData.result->position[0] = this->_stateEstimatorData.lowState->position[0]; 33 | this->_stateEstimatorData.result->position[1] = this->_stateEstimatorData.lowState->position[1]; 34 | this->_stateEstimatorData.result->position[2] = this->_stateEstimatorData.lowState->position[2]; 35 | // don't need acceleration at all 36 | this->_stateEstimatorData.result->aBody[0] = 0; 37 | this->_stateEstimatorData.result->aBody[1] = 0; 38 | this->_stateEstimatorData.result->aBody[2] = 0; 39 | 40 | this->_stateEstimatorData.result->aWorld = 41 | this->_stateEstimatorData.result->rBody.transpose() * 42 | this->_stateEstimatorData.result->aBody; 43 | } 44 | virtual void setup() {} 45 | }; 46 | 47 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/ControlFSMData.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLFSMDATA_H 2 | #define CONTROLFSMDATA_H 3 | 4 | #include "DesiredCommand.h" 5 | #include "LegController.h" 6 | #include "Biped.h" 7 | #include "../messages/LowLevelCmd.h" 8 | #include "../messages/LowlevelState.h" 9 | #include "../interface/IOInterface.h" 10 | #include "StateEstimatorContainer.h" 11 | 12 | struct ControlFSMData { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | Biped *_biped; 15 | StateEstimatorContainer *_stateEstimator; 16 | LegController *_legController; 17 | DesiredStateCommand *_desiredStateCommand; 18 | IOInterface *_interface; 19 | std::shared_ptr _lowCmd; 20 | LowlevelState *_lowState; 21 | 22 | void sendRecv(){ 23 | _interface->sendRecv(_lowCmd, _lowState); 24 | } 25 | }; 26 | 27 | 28 | #endif // CONTROLFSM_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/DesiredCommand.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file DesiredCommand.h 3 | * @brief convert keyborad/gamepad command into desired 4 | * tracjectory for the robot 5 | */ 6 | 7 | #ifndef DESIREDCOMMAND_H 8 | #define DESIREDCOMMAND_H 9 | 10 | #include "cppTypes.h" 11 | #include 12 | #include 13 | #include 14 | #include "StateEstimatorContainer.h" 15 | #include "../interface/CmdPanel.h" 16 | 17 | struct DesiredStateData{ 18 | 19 | DesiredStateData() { zero(); } 20 | 21 | // Zero all data 22 | void zero(); 23 | 24 | // Instataneous desired state comman 25 | Vec12 stateDes; 26 | Vec12 pre_stateDes; 27 | 28 | int mode; 29 | }; 30 | 31 | class DesiredStateCommand { 32 | public: 33 | // Initialize 34 | DesiredStateCommand(StateEstimate* _estimate, double _dt){ 35 | stateEstimate = _estimate; 36 | dt = _dt; 37 | } 38 | void convertToStateCommands(UserValue _userValue); 39 | void setStateCommands(double r, double p, Vec3 v_des, double yaw_rate); 40 | void setmode(int ctrl_mode) {data.mode = ctrl_mode;} 41 | double deadband(double command, double minVal, double maxVal); 42 | // These should come from the inferface 43 | 44 | bool firstRun = true; 45 | 46 | double maxRoll = 0.4; 47 | double minRoll = -0.4; 48 | double maxPitch = 0.4; 49 | double minPitch = -0.4; 50 | double maxVelX = 2.0; 51 | double minVelX = -2.0; 52 | //double maxVelX = 5.0; 53 | //double minVelX = -5.0; 54 | double maxVelY = 0.5; 55 | double minVelY = -0.5; 56 | double maxTurnRate = 2.0; 57 | double minTurnRate = -2.0; 58 | DesiredStateData data; 59 | 60 | //~DesiredStateCommand(); 61 | private: 62 | StateEstimate* stateEstimate; 63 | 64 | double dt; // Control loop time step 65 | }; 66 | 67 | 68 | 69 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/FootSwingTrajectory.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file FootSwingTrajectory.h 3 | * @brief Utility to generate foot swing trajectories. 4 | * 5 | * Currently uses Bezier curves like Cheetah 3 does 6 | */ 7 | 8 | #ifndef CHEETAH_SOFTWARE_FOOTSWINGTRAJECTORY_H 9 | #define CHEETAH_SOFTWARE_FOOTSWINGTRAJECTORY_H 10 | 11 | #include "cppTypes.h" 12 | 13 | /*! 14 | * A foot swing trajectory for a single foot 15 | */ 16 | template 17 | class FootSwingTrajectory { 18 | public: 19 | 20 | /*! 21 | * Construct a new foot swing trajectory with everything set to zero 22 | */ 23 | FootSwingTrajectory() { 24 | _p0.setZero(); 25 | _pf.setZero(); 26 | _p.setZero(); 27 | _v.setZero(); 28 | _height = 0; 29 | } 30 | 31 | /*! 32 | * Set the starting location of the foot 33 | * @param p0 : the initial foot position 34 | */ 35 | void setInitialPosition(Vec3 p0) { 36 | _p0 = p0; 37 | } 38 | 39 | /*! 40 | * Set the desired final position of the foot 41 | * @param pf : the final foot posiiton 42 | */ 43 | void setFinalPosition(Vec3 pf) { 44 | _pf = pf; 45 | } 46 | 47 | /*! 48 | * Set the maximum height of the swing 49 | * @param h : the maximum height of the swing, achieved halfway through the swing 50 | */ 51 | void setHeight(T h) { 52 | _height = h; 53 | } 54 | 55 | void computeSwingTrajectoryBezier(T phase, T swingTime); 56 | 57 | /*! 58 | * Get the foot position at the current point along the swing 59 | * @return : the foot position 60 | */ 61 | Vec3 getPosition() { 62 | return _p; 63 | } 64 | 65 | /*! 66 | * Get the foot velocity at the current point along the swing 67 | * @return : the foot velocity 68 | */ 69 | Vec3 getVelocity() { 70 | return _v; 71 | } 72 | 73 | private: 74 | Vec3 _p0, _pf, _p, _v; 75 | T _height; 76 | }; 77 | 78 | 79 | #endif //CHEETAH_SOFTWARE_FOOTSWINGTRAJECTORY_H 80 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Math/FirstOrderIIRFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2019 MIT Biomimetics Robotics Lab 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | /*! @file FirstOrderIIRFilter.h 26 | * @brief A simple first order filter 27 | * 28 | * 29 | */ 30 | 31 | #ifndef PROJECT_FIRSTORDERIIRFILTER_H 32 | #define PROJECT_FIRSTORDERIIRFILTER_H 33 | 34 | #include 35 | 36 | /*! 37 | * First Order Filter 38 | * @tparam T : type of the data to be filtered 39 | * @tparam T2 : floating point type for the cutoff/sample frequencies, gain 40 | */ 41 | template 42 | class FirstOrderIIRFilter { 43 | public: 44 | /*! 45 | * Create a new first order filter 46 | * @param cutoffFrequency : cutoff frequency of filter 47 | * @param sampleFrequency : sample frequency (rate at which update is called) 48 | * @param initialialValue : initial value stored in the filter 49 | */ 50 | FirstOrderIIRFilter(T2 cutoffFrequency, T2 sampleFrequency, T& initialValue) { 51 | _alpha = 1 - std::exp(-2 * M_PI * cutoffFrequency / sampleFrequency); 52 | _state = initialValue; 53 | } 54 | 55 | /*! 56 | * Create a new first order filter 57 | * @param alpha : filter parameter 58 | * @param initialValue : initial value 59 | */ 60 | FirstOrderIIRFilter(T2 alpha, T& initialValue) 61 | : _state(initialValue), _alpha(alpha) {} 62 | 63 | /*! 64 | * Update the filter with a new sample 65 | * @param x : the new sample 66 | * @return the new state of the filter 67 | */ 68 | T update(T& x) { 69 | _state = _alpha * x + (T2(1) - _alpha) * _state; 70 | return _state; 71 | } 72 | 73 | /*! 74 | * Get the value of the filter, without updating 75 | */ 76 | T get() { return _state; } 77 | 78 | /*! 79 | * Reset the filter to zero. 80 | */ 81 | void reset() { _state *= T2(0); } 82 | 83 | private: 84 | T _state; 85 | T2 _alpha; 86 | }; 87 | 88 | #endif // PROJECT_FIRSTORDERIIRFILTER_H 89 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Math/Interpolation.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2019 MIT Biomimetics Robotics Lab 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | /*! @file Interpolation.h 26 | * @brief Utility functions to interpolate between two values 27 | * 28 | */ 29 | 30 | #ifndef PROJECT_INTERPOLATION_H 31 | #define PROJECT_INTERPOLATION_H 32 | 33 | #include 34 | #include 35 | 36 | namespace Interpolate { 37 | 38 | /*! 39 | * Linear interpolation between y0 and yf. x is between 0 and 1 40 | */ 41 | template 42 | y_t lerp(y_t y0, y_t yf, x_t x) { 43 | static_assert(std::is_floating_point::value, 44 | "must use floating point value"); 45 | assert(x >= 0 && x <= 1); 46 | return y0 + (yf - y0) * x; 47 | } 48 | 49 | /*! 50 | * Cubic bezier interpolation between y0 and yf. x is between 0 and 1 51 | */ 52 | template 53 | y_t cubicBezier(y_t y0, y_t yf, x_t x) { 54 | static_assert(std::is_floating_point::value, 55 | "must use floating point value"); 56 | assert(x >= 0 && x <= 1); 57 | y_t yDiff = yf - y0; 58 | x_t bezier = x * x * x + x_t(3) * (x * x * (x_t(1) - x)); 59 | return y0 + bezier * yDiff; 60 | } 61 | 62 | /*! 63 | * Cubic bezier interpolation derivative between y0 and yf. x is between 0 and 64 | * 1 65 | */ 66 | template 67 | y_t cubicBezierFirstDerivative(y_t y0, y_t yf, x_t x) { 68 | static_assert(std::is_floating_point::value, 69 | "must use floating point value"); 70 | assert(x >= 0 && x <= 1); 71 | y_t yDiff = yf - y0; 72 | x_t bezier = x_t(6) * x * (x_t(1) - x); 73 | return bezier * yDiff; 74 | } 75 | 76 | /*! 77 | * Cubic bezier interpolation derivative between y0 and yf. x is between 0 and 78 | * 1 79 | */ 80 | template 81 | y_t cubicBezierSecondDerivative(y_t y0, y_t yf, x_t x) { 82 | static_assert(std::is_floating_point::value, 83 | "must use floating point value"); 84 | assert(x >= 0 && x <= 1); 85 | y_t yDiff = yf - y0; 86 | x_t bezier = - x_t(12) * x; 87 | return bezier * yDiff; 88 | } 89 | 90 | } // namespace Interpolate 91 | 92 | #endif // PROJECT_INTERPOLATION_H 93 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Math/MathUtilities.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2019 MIT Biomimetics Robotics Lab 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | /*! @file MathUtilities.h 26 | * @brief Utility functions for math 27 | * 28 | */ 29 | 30 | #ifndef PROJECT_MATHUTILITIES_H 31 | #define PROJECT_MATHUTILITIES_H 32 | 33 | #include 34 | 35 | /*! 36 | * Square a number 37 | */ 38 | template 39 | T square(T a) { 40 | return a * a; 41 | } 42 | 43 | /*! 44 | * Are two eigen matrices almost equal? 45 | */ 46 | template 47 | bool almostEqual(const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, 48 | T2 tol) { 49 | long x = T::RowsAtCompileTime; 50 | long y = T::ColsAtCompileTime; 51 | 52 | if (T::RowsAtCompileTime == Eigen::Dynamic || 53 | T::ColsAtCompileTime == Eigen::Dynamic) { 54 | assert(a.rows() == b.rows()); 55 | assert(a.cols() == b.cols()); 56 | x = a.rows(); 57 | y = a.cols(); 58 | } 59 | 60 | for (long i = 0; i < x; i++) { 61 | for (long j = 0; j < y; j++) { 62 | T2 error = std::abs(a(i, j) - b(i, j)); 63 | if (error >= tol) return false; 64 | } 65 | } 66 | return true; 67 | } 68 | 69 | #endif // PROJECT_MATHUTILITIES_H 70 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/OrientationEstimator.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file 3 | * @brief Orientation Estimation Algorithms 4 | * 5 | * orientation: quaternion 6 | * rBody: transformation matrix( vBody = Rbody * vWorld) 7 | * omegaBody: angular vel in body frame 8 | * omegaWorld: ... in world frame 9 | * rpy: roll pitch yaw 10 | */ 11 | 12 | #ifndef PROJECT_ORIENTATIONESTIMATOR_H 13 | #define PROJECT_ORIENTATIONESTIMATOR_H 14 | 15 | #include "StateEstimatorContainer.h" 16 | #include "Math/orientation_tools.h" 17 | 18 | /*! 19 | * "Cheater" estimator for orientation which always returns the correct value in simulation 20 | */ 21 | 22 | class CheaterOrientationEstimator : public GenericEstimator { 23 | public: 24 | virtual void run(); 25 | virtual void setup() {} 26 | }; 27 | 28 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/PositionVelocityEstimator.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file PositionVelocityEstimator.h 3 | * @brief compute body position/velocity in world/body frames 4 | */ 5 | 6 | 7 | 8 | #ifndef PROJECT_POSITIONVELOCITYESTIMATOR_H 9 | #define PROJECT_POSITIONVELOCITYESTIMATOR_H 10 | #include "StateEstimatorContainer.h" 11 | 12 | class CheaterPositionVelocityEstimator : public GenericEstimator{ 13 | public: 14 | virtual void run(); 15 | virtual void setup() {}; 16 | }; 17 | 18 | 19 | class LinearKFPositionVelocityEstimator : public GenericEstimator{ 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | LinearKFPositionVelocityEstimator(){} 23 | virtual void run(); 24 | virtual void setup(); 25 | 26 | private: 27 | Eigen::Matrix _xhat; 28 | Eigen::Matrix _ps; 29 | Eigen::Matrix _vs; 30 | Eigen::Matrix _A; 31 | Eigen::Matrix _Q0; 32 | Eigen::Matrix _P; 33 | Eigen::Matrix _R0; 34 | Eigen::Matrix _B; 35 | Eigen::Matrix _C; 36 | }; 37 | 38 | 39 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Utilities/BezierCurve.h: -------------------------------------------------------------------------------- 1 | #ifndef BEZIER_CURVE 2 | #define BEZIER_CURVE 3 | 4 | #include 5 | 6 | // N = NUM_CTR_PT -1 7 | template 8 | class BezierCurve { 9 | public: 10 | T _CtrlPt[NUM_CTR_PT][DIM]; 11 | T _coeff[NUM_CTR_PT]; 12 | T _end_time; 13 | BezierCurve() { 14 | for (int j(0); j < NUM_CTR_PT; ++j) { 15 | for (int i(0); i < DIM; ++i) { 16 | _CtrlPt[j][i] = 0.; 17 | } 18 | _coeff[j] = 0.; 19 | } 20 | } 21 | 22 | // ctrl pt 0: initial 23 | // ctrl pt n-1(end): final 24 | bool SetParam(T** ctrl_pt, T fin_time) { 25 | _end_time = fin_time; 26 | T n_fact = _factorial(NUM_CTR_PT - 1); 27 | for (int j(0); j < NUM_CTR_PT; ++j) { 28 | for (int i(0); i < DIM; ++i) { 29 | _CtrlPt[j][i] = ctrl_pt[j][i]; 30 | } 31 | _coeff[j] = n_fact / (_factorial(j) * _factorial((NUM_CTR_PT - 1) - j)); 32 | } 33 | return true; 34 | } 35 | 36 | bool getCurvePoint(T u, T* ret) { 37 | if (u > _end_time) { 38 | for (int i(0); i < DIM; ++i) { 39 | ret[i] = _CtrlPt[NUM_CTR_PT - 1][i]; 40 | } 41 | return true; 42 | } else if (u < 0.) { 43 | for (int i(0); i < DIM; ++i) { 44 | ret[i] = _CtrlPt[0][i]; 45 | } 46 | return true; 47 | } else { 48 | u /= _end_time; 49 | for (int i(0); i < DIM; ++i) { 50 | ret[i] = 0.; 51 | for (int j(0); j < NUM_CTR_PT; ++j) { 52 | ret[i] += _coeff[j] * pow(u, j) * pow(1 - u, (NUM_CTR_PT - 1) - j) * 53 | _CtrlPt[j][i]; 54 | } 55 | } 56 | } 57 | return true; 58 | } 59 | 60 | bool getCurveVelocity(T u, T* ret) { 61 | if (u > _end_time) { 62 | for (int i(0); i < DIM; ++i) { 63 | ret[i] = 0.; 64 | } 65 | return true; 66 | } else if (u < 0.) { 67 | for (int i(0); i < DIM; ++i) { 68 | ret[i] = 0.; 69 | } 70 | return true; 71 | } else { 72 | u /= _end_time; 73 | for (int i(0); i < DIM; ++i) { 74 | ret[i] = 0.; 75 | ret[i] += _coeff[0] * 76 | (-(NUM_CTR_PT - 1) * pow(1 - u, (NUM_CTR_PT - 1) - 1)) * 77 | _CtrlPt[0][i]; 78 | for (int j(1); j < NUM_CTR_PT - 1; ++j) { 79 | ret[i] += _coeff[j] * 80 | (j * pow(u, j - 1) * pow(1 - u, (NUM_CTR_PT - 1) - j) - 81 | (NUM_CTR_PT - 1 - j) * pow(u, j) * 82 | pow(1 - u, (NUM_CTR_PT - 1) - j - 1)) * 83 | _CtrlPt[j][i]; 84 | } 85 | ret[i] += _coeff[NUM_CTR_PT - 1] * (NUM_CTR_PT - 1) * 86 | pow(u, NUM_CTR_PT - 2) * _CtrlPt[NUM_CTR_PT - 1][i]; 87 | 88 | ret[i] /= _end_time; 89 | } 90 | } 91 | return true; 92 | } 93 | 94 | private: 95 | int _factorial(const int& n) { 96 | int ret = 1; 97 | for (int i(1); i < n + 1; ++i) ret *= (i); 98 | 99 | return ret; 100 | } 101 | }; 102 | #endif 103 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Utilities/Timer.h: -------------------------------------------------------------------------------- 1 | /*! @file Timer.h 2 | * @brief Timer for measuring how long things take 3 | */ 4 | 5 | #ifndef PROJECT_TIMER_H 6 | #define PROJECT_TIMER_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | /*! 13 | * Timer for measuring time elapsed with clock_monotonic 14 | */ 15 | class Timer { 16 | public: 17 | 18 | /*! 19 | * Construct and start timer 20 | */ 21 | explicit Timer() { start(); } 22 | 23 | /*! 24 | * Start the timer 25 | */ 26 | void start() { clock_gettime(CLOCK_MONOTONIC, &_startTime); } 27 | 28 | /*! 29 | * Get milliseconds elapsed 30 | */ 31 | double getMs() { return (double)getNs() / 1.e6; } 32 | 33 | /*! 34 | * Get nanoseconds elapsed 35 | */ 36 | int64_t getNs() { 37 | struct timespec now; 38 | clock_gettime(CLOCK_MONOTONIC, &now); 39 | return (int64_t)(now.tv_nsec - _startTime.tv_nsec) + 40 | 1000000000 * (now.tv_sec - _startTime.tv_sec); 41 | } 42 | 43 | /*! 44 | * Get seconds elapsed 45 | */ 46 | double getSeconds() { return (double)getNs() / 1.e9; } 47 | 48 | struct timespec _startTime; 49 | }; 50 | 51 | #endif // PROJECT_TIMER_H 52 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/Utilities/pseudoInverse.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "../cppTypes.h" 5 | using namespace std; 6 | 7 | /*! 8 | * Compute the pseudo inverse of a matrix 9 | * @param matrix : input matrix 10 | * @param sigmaThreshold : threshold for singular values being zero 11 | * @param invMatrix : output matrix 12 | */ 13 | template 14 | void pseudoInverse(DMat const& matrix, double sigmaThreshold, 15 | DMat& invMatrix) { 16 | if ((1 == matrix.rows()) && (1 == matrix.cols())) { 17 | invMatrix.resize(1, 1); 18 | if (matrix.coeff(0, 0) > sigmaThreshold) { 19 | invMatrix.coeffRef(0, 0) = 1.0 / matrix.coeff(0, 0); 20 | } else { 21 | invMatrix.coeffRef(0, 0) = 0.0; 22 | } 23 | return; 24 | }; 25 | 26 | Eigen::JacobiSVD> svd(matrix, 27 | Eigen::ComputeThinU | Eigen::ComputeThinV); 28 | // not sure if we need to svd.sort()... probably not 29 | int const nrows(svd.singularValues().rows()); 30 | DMat invS; 31 | invS = DMat::Zero(nrows, nrows); 32 | for (int ii(0); ii < nrows; ++ii) { 33 | if (svd.singularValues().coeff(ii) > sigmaThreshold) { 34 | invS.coeffRef(ii, ii) = 1.0 / svd.singularValues().coeff(ii); 35 | } else { 36 | // invS.coeffRef(ii, ii) = 1.0/ sigmaThreshold; 37 | // printf("sigular value is too small: %f\n", 38 | // svd.singularValues().coeff(ii)); 39 | } 40 | } 41 | invMatrix = svd.matrixV() * invS * svd.matrixU().transpose(); 42 | }; 43 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/cTypes.h: -------------------------------------------------------------------------------- 1 | /*! @file cTypes.h 2 | * @brief Common types that can also be included in C code 3 | * 4 | * This file contains types which are shared between C and C++ code. The 5 | * low-level drivers (rt folder) are all in C and everything else is C++. 6 | * Because this file is included in both C and C++, it can't contain C++ type 7 | * alias ("using"), namespaces, or templates. 8 | */ 9 | 10 | #ifndef PROJECT_CTYPES_H 11 | #define PROJECT_CTYPES_H 12 | 13 | #include // for size_t 14 | #include 15 | 16 | // short version of the stdint default integer types 17 | typedef uint64_t u64; 18 | typedef uint32_t u32; 19 | typedef uint16_t u16; 20 | typedef uint8_t u8; 21 | typedef int8_t s8; 22 | typedef int16_t s16; 23 | typedef int32_t s32; 24 | typedef int64_t s64; 25 | 26 | #endif // PROJECT_CTYPES_H 27 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/common/enumClass.h: -------------------------------------------------------------------------------- 1 | #ifndef ENUMCLASS_H 2 | #define ENUMCLASS_H 3 | 4 | #include 5 | #include 6 | 7 | enum class CtrlPlatform{ 8 | GAZEBO_A1, 9 | REAL_A1 10 | }; 11 | 12 | enum class UserCommand{ 13 | // EXIT, 14 | NONE, 15 | START, // walking 16 | L2_A, // fixedStand 17 | L2_B, // passive 18 | L2_X, // pushing 19 | L2_Y, // probe 20 | L1_X, // QPStand 21 | L1_A, 22 | L1_Y 23 | }; 24 | 25 | enum class FSMMode{ 26 | NORMAL, 27 | CHANGE 28 | }; 29 | 30 | enum class FSMStateName{ 31 | // EXIT, 32 | INVALID, 33 | PASSIVE, 34 | PDSTAND, 35 | QPSTAND, 36 | WALKING, 37 | PUSHING, 38 | PROBE, 39 | SLAM, // slam 40 | }; 41 | 42 | 43 | #endif // ENUMCLASS_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/interface/CheatIO.h: -------------------------------------------------------------------------------- 1 | #ifndef CHEATIO_H 2 | #define CHEATIO_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "IOInterface.h" 18 | #include "ros/publisher.h" 19 | #include "ros/ros.h" 20 | #include "ros/subscriber.h" 21 | #include "unitree_legged_msgs/HighState.h" 22 | #include "unitree_legged_msgs/LowCmd.h" 23 | #include "unitree_legged_msgs/MotorCmd.h" 24 | #include "unitree_legged_msgs/MotorState.h" 25 | 26 | class CheatIO : public IOInterface { 27 | public: 28 | CheatIO(std::string robot_name,ros::NodeHandle n); 29 | ~CheatIO(); 30 | void sendRecv(const std::shared_ptr cmd, LowlevelState* state); 31 | 32 | private: 33 | void sendCmd(const std::shared_ptr cmd); 34 | void recvState(LowlevelState* state); 35 | ros::NodeHandle& _nm; 36 | ros::Subscriber _servo_sub[10], _state_sub; 37 | ros::Publisher _servo_pub[10]; 38 | // * For mujoco simulation 39 | ros::Subscriber joinsPosVelSub_, bodyPoseSub_, bodyTwistSub_; 40 | ros::Publisher jointsTorquePub_; 41 | void jointsPosVelCallback(const std_msgs::Float32MultiArray::ConstPtr& msg); 42 | void bodyPoseCallback(const geometry_msgs::Pose::ConstPtr& msg); 43 | void bodyTwistCallback(const geometry_msgs::TwistConstPtr& msg); 44 | void sendCmdMj(const std::shared_ptr cmd); 45 | 46 | unitree_legged_msgs::LowCmd _lowCmd; 47 | unitree_legged_msgs::HighState _highState; 48 | 49 | std::string _robot_name; 50 | void initRecv(); // initialize subscribers 51 | void initSend(); // initialize publishers 52 | 53 | void StateCallback(const gazebo_msgs::ModelStates& msg); 54 | 55 | void LhipCallback(const unitree_legged_msgs::MotorState& msg); 56 | void Lhip2Callback(const unitree_legged_msgs::MotorState& msg); 57 | void LthighCallback(const unitree_legged_msgs::MotorState& msg); 58 | void LcalfCallback(const unitree_legged_msgs::MotorState& msg); 59 | void LtoeCallback(const unitree_legged_msgs::MotorState& msg); 60 | void RhipCallback(const unitree_legged_msgs::MotorState& msg); 61 | void Rhip2Callback(const unitree_legged_msgs::MotorState& msg); 62 | void RthighCallback(const unitree_legged_msgs::MotorState& msg); 63 | void RcalfCallback(const unitree_legged_msgs::MotorState& msg); 64 | void RtoeCallback(const unitree_legged_msgs::MotorState& msg); 65 | }; 66 | 67 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/interface/CmdPanel.h: -------------------------------------------------------------------------------- 1 | /* 2 | BSD 3-Clause License 3 | 4 | Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef CMDPANEL_H 34 | #define CMDPANEL_H 35 | 36 | #include "../messages/unitree_joystick.h" 37 | #include "../common/enumClass.h" 38 | #include "../sdk/include/unitree_legged_sdk/unitree_legged_sdk.h" 39 | #include "../messages/LowlevelState.h" 40 | #include 41 | 42 | struct WaypointCmd{ 43 | float x; 44 | float y; 45 | float yaw; 46 | int mode; 47 | WaypointCmd(){ 48 | setZero(); 49 | } 50 | void setZero(){ 51 | x = 0; 52 | y = 0; 53 | yaw = 0; 54 | mode = 0; 55 | } 56 | }; 57 | 58 | class CmdPanel{ 59 | public: 60 | CmdPanel(){} 61 | ~CmdPanel(){} 62 | UserCommand getUserCmd(){return userCmd;} 63 | UserValue getUserValue(){return userValue;} 64 | void setPassive(){userCmd = UserCommand::L2_B;} 65 | void setZero(){userValue.setZero();} 66 | virtual void receiveHandle(UNITREE_LEGGED_SDK::LowState *lowState){}; 67 | virtual void updateVelCmd(const LowlevelState *state){}; 68 | 69 | protected: 70 | virtual void *run(void *arg){}; 71 | UserCommand userCmd; 72 | UserValue userValue; 73 | }; 74 | 75 | #endif // CMDPANEL_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/interface/IOInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | BSD 3-Clause License 3 | 4 | Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef IOINTERFACE_H 34 | #define IOINTERFACE_H 35 | 36 | #include "../messages/LowLevelCmd.h" 37 | #include "../messages/LowlevelState.h" 38 | #include "CmdPanel.h" 39 | #include 40 | 41 | class IOInterface 42 | { 43 | public: 44 | IOInterface(){} 45 | ~IOInterface(){} 46 | virtual void sendRecv(const std::shared_ptr cmd, LowlevelState *state) = 0; 47 | void zeroCmdPanel(){cmdPanel->setZero();} 48 | void setPassive(){cmdPanel->setPassive();} 49 | CmdPanel *cmdPanel; 50 | }; 51 | 52 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/interface/KeyBoard.h: -------------------------------------------------------------------------------- 1 | #ifndef KEYBOARD_H 2 | #define KEYBOARD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "CmdPanel.h" 12 | #include "../common/cppTypes.h" 13 | 14 | class KeyBoard : public CmdPanel{ 15 | public: 16 | KeyBoard(); 17 | ~KeyBoard(); 18 | private: 19 | static void *runKeyBoard(void *arg); 20 | void *run(void *arg); 21 | UserCommand checkCmd(); 22 | void changeValue(); 23 | 24 | pthread_t _tid; 25 | float sensitivityLeft = 0.025; 26 | float sensitivityRight = 0.025; 27 | struct termios _oldSettings, _newSettings; 28 | fd_set set; 29 | int res; 30 | char _c; 31 | }; 32 | 33 | #endif // KEYBOARD_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/interface/MujocoIO.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include "IOInterface.h" 17 | #include "ros/publisher.h" 18 | #include "ros/ros.h" 19 | #include "ros/subscriber.h" 20 | #include "unitree_legged_msgs/HighState.h" 21 | #include "unitree_legged_msgs/LowCmd.h" 22 | #include "unitree_legged_msgs/MotorCmd.h" 23 | #include "unitree_legged_msgs/MotorState.h" 24 | #include "simulation.h" 25 | #include "interface/KeyBoard.h" 26 | 27 | class MujocoIO : public IOInterface { 28 | public: 29 | MujocoIO(std::string robot_name,std::shared_ptr sim):IOInterface(),sim_(sim){ 30 | cmdPanel = new KeyBoard(); 31 | } 32 | ~MujocoIO()=default; 33 | void sendRecv(const std::shared_ptr cmd, LowlevelState* state); 34 | std::shared_ptr sim_; 35 | }; 36 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/messages/LowLevelCmd.h: -------------------------------------------------------------------------------- 1 | /* 2 | BSD 3-Clause License 3 | 4 | Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef LOWLEVELCMD_H 34 | #define LOWLEVELCMD_H 35 | 36 | #include "../common/cppTypes.h" 37 | 38 | struct MotorCmd{ 39 | unsigned int mode; 40 | float q; 41 | float dq; 42 | float tau; 43 | float Kp; 44 | float Kd; 45 | 46 | MotorCmd(){ 47 | mode = 0; 48 | q = 0; 49 | dq = 0; 50 | tau = 0; 51 | Kp = 0; 52 | Kd = 0; 53 | } 54 | }; 55 | 56 | struct LowlevelCmd{ 57 | MotorCmd motorCmd[10]; 58 | }; 59 | 60 | #endif // LOWLEVELCMD_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/messages/unitree_joystick.h: -------------------------------------------------------------------------------- 1 | #ifndef UNITREE_JOYSTICK_H 2 | #define UNITREE_JOYSTICK_H 3 | 4 | #include 5 | // 16b 6 | typedef union { 7 | struct { 8 | uint8_t R1 :1; 9 | uint8_t L1 :1; 10 | uint8_t start :1; 11 | uint8_t select :1; 12 | uint8_t R2 :1; 13 | uint8_t L2 :1; 14 | uint8_t F1 :1; 15 | uint8_t F2 :1; 16 | uint8_t A :1; 17 | uint8_t B :1; 18 | uint8_t X :1; 19 | uint8_t Y :1; 20 | uint8_t up :1; 21 | uint8_t right :1; 22 | uint8_t down :1; 23 | uint8_t left :1; 24 | } components; 25 | uint16_t value; 26 | } xKeySwitchUnion; 27 | 28 | // 40 Byte (now used 24B) 29 | typedef struct { 30 | uint8_t head[2]; 31 | xKeySwitchUnion btn; 32 | float lx; 33 | float rx; 34 | float ry; 35 | float L2; 36 | float ly; 37 | 38 | uint8_t idle[16]; 39 | } xRockerBtnDataStruct; 40 | 41 | #endif // UNITREE_JOYSTICK_H -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/a1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_A1_H_ 6 | #define _UNITREE_LEGGED_A1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double a1_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double a1_Hip_min = -0.873; // unit:radian ( = -50 degree) 12 | constexpr double a1_Thigh_max = 3.927; // unit:radian ( = 225 degree) 13 | constexpr double a1_Thigh_min = -0.524; // unit:radian ( = -30 degree) 14 | constexpr double a1_Calf_max = -0.611; // unit:radian ( = -35 degree) 15 | constexpr double a1_Calf_min = -2.775; // unit:radian ( = -159 degree) 16 | } 17 | 18 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/aliengo_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_ALIENGO_H_ 6 | #define _UNITREE_LEGGED_ALIENGO_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) 12 | constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) 13 | constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) 14 | constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) 15 | constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) 16 | } 17 | 18 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/control.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_CONTROL_H_ 6 | #define _UNITREE_LEGGED_CONTROL_H_ 7 | 8 | #include "comm.h" 9 | #include "quadruped.h" 10 | 11 | namespace UNITREE_LEGGED_SDK 12 | { 13 | 14 | class Control{ 15 | public: 16 | Control(LeggedType type, uint8_t level); 17 | ~Control(); 18 | void InitCmdData(HighCmd& cmd); 19 | void InitCmdData(LowCmd& cmd); 20 | void SetBandWidth(int hz); 21 | void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode 22 | void PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, 23 | means 10%~100% power limit. If you are new, then use 1; if you are familiar, 24 | then can try bigger number or even comment this function. */ 25 | void PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree 26 | private: 27 | int BandWidth; // 1kHz, for transmit 28 | int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. 29 | double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; 30 | }; 31 | 32 | 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/lcm.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LCM_H_ 6 | #define _UNITREE_LEGGED_LCM_H_ 7 | 8 | #include "comm.h" 9 | #include 10 | #include 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | constexpr char highCmdChannel[] = "LCM_High_Cmd"; 16 | constexpr char highStateChannel[] = "LCM_High_State"; 17 | constexpr char lowCmdChannel[] = "LCM_Low_Cmd"; 18 | constexpr char lowStateChannel[] = "LCM_Low_State"; 19 | 20 | template 21 | class LCMHandler 22 | { 23 | public: 24 | LCMHandler(){ 25 | pthread_mutex_init(&countMut, NULL); 26 | pthread_mutex_init(&recvMut, NULL); 27 | } 28 | 29 | void onMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel){ 30 | isrunning = true; 31 | 32 | pthread_mutex_lock(&countMut); 33 | counter = 0; 34 | pthread_mutex_unlock(&countMut); 35 | 36 | T *msg = NULL; 37 | msg = (T *)(rbuf->data); 38 | pthread_mutex_lock(&recvMut); 39 | // sourceBuf = *msg; 40 | memcpy(&sourceBuf, msg, sizeof(T)); 41 | pthread_mutex_unlock(&recvMut); 42 | } 43 | 44 | bool isrunning = false; 45 | T sourceBuf = {0}; 46 | pthread_mutex_t countMut; 47 | pthread_mutex_t recvMut; 48 | int counter = 0; 49 | }; 50 | 51 | class LCM { 52 | public: 53 | LCM(); 54 | ~LCM(); 55 | void SubscribeCmd(); 56 | void SubscribeState(); // remember to call this when change control level 57 | int Send(HighCmd&); // lcm send cmd 58 | int Send(LowCmd&); // lcm send cmd 59 | int Send(HighState&); // lcm send state 60 | int Send(LowState&); // lcm send state 61 | int Recv(); // directly save in buffer 62 | void Get(HighCmd&); 63 | void Get(LowCmd&); 64 | void Get(HighState&); 65 | void Get(LowState&); 66 | 67 | LCMHandler highStateLCMHandler; 68 | LCMHandler lowStateLCMHandler; 69 | LCMHandler highCmdLCMHandler; 70 | LCMHandler lowCmdLCMHandler; 71 | private: 72 | lcm::LCM lcm; 73 | lcm::Subscription* subLcm; 74 | int lcmFd; 75 | int LCM_PERIOD = 2000; //default 2ms 76 | }; 77 | 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/loop.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LOOP_H_ 6 | #define _UNITREE_LEGGED_LOOP_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace UNITREE_LEGGED_SDK 18 | { 19 | 20 | constexpr int THREAD_PRIORITY = 99; // real-time priority 21 | 22 | typedef boost::function Callback; 23 | 24 | class Loop { 25 | public: 26 | Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} 27 | ~Loop(); 28 | void start(); 29 | void shutdown(); 30 | virtual void functionCB() = 0; 31 | 32 | private: 33 | void entryFunc(); 34 | 35 | std::string _name; 36 | float _period; 37 | int _bindCPU; 38 | bool _bind_cpu_flag = false; 39 | bool _isrunning = false; 40 | std::thread _thread; 41 | }; 42 | 43 | class LoopFunc : public Loop { 44 | public: 45 | LoopFunc(std::string name, float period, const Callback& _cb) 46 | : Loop(name, period), _fp(_cb){} 47 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 48 | : Loop(name, period, bindCPU), _fp(_cb){} 49 | void functionCB() { (_fp)(); } 50 | private: 51 | boost::function _fp; 52 | }; 53 | 54 | 55 | } 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/quadruped.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_QUADRUPED_H_ 6 | #define _UNITREE_LEGGED_QUADRUPED_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | 11 | enum class LeggedType { 12 | Aliengo, 13 | A1 14 | }; 15 | 16 | enum class HighLevelType { 17 | Basic, 18 | Sport 19 | }; 20 | 21 | void SetLevel(uint8_t level); 22 | 23 | // definition of each leg and joint 24 | constexpr int FR_ = 0; // leg index 25 | constexpr int FL_ = 1; 26 | constexpr int RR_ = 2; 27 | constexpr int RL_ = 3; 28 | 29 | constexpr int FR_0 = 0; // joint index 30 | constexpr int FR_1 = 1; 31 | constexpr int FR_2 = 2; 32 | 33 | constexpr int FL_0 = 3; 34 | constexpr int FL_1 = 4; 35 | constexpr int FL_2 = 5; 36 | 37 | constexpr int RR_0 = 6; 38 | constexpr int RR_1 = 7; 39 | constexpr int RR_2 = 8; 40 | 41 | constexpr int RL_0 = 9; 42 | constexpr int RL_1 = 10; 43 | constexpr int RL_2 = 11; 44 | 45 | } 46 | 47 | #endif -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/udp.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_UDP_H_ 6 | #define _UNITREE_LEGGED_UDP_H_ 7 | 8 | #include "comm.h" 9 | #include "quadruped.h" 10 | #include 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | constexpr int UDP_CLIENT_PORT = 8080; // local port 16 | constexpr int UDP_SERVER_PORT = 8007; // target port 17 | constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address 18 | constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address 19 | 20 | // Notice: User defined data(like struct) should add crc(4Byte) at the end. 21 | class UDP { 22 | public: 23 | UDP(HighLevelType highControl = HighLevelType::Basic); // unitree dafault IP and Port 24 | UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, int sendLength, int recvLength); 25 | UDP(uint16_t localPort, uint16_t targetPort, int sendLength, int recvLength); // as server, client IP can change 26 | ~UDP(); 27 | void switchLevel(int level); 28 | 29 | int SetSend(HighCmd&); 30 | int SetSend(LowCmd&); 31 | int SetSend(char* cmd); 32 | void GetRecv(HighState&); 33 | void GetRecv(LowState&); 34 | void GetRecv(char*); 35 | int Send(); 36 | int Recv(); // directly save in buffer 37 | 38 | UDPState udpState; 39 | char* targetIP; 40 | uint16_t targetPort; 41 | char* localIP; 42 | uint16_t localPort; 43 | private: 44 | void init(uint16_t localPort, const char* targetIP, uint16_t targetPort); 45 | 46 | int sockFd; 47 | bool connected; // udp only works when connected 48 | int sendLength; 49 | int recvLength; 50 | char* recvTemp; 51 | char* recvBuf; 52 | char* sendBuf; 53 | int lose_recv; 54 | pthread_mutex_t sendMut; 55 | pthread_mutex_t recvMut; 56 | }; 57 | 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/include/unitree_legged_sdk/unitree_legged_sdk.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SDK_H_ 6 | #define _UNITREE_LEGGED_SDK_H_ 7 | 8 | #include "comm.h" 9 | #include "control.h" 10 | #include "udp.h" 11 | #include "loop.h" 12 | #include "lcm.h" 13 | #include "quadruped.h" 14 | #include 15 | 16 | #define UTK UNITREE_LEGGED_SDK //short name 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/sdk/lib/libunitree_legged_sdk.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_control/include/sdk/lib/libunitree_legged_sdk.so -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/include/simulation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | class Simulation { 14 | public: 15 | Simulation(const std::string& xml_path); 16 | ~Simulation(); 17 | void Step(Eigen::VectorXd tau); 18 | mjData* data; 19 | mjModel* model; 20 | mjvCamera cam; 21 | mjvOption opt; 22 | mjvScene scene; 23 | mjrContext context; 24 | GLFWwindow* window; 25 | bool button_left{false}, button_middle{false}, button_right{false}; 26 | double lastx{0}, lasty{0}; 27 | int counter{0}; 28 | int viewport_width{0}, viewport_height{0}; 29 | Eigen::VectorXd qpos{Eigen::VectorXd::Zero(10)}; 30 | Eigen::VectorXd qvel{Eigen::VectorXd::Zero(10)}; 31 | Eigen::Vector3d bodyPos{Eigen::Vector3d::Zero()}; 32 | Eigen::Vector3d bodyVel{Eigen::Vector3d::Zero()}; 33 | Eigen::Vector3d bodyGyro{Eigen::Vector3d::Zero()}; 34 | Eigen::Quaterniond bodyQuat{Eigen::Quaterniond::Identity()}; 35 | 36 | // mujoco::GlfwAdapter adapter_; 37 | }; 38 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_control 4 | 0.0.0 5 | The quadruped controller package 6 | 7 | yiyuc 8 | TODO 9 | 10 | catkin 11 | genmsg 12 | controller_manager 13 | joint_state_controller 14 | robot_state_publisher 15 | roscpp 16 | rospy 17 | std_msgs 18 | controller_manager 19 | joint_state_controller 20 | robot_state_publisher 21 | roscpp 22 | std_msgs 23 | controller_manager 24 | joint_state_controller 25 | robot_state_publisher 26 | roscpp 27 | std_msgs 28 | unitree_legged_msgs 29 | qpoases 30 | cython 31 | python-numpy 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/FSM/FSM.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/FSM/FSM.h" 2 | #include 3 | 4 | FSM::FSM(std::shared_ptr data) 5 | :_data(data) 6 | { 7 | 8 | _stateList.invalid = nullptr; 9 | _stateList.passive = new FSMState_Passive(_data); 10 | _stateList.walking = new FSMState_Walking(_data); 11 | 12 | initialize(); 13 | } 14 | 15 | FSM::~FSM(){ 16 | _stateList.deletePtr(); 17 | } 18 | 19 | void FSM::initialize() 20 | { 21 | count = 0; 22 | _currentState = _stateList.walking; 23 | _currentState -> enter(); 24 | _nextState = _currentState; 25 | _mode = FSMMode::NORMAL; 26 | } 27 | 28 | void FSM::run() 29 | { 30 | _data->sendRecv(); 31 | 32 | if(!checkSafty()) 33 | { 34 | _data->_interface->setPassive(); 35 | } 36 | 37 | if(_mode == FSMMode::NORMAL) 38 | { 39 | _currentState->run(); 40 | _nextStateName = _currentState->checkTransition(); 41 | if(_nextStateName != _currentState->_stateName) 42 | { 43 | _mode = FSMMode::CHANGE; 44 | _nextState = getNextState(_nextStateName); 45 | } 46 | } 47 | else if(_mode == FSMMode::CHANGE) 48 | { 49 | // std::cout << "change state" << std::endl; 50 | _currentState->exit(); 51 | _currentState = _nextState; 52 | _currentState->enter(); 53 | _mode = FSMMode::NORMAL; 54 | _currentState->run(); 55 | } 56 | 57 | count++; 58 | } 59 | 60 | FSMState* FSM::getNextState(FSMStateName stateName) 61 | { 62 | switch(stateName) 63 | { 64 | case FSMStateName::INVALID: 65 | return _stateList.invalid; 66 | break; 67 | case FSMStateName::PASSIVE: 68 | return _stateList.passive; 69 | break; 70 | case FSMStateName::WALKING: 71 | return _stateList.walking; 72 | break; 73 | default: 74 | return _stateList.invalid; 75 | break; 76 | } 77 | } 78 | 79 | bool FSM::checkSafty() 80 | { 81 | if(_data->_stateEstimator->getResult().rBody(2,2) < 0.5) 82 | { 83 | return false; 84 | } 85 | else 86 | { 87 | return true; 88 | } 89 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/FSM/FSMState.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/FSM/FSMState.h" 2 | 3 | FSMState::FSMState(std::shared_ptr data, FSMStateName stateName, std::string stateNameStr): 4 | _data(data), _stateName(stateName), _stateNameStr(stateNameStr) 5 | { 6 | _lowCmd = _data->_lowCmd; 7 | _lowState = _data->_lowState; 8 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/FSM/FSMState_Passive.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/FSM/FSMState_Passive.h" 2 | 3 | FSMState_Passive::FSMState_Passive(std::shared_ptr data): 4 | FSMState(data, FSMStateName::PASSIVE, "passive"){} 5 | 6 | void FSMState_Passive::enter() 7 | { 8 | _data->_legController->zeroCommand(); 9 | for(int i = 0; i < 2; i++) 10 | { 11 | _data->_legController->commands[i].kdJoint.diagonal()<< 5, 5, 5, 5, 5; 12 | } 13 | 14 | } 15 | 16 | void FSMState_Passive::run() 17 | { 18 | _data->_legController->updateData(_data->_lowState); 19 | _data->_stateEstimator->run(); 20 | _data->_legController->updateCommand(_data->_lowCmd); 21 | } 22 | 23 | void FSMState_Passive::exit() 24 | { 25 | for(int i = 0; i < 2; i++) 26 | { 27 | _data->_legController->commands[i].kdJoint.setZero(); 28 | } 29 | } 30 | 31 | FSMStateName FSMState_Passive::checkTransition() 32 | { 33 | if(_lowState->userCmd == UserCommand::L1_X){ 34 | FSMStateName::WALKING; 35 | } 36 | else{ 37 | return FSMStateName::PASSIVE; 38 | } 39 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/FSM/FSMState_Walking.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/FSM/FSMState_Walking.h" 2 | 3 | FSMState_Walking::FSMState_Walking(std::shared_ptr data) 4 | :FSMState(data, FSMStateName::WALKING, "walking"), 5 | Cmpc(0.001, 40) {} 6 | 7 | template 8 | T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){ 9 | return (value-minLim)*(max-min)/(maxLim-minLim) + min; 10 | } 11 | 12 | void FSMState_Walking::enter() 13 | { 14 | v_des_body << 0, 0, 0; 15 | pitch = 0; 16 | roll = 0; 17 | _data->_interface->zeroCmdPanel(); 18 | counter = 0; 19 | _data->_desiredStateCommand->firstRun = true; 20 | _data->_stateEstimator->run(); 21 | _data->_legController->zeroCommand(); 22 | Cmpc.firstRun = true; 23 | } 24 | 25 | void FSMState_Walking::run() 26 | { 27 | _data->_legController->updateData(_data->_lowState); 28 | _data->_stateEstimator->run(); 29 | _userValue = _data->_lowState->userValue; 30 | 31 | v_des_body[0] = (double)invNormalize(_userValue.ly, -0.75, 0.75); 32 | v_des_body[1] = (double)invNormalize(_userValue.rx, -0.25, 0.25); 33 | turn_rate = (double)invNormalize(_userValue.lx, -0.5, 0.5); 34 | // std::cout << "vx vy " << v_des_body[0] << " " << v_des_body[1] << std::endl; 35 | _data->_desiredStateCommand->setStateCommands(roll, pitch, v_des_body, turn_rate); 36 | 37 | Cmpc.setGaitNum(2); // 2 for walking 38 | Cmpc.run(*_data); 39 | 40 | _data->_legController->updateCommand(_data->_lowCmd); 41 | } 42 | 43 | void FSMState_Walking::exit() 44 | { 45 | counter = 0; 46 | _data->_interface->zeroCmdPanel(); 47 | } 48 | 49 | FSMStateName FSMState_Walking::checkTransition() 50 | { 51 | if(_lowState->userCmd == UserCommand::L2_B){ 52 | return FSMStateName::PASSIVE; 53 | } 54 | else{ 55 | return FSMStateName::WALKING; 56 | } 57 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/common/DesiredCommand.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/common/DesiredCommand.h" 2 | 3 | void DesiredStateData::zero(){ 4 | pre_stateDes = Vec12::Zero(); 5 | stateDes = Vec12::Zero(); 6 | } 7 | 8 | void DesiredStateCommand::setStateCommands(double r, double p, Vec3 v_des, double yaw_rate){ 9 | if(firstRun) 10 | { 11 | data.pre_stateDes(5) = stateEstimate->rpy(2); 12 | firstRun = false; 13 | } 14 | 15 | data.stateDes(6) = v_des[0]; 16 | data.stateDes(7) = v_des[1]; 17 | data.stateDes(8) = 0; 18 | 19 | data.stateDes(9) = 0; 20 | data.stateDes(10) = 0; 21 | data.stateDes(11) = yaw_rate; 22 | 23 | data.stateDes(0) = stateEstimate->position(0) + dt * data.stateDes(6); 24 | data.stateDes(1) = stateEstimate->position(1) + dt * data.stateDes(7); 25 | data.stateDes(5) = data.pre_stateDes(5) + dt * data.stateDes(11); 26 | 27 | if(data.stateDes(5) > 3.1 && stateEstimate->rpy(2) < 0){ 28 | data.stateDes(5) = stateEstimate->rpy(2); 29 | } 30 | 31 | 32 | if(data.stateDes(5) < -3.1 && stateEstimate->rpy(2) > 0){ 33 | data.stateDes(5) = stateEstimate->rpy(2); 34 | } 35 | 36 | data.pre_stateDes(5) = data.stateDes(5); 37 | 38 | // Roll 39 | data.stateDes(3) = r; 40 | 41 | // Pitch 42 | data.stateDes(4) = p; 43 | } 44 | 45 | double DesiredStateCommand::deadband(double command, double minVal, double maxVal){ 46 | return (command + 1)*(maxVal-minVal)/2.0 + minVal; 47 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/common/FootSwingTrajectory.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file FootSwingTrajectory.cpp 3 | * @brief Utility to generate foot swing trajectories. 4 | * 5 | * Currently uses Bezier curves like Cheetah 3 does 6 | */ 7 | 8 | #include "../../include/common/Math/Interpolation.h" 9 | #include "../../include/common/FootSwingTrajectory.h" 10 | 11 | /*! 12 | * Compute foot swing trajectory with a bezier curve 13 | * @param phase : How far along we are in the swing (0 to 1) 14 | * @param swingTime : How long the swing should take (seconds) 15 | */ 16 | template 17 | void FootSwingTrajectory::computeSwingTrajectoryBezier(T phase, T swingTime) { 18 | _p = Interpolate::cubicBezier>(_p0, _pf, phase); 19 | _v = Interpolate::cubicBezierFirstDerivative>(_p0, _pf, phase); 20 | 21 | T zp, zv; 22 | 23 | if(phase < T(0.5)) { 24 | zp = Interpolate::cubicBezier(_p0[2], _p0[2] + _height, phase * 2); 25 | zv = Interpolate::cubicBezierFirstDerivative(_p0[2], _p0[2] + _height, phase * 2); 26 | 27 | } else { 28 | zp = Interpolate::cubicBezier(_p0[2] + _height, _pf[2], phase * 2 - 1); 29 | zv = Interpolate::cubicBezierFirstDerivative(_p0[2] + _height, _pf[2], phase * 2 - 1); 30 | 31 | } 32 | 33 | _p[2] = zp; 34 | _v[2] = zv; 35 | 36 | } 37 | 38 | template class FootSwingTrajectory; 39 | template class FootSwingTrajectory; -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/common/OrientationEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/common/OrientationEstimator.h" 2 | 3 | /*! 4 | * Get quaternion, rotation matrix, angular velocity (body and world), 5 | * rpy, acceleration (world, body) from vector nav IMU 6 | */ 7 | void CheaterOrientationEstimator::run() { 8 | //std::cout << "orientation" << std::endl; 9 | this->_stateEstimatorData.result->orientation[0] = 10 | this->_stateEstimatorData.lowState->imu.quaternion[0]; 11 | this->_stateEstimatorData.result->orientation[1] = 12 | this->_stateEstimatorData.lowState->imu.quaternion[1]; 13 | this->_stateEstimatorData.result->orientation[2] = 14 | this->_stateEstimatorData.lowState->imu.quaternion[2]; 15 | this->_stateEstimatorData.result->orientation[3] = 16 | this->_stateEstimatorData.lowState->imu.quaternion[3]; 17 | 18 | this->_stateEstimatorData.result->rBody = ori::quaternionToRotationMatrix( 19 | this->_stateEstimatorData.result->orientation); 20 | 21 | this->_stateEstimatorData.result->omegaBody(0) = 22 | this->_stateEstimatorData.lowState->imu.gyroscope[0]; 23 | this->_stateEstimatorData.result->omegaBody(1) = 24 | this->_stateEstimatorData.lowState->imu.gyroscope[1]; 25 | this->_stateEstimatorData.result->omegaBody(2) = 26 | this->_stateEstimatorData.lowState->imu.gyroscope[2]; 27 | this->_stateEstimatorData.result->rpy = 28 | ori::quatToRPY(this->_stateEstimatorData.result->orientation); 29 | 30 | this->_stateEstimatorData.result->omegaWorld = 31 | this->_stateEstimatorData.result->rBody.transpose() * 32 | this->_stateEstimatorData.result->omegaBody; 33 | 34 | this->_stateEstimatorData.result->rpy = 35 | ori::quatToRPY(this->_stateEstimatorData.result->orientation); 36 | 37 | } 38 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/interface/MujocoIO.cpp: -------------------------------------------------------------------------------- 1 | #include "interface/MujocoIO.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "Eigen/src/Core/Matrix.h" 7 | #include "messages/LowLevelCmd.h" 8 | #include "messages/LowlevelState.h" 9 | 10 | void MujocoIO::sendRecv(const std::shared_ptr cmd, LowlevelState* state) { 11 | // set command torque 12 | Eigen::VectorXd jointTorque(10); 13 | for (int i = 0; i < 10; i++) { 14 | jointTorque(i) = cmd->motorCmd[i].tau; 15 | } 16 | // sim 17 | sim_->Step(jointTorque); 18 | 19 | // return state 20 | for (int i = 0; i < 10; i++) { 21 | state->motorState[i].q = sim_->qpos(i); 22 | state->motorState[i].dq = sim_->qvel(i); 23 | state->motorState[i].tauEst = jointTorque(i); 24 | } 25 | for (int i = 0; i < 3; i++) { 26 | state->imu.gyroscope[i] = sim_->bodyGyro(i); 27 | state->position[i] = sim_->bodyPos(i); 28 | state->vWorld[i] = sim_->bodyVel(i); 29 | } 30 | state->imu.quaternion[0] = sim_->bodyQuat.w(); 31 | state->imu.quaternion[1] = sim_->bodyQuat.x(); 32 | state->imu.quaternion[2] = sim_->bodyQuat.y(); 33 | state->imu.quaternion[3] = sim_->bodyQuat.z(); 34 | 35 | //update command vel 36 | cmdPanel->updateVelCmd(state); 37 | state->userCmd = cmdPanel->getUserCmd(); 38 | state->userValue = cmdPanel->getUserValue(); 39 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "../include/common/ControlFSMData.h" 8 | #include "../include/common/OrientationEstimator.h" 9 | #include "../include/common/PositionVelocityEstimator.h" 10 | #include "../include/interface/CheatIO.h" 11 | #include "../include/FSM/FSM.h" 12 | 13 | #include "std_msgs/Bool.h" 14 | 15 | bool running = true; 16 | bool pauseFlag = true; 17 | 18 | void ShutDown(int sig) { 19 | std::cout << "stop" << std::endl; 20 | running = false; 21 | } 22 | 23 | void callback(std_msgs::Bool::ConstPtr msg) { 24 | if (msg->data) { 25 | std::cout << "pause" << std::endl; 26 | pauseFlag = true; 27 | } else { 28 | std::cout << "resume" << std::endl; 29 | pauseFlag = false; 30 | } 31 | } 32 | 33 | int main(int argc, char** argv) { 34 | IOInterface* ioInter; 35 | ros::init(argc, argv, "hector_control", ros::init_options::AnonymousName); 36 | ros::NodeHandle nh; 37 | ros::Subscriber simStatesub = nh.subscribe("/pauseFlag", 1, callback); 38 | ros::AsyncSpinner subSpinner(4); // one threads 39 | subSpinner.start(); 40 | std::string robot_name = "hector"; 41 | std::cout << "robot name " << robot_name << std::endl; 42 | 43 | ioInter = new CheatIO(robot_name,nh); 44 | ros::Rate rate(1000); 45 | 46 | double dt = 0.001; 47 | Biped biped; 48 | biped.setBiped(); 49 | 50 | LegController* legController = new LegController(biped); 51 | std::shared_ptr cmd = std::make_shared(); 52 | LowlevelState* state = new LowlevelState(); 53 | 54 | std::cout << "start setup " << std::endl; 55 | StateEstimate stateEstimate; 56 | StateEstimatorContainer* stateEstimator = new StateEstimatorContainer(state, legController->data, &stateEstimate); 57 | 58 | stateEstimator->addEstimator(); 59 | stateEstimator->addEstimator(); 60 | 61 | std::cout << "setup state etimator" << std::endl; 62 | 63 | DesiredStateCommand* desiredStateCommand = new DesiredStateCommand(&stateEstimate, dt); 64 | 65 | std::shared_ptr _controlData = std::make_shared(); 66 | 67 | _controlData->_biped = &biped; 68 | _controlData->_stateEstimator = stateEstimator; 69 | _controlData->_legController = legController; 70 | _controlData->_desiredStateCommand = desiredStateCommand; 71 | _controlData->_interface = ioInter; 72 | _controlData->_lowCmd = cmd; 73 | _controlData->_lowState = state; 74 | 75 | std::shared_ptr _FSMController = std::make_shared(_controlData); 76 | 77 | // signal(SIGINT, ShutDown); 78 | 79 | while (ros::ok()) { 80 | if (!pauseFlag) _FSMController->run(); 81 | rate.sleep(); 82 | } 83 | ros::waitForShutdown(); 84 | 85 | return 0; 86 | 87 | } 88 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/src/mjSim.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "../include/FSM/FSM.h" 15 | #include "../include/common/ControlFSMData.h" 16 | #include "../include/common/OrientationEstimator.h" 17 | #include "../include/common/PositionVelocityEstimator.h" 18 | #include "../include/interface/MujocoIO.h" 19 | #include "glfw/glfw_adapter.h" 20 | #include "interface/CheatIO.h" 21 | #include "ros/node_handle.h" 22 | #include "ros/subscriber.h" 23 | #include "simulation.h" 24 | 25 | int main(int argc, char** argv) { 26 | // Get mujoco xml path 27 | std::string path = ros::package::getPath("hector_description"); 28 | path += "/mjcf/hector.xml"; 29 | std::cout << "path: " << path << std::endl; 30 | std::shared_ptr sim = std::make_shared(path); 31 | auto last = std::chrono::high_resolution_clock::now(); 32 | 33 | // ros::init(argc, argv, "hector_control"); 34 | // ros::NodeHandle nh; 35 | IOInterface* ioInter; 36 | ioInter = new MujocoIO("hector",sim); 37 | // ioInter = new CheatIO("hector",nh); 38 | double dt = 0.001; 39 | Biped biped; 40 | biped.setBiped(); 41 | 42 | LegController* legController = new LegController(biped); 43 | std::shared_ptr cmd = std::make_shared(); 44 | LowlevelState* state = new LowlevelState(); 45 | 46 | std::cout << "start setup " << std::endl; 47 | StateEstimate stateEstimate; 48 | StateEstimatorContainer* stateEstimator = new StateEstimatorContainer(state, legController->data, &stateEstimate); 49 | 50 | stateEstimator->addEstimator(); 51 | stateEstimator->addEstimator(); 52 | 53 | std::cout << "setup state etimator" << std::endl; 54 | 55 | DesiredStateCommand* desiredStateCommand = new DesiredStateCommand(&stateEstimate, dt); 56 | 57 | std::shared_ptr _controlData = std::make_shared(); 58 | 59 | _controlData->_biped = &biped; 60 | _controlData->_stateEstimator = stateEstimator; 61 | _controlData->_legController = legController; 62 | _controlData->_desiredStateCommand = desiredStateCommand; 63 | _controlData->_interface = ioInter; 64 | _controlData->_lowCmd = cmd; 65 | _controlData->_lowState = state; 66 | 67 | std::shared_ptr _FSMController = std::make_shared(_controlData); 68 | 69 | // main loop 70 | while (!glfwWindowShouldClose(sim->window)) { 71 | _FSMController->run(); 72 | // sim->Step(); 73 | // // get current time 74 | // auto now = std::chrono::high_resolution_clock::now(); 75 | // // get time difference 76 | // auto time = std::chrono::duration_cast(now - last); 77 | // last = now; 78 | // std::cout << "time: " << time.count() << std::endl; 79 | } 80 | 81 | return 0; 82 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/AUTHORS: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | MAIN AUTHORS 26 | ============ 27 | 28 | qpOASES's core functionality and software design have been developed by the 29 | following main developers (in alphabetical order): 30 | 31 | Hans Joachim Ferreau 32 | Christian Kirches 33 | Andreas Potschka 34 | 35 | 36 | 37 | FURTHER AUTHORS 38 | =============== 39 | 40 | Moreover, the following developers have contributed code to qpOASES's 41 | third-party interfaces or provided additional functionality 42 | (in alphabetical order): 43 | 44 | Alexander Buchner 45 | Holger Diedam 46 | Dennis Janka 47 | Manuel Kudruss 48 | Andreas Waechter 49 | Sebastian F. Walter 50 | 51 | 52 | 53 | CONTRIBUTORS 54 | ============ 55 | 56 | Finally, the following people have not contributed to the source code, 57 | but have helped making qpOASES even more useful by testing, reporting 58 | bugs or proposing algorithmic improvements (in alphabetical order): 59 | 60 | Eckhard Arnold 61 | Joris Gillis 62 | Boris Houska 63 | D. Kwame Minde Kufoalor 64 | Aude Perrin 65 | Milan Vukov 66 | Thomas Wiese 67 | Leonard Wirsching 68 | 69 | 70 | 71 | SCIENTIFIC MENTORS 72 | ================== 73 | 74 | We also would like to thank two persons who had a major share in making 75 | qpOASES a success. Not by writing even a single line of code, but by 76 | establishing the idea of using a homotopy-based approach for high-speed 77 | QP solutions and by excellent scientific guidance during the development 78 | process: 79 | 80 | Hans Georg Bock 81 | Moritz Diehl 82 | 83 | 84 | 85 | All users are invited to further improve qpOASES by providing comments, 86 | code enhancements, bug reports, additional documentation or whatever you 87 | feel is missing. 88 | 89 | 90 | 91 | ## 92 | ## end of file 93 | ## 94 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/AUTHORS.txt: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | MAIN AUTHORS 26 | ============ 27 | 28 | qpOASES's core functionality and software design have been developed by the 29 | following main developers (in alphabetical order): 30 | 31 | Hans Joachim Ferreau 32 | Christian Kirches 33 | Andreas Potschka 34 | 35 | 36 | 37 | FURTHER AUTHORS 38 | =============== 39 | 40 | Moreover, the following developers have contributed code to qpOASES's 41 | third-party interfaces or provided additional functionality 42 | (in alphabetical order): 43 | 44 | Alexander Buchner 45 | Holger Diedam 46 | Dennis Janka 47 | Manuel Kudruss 48 | Andreas Waechter 49 | Sebastian F. Walter 50 | 51 | 52 | 53 | CONTRIBUTORS 54 | ============ 55 | 56 | Finally, the following people have not contributed to the source code, 57 | but have helped making qpOASES even more useful by testing, reporting 58 | bugs or proposing algorithmic improvements (in alphabetical order): 59 | 60 | Eckhard Arnold 61 | Joris Gillis 62 | Boris Houska 63 | D. Kwame Minde Kufoalor 64 | Aude Perrin 65 | Milan Vukov 66 | Thomas Wiese 67 | Leonard Wirsching 68 | 69 | 70 | 71 | SCIENTIFIC MENTORS 72 | ================== 73 | 74 | We also would like to thank two persons who had a major share in making 75 | qpOASES a success. Not by writing even a single line of code, but by 76 | establishing the idea of using a homotopy-based approach for high-speed 77 | QP solutions and by excellent scientific guidance during the development 78 | process: 79 | 80 | Hans Georg Bock 81 | Moritz Diehl 82 | 83 | 84 | 85 | All users are invited to further improve qpOASES by providing comments, 86 | code enhancements, bug reports, additional documentation or whatever you 87 | feel is missing. 88 | 89 | 90 | 91 | ## 92 | ## end of file 93 | ## 94 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/INSTALL: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | INSTALLATION UNDER LINUX 26 | ======================== 27 | 28 | 0. Obtain qpOASES from COIN-OR: 29 | 30 | Download a zipped archive containg the latest stable release and unpack it 31 | into . Alternatively, you check out the latest stable branch, 32 | e.g. by running 33 | 34 | svn co https://projects.coin-or.org/svn/qpOASES/stable/3.2 35 | 36 | from your shell. 37 | 38 | 39 | 1. Compilation of the qpOASES library libqpOASES.a (or .so) and test examples: 40 | 41 | cd 42 | make 43 | 44 | The library libqpOASES.a (or .so) provides the complete functionality of the 45 | qpOASES software package. It can be used by, e.g., linking it against a main 46 | function from the examples folder. The make also compiles a couple of test 47 | examples; executables are stored within the directory /bin. 48 | 49 | 50 | 2. Running a simple test example: 51 | 52 | Among others, an executable called example1 should have been created; run 53 | it in order to test your installation: 54 | 55 | cd /bin 56 | ./example1 57 | 58 | If it terminates after successfully solving two QP problems, qpOASES has been 59 | successfully installed! 60 | 61 | 62 | 3. Optional, create source code documentation (using doxygen): 63 | 64 | cd /doc 65 | doxygen doxygen.config 66 | 67 | Afterwards, you can open the file /doc/html/index.html with 68 | your favorite browser in order to view qpOASES's source code documentation. 69 | 70 | 71 | NOTE: More detailed installation instructions, including information on how 72 | to run unit tests can be found in the qpOASES User's Manual located 73 | at /doc/manual.pdf! 74 | 75 | 76 | 77 | ## 78 | ## end of file 79 | ## 80 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/INSTALL.txt: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | INSTALLATION UNDER LINUX 26 | ======================== 27 | 28 | 0. Obtain qpOASES from COIN-OR: 29 | 30 | Download a zipped archive containg the latest stable release and unpack it 31 | into . Alternatively, you check out the latest stable branch, 32 | e.g. by running 33 | 34 | svn co https://projects.coin-or.org/svn/qpOASES/stable/3.2 35 | 36 | from your shell. 37 | 38 | 39 | 1. Compilation of the qpOASES library libqpOASES.a (or .so) and test examples: 40 | 41 | cd 42 | make 43 | 44 | The library libqpOASES.a (or .so) provides the complete functionality of the 45 | qpOASES software package. It can be used by, e.g., linking it against a main 46 | function from the examples folder. The make also compiles a couple of test 47 | examples; executables are stored within the directory /bin. 48 | 49 | 50 | 2. Running a simple test example: 51 | 52 | Among others, an executable called example1 should have been created; run 53 | it in order to test your installation: 54 | 55 | cd /bin 56 | ./example1 57 | 58 | If it terminates after successfully solving two QP problems, qpOASES has been 59 | successfully installed! 60 | 61 | 62 | 3. Optional, create source code documentation (using doxygen): 63 | 64 | cd /doc 65 | doxygen doxygen.config 66 | 67 | Afterwards, you can open the file /doc/html/index.html with 68 | your favorite browser in order to view qpOASES's source code documentation. 69 | 70 | 71 | NOTE: More detailed installation instructions, including information on how 72 | to run unit tests can be found in the qpOASES User's Manual located 73 | at /doc/manual.pdf! 74 | 75 | 76 | 77 | ## 78 | ## end of file 79 | ## 80 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/Makefile: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | ## 26 | ## Filename: Makefile 27 | ## Author: Hans Joachim Ferreau 28 | ## Version: 3.2 29 | ## Date: 2007-2017 30 | ## 31 | 32 | include make.mk 33 | 34 | ## 35 | ## targets 36 | ## 37 | 38 | 39 | all: src examples 40 | #src_aw testing 41 | 42 | src: 43 | @cd $@; ${MAKE} -s 44 | 45 | #src_aw: 46 | # @cd $@; ${MAKE} -s 47 | 48 | examples: src 49 | @cd $@; ${MAKE} -s 50 | 51 | doc: 52 | @cd $@; ${MAKE} -s 53 | 54 | testing: src 55 | @cd testing/cpp; ${MAKE} -s 56 | 57 | test: testing 58 | @cd testing/cpp; ${MAKE} -s runTests 59 | 60 | debugging: 61 | @cd $@; ${MAKE} -s 62 | 63 | clean: 64 | @cd src && ${MAKE} -s clean 65 | @cd examples && ${MAKE} -s clean 66 | @cd bin && ${RM} -f *.* *{EXE} 67 | @cd testing/cpp && ${MAKE} -s clean 68 | 69 | # && cd src_aw && ${MAKE} -s clean && cd .. \ 70 | # && cd debugging && ${MAKE} -s clean && cd .. \ 71 | 72 | 73 | clobber: clean 74 | 75 | scilab: 76 | @echo Compiling Scilab interface... 77 | @cd ./interfaces/scilab/; ${MAKE} -s 78 | 79 | python: all 80 | cd ./interfaces/python/ && python setup.py build_ext --inplace 81 | 82 | pythoninstall: all 83 | cd ./interfaces/python/ && python setup.py install 84 | 85 | c_wrapper: 86 | @echo Compiling C interface... 87 | @cd ./interfaces/c/; ${MAKE} -s 88 | 89 | .PHONY : all src examples doc testing debugging clean clobber scilab python phythoninstall c_wrapper 90 | 91 | 92 | ## 93 | ## end of file 94 | ## 95 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/README: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | INTRODUCTION 26 | ============ 27 | 28 | qpOASES is an open-source C++ implementation of the recently proposed 29 | online active set strategy, which was inspired by important observations 30 | from the field of parametric quadratic programming (QP). It has several 31 | theoretical features that make it particularly suited for model predictive 32 | control (MPC) applications. Further numerical modifications have made 33 | qpOASES a reliable QP solver, even when tackling semi-definite, ill-posed or 34 | degenerated QP problems. Moreover, several interfaces to third-party software 35 | like ​Matlab or ​Simulink are provided that make qpOASES easy-to-use even for 36 | users without knowledge of C/C++. 37 | 38 | 39 | 40 | GETTING STARTED 41 | =============== 42 | 43 | 1. For installation, usage and additional information on this software package 44 | see the qpOASES User's Manual located at doc/manual.pdf or check its 45 | source code documentation! 46 | 47 | 48 | 2. The file LICENSE.txt contains a copy of the GNU Lesser General Public 49 | License (v2.1). Please read it carefully before using qpOASES! 50 | 51 | 52 | 3. The whole software package can be obtained from 53 | 54 | http://www.qpOASES.org/ or 55 | https://projects.coin-or.org/qpOASES/ 56 | 57 | On this webpage you will also find further support such as a list of 58 | questions posed by other users. 59 | 60 | 61 | 62 | CONTACT THE AUTHORS 63 | =================== 64 | 65 | If you have got questions, remarks or comments on qpOASES, it is strongly 66 | encouraged to report them by creating a new ticket at the qpOASES webpage. 67 | In case you do not want to disclose your feedback to the public, you may 68 | send an e-mail to 69 | 70 | support@qpOASES.org 71 | 72 | Finally, you may contact one of the main authors directly: 73 | 74 | Hans Joachim Ferreau, joachim.ferreau@ch.abb.com 75 | Andreas Potschka, potschka@iwr.uni-heidelberg.de 76 | Christian Kirches, christian.kirches@iwr.uni-heidelberg.de 77 | 78 | Also bug reports, source code enhancements or success stories are most welcome! 79 | 80 | 81 | 82 | ## 83 | ## end of file 84 | ## 85 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES.hpp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | */ 31 | 32 | 33 | #if defined(__SINGLE_OBJECT__) || defined(__C_WRAPPER__) 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #if !defined(__MATLAB__) || defined(WIN32) 43 | #include 44 | #include 45 | #endif 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #if defined(SOLVER_MA27) || defined(SOLVER_MA57) 55 | #include 56 | #include 57 | #endif 58 | 59 | #if !defined(__C_WRAPPER__) && !defined(__MATLAB__) 60 | #include 61 | #include 62 | #endif 63 | 64 | #else /* default compilation mode */ 65 | 66 | #include "qpOASES/QProblemB.hpp" 67 | #include "qpOASES/QProblem.hpp" 68 | #include "qpOASES/SQProblem.hpp" 69 | #include "qpOASES/SQProblemSchur.hpp" 70 | #include "qpOASES/extras/OQPinterface.hpp" 71 | #include "qpOASES/extras/SolutionAnalysis.hpp" 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/Bounds.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/Bounds.ipp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | * 31 | * Implementation of inlined member functions of the Bounds class designed 32 | * to manage working sets of bounds within a QProblem. 33 | */ 34 | 35 | 36 | BEGIN_NAMESPACE_QPOASES 37 | 38 | 39 | /***************************************************************************** 40 | * P U B L I C * 41 | *****************************************************************************/ 42 | 43 | /* 44 | * g e t N V 45 | */ 46 | inline int_t Bounds::getNV( ) const 47 | { 48 | return n; 49 | } 50 | 51 | 52 | /* 53 | * g e t N F V 54 | */ 55 | inline int_t Bounds::getNFV( ) const 56 | { 57 | return getNumberOfType( ST_EQUALITY ); 58 | } 59 | 60 | 61 | /* 62 | * g e t N B V 63 | */ 64 | inline int_t Bounds::getNBV( ) const 65 | { 66 | return getNumberOfType( ST_BOUNDED ); 67 | } 68 | 69 | 70 | /* 71 | * g e t N U V 72 | */ 73 | inline int_t Bounds::getNUV( ) const 74 | { 75 | return getNumberOfType( ST_UNBOUNDED ); 76 | } 77 | 78 | 79 | /* 80 | * g e t N F R 81 | */ 82 | inline int_t Bounds::getNFR( ) const 83 | { 84 | return freee.getLength( ); 85 | } 86 | 87 | 88 | /* 89 | * g e t N F X 90 | */ 91 | inline int_t Bounds::getNFX( ) const 92 | { 93 | return fixed.getLength( ); 94 | } 95 | 96 | 97 | /* 98 | * g e t F r e e 99 | */ 100 | inline Indexlist* Bounds::getFree( ) 101 | { 102 | return &freee; 103 | } 104 | 105 | 106 | /* 107 | * g e t F i x e d 108 | */ 109 | inline Indexlist* Bounds::getFixed( ) 110 | { 111 | return &fixed; 112 | } 113 | 114 | 115 | END_NAMESPACE_QPOASES 116 | 117 | 118 | /* 119 | * end of file 120 | */ 121 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/Constants.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/Constants.hpp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | * 31 | * Definition of all global constants. 32 | */ 33 | 34 | 35 | #ifndef QPOASES_CONSTANTS_HPP 36 | #define QPOASES_CONSTANTS_HPP 37 | 38 | 39 | #include "Types.hpp" 40 | 41 | 42 | BEGIN_NAMESPACE_QPOASES 43 | 44 | 45 | /** Numerical value of machine precision (min eps, s.t. 1+eps > 1). 46 | * Note: this value has to be positive! */ 47 | #ifdef __USE_SINGLE_PRECISION__ 48 | const real_t EPS = 1.193e-07f; 49 | #else 50 | const real_t EPS = 2.221e-16; 51 | #endif /* __USE_SINGLE_PRECISION__ */ 52 | 53 | 54 | /** Numerical value of zero (for situations in which it would be 55 | * unreasonable to compare with 0.0). 56 | * Note: this value has to be positive! */ 57 | const real_t ZERO = 1.0e-25; 58 | 59 | /** Numerical value of infinity (e.g. for non-existing bounds). 60 | Note: this value has to be positive! */ 61 | const real_t INFTY = 1.0e20; 62 | 63 | 64 | /** Maximum number of characters within a string. 65 | * Note: this value should be at least 41! */ 66 | const uint_t MAX_STRING_LENGTH = 160; 67 | 68 | 69 | END_NAMESPACE_QPOASES 70 | 71 | 72 | #endif /* QPOASES_CONSTANTS_HPP */ 73 | 74 | 75 | /* 76 | * end of file 77 | */ 78 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/Constraints.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/Constraints.ipp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | * 31 | * Declaration of inlined member functions of the Constraints class designed 32 | * to manage working sets of constraints within a QProblem. 33 | */ 34 | 35 | 36 | BEGIN_NAMESPACE_QPOASES 37 | 38 | 39 | /***************************************************************************** 40 | * P U B L I C * 41 | *****************************************************************************/ 42 | 43 | 44 | /* 45 | * g e t N C 46 | */ 47 | inline int_t Constraints::getNC( ) const 48 | { 49 | return n; 50 | } 51 | 52 | 53 | /* 54 | * g e t N E C 55 | */ 56 | inline int_t Constraints::getNEC( ) const 57 | { 58 | return getNumberOfType( ST_EQUALITY ); 59 | } 60 | 61 | 62 | /* 63 | * g e t N I C 64 | */ 65 | inline int_t Constraints::getNIC( ) const 66 | { 67 | return getNumberOfType( ST_BOUNDED ); 68 | } 69 | 70 | 71 | /* 72 | * g e t N U C 73 | */ 74 | inline int_t Constraints::getNUC( ) const 75 | { 76 | return getNumberOfType( ST_UNBOUNDED ); 77 | } 78 | 79 | 80 | /* 81 | * g e t N A C 82 | */ 83 | inline int_t Constraints::getNAC( ) const 84 | { 85 | return active.getLength( ); 86 | } 87 | 88 | 89 | /* 90 | * g e t N I A C 91 | */ 92 | inline int_t Constraints::getNIAC( ) const 93 | { 94 | return inactive.getLength( ); 95 | } 96 | 97 | 98 | 99 | /* 100 | * g e t A c t i v e 101 | */ 102 | inline Indexlist* Constraints::getActive( ) 103 | { 104 | return &active; 105 | } 106 | 107 | 108 | /* 109 | * g e t I n a c t i v e 110 | */ 111 | inline Indexlist* Constraints::getInactive( ) 112 | { 113 | return &inactive; 114 | } 115 | 116 | 117 | END_NAMESPACE_QPOASES 118 | 119 | 120 | /* 121 | * end of file 122 | */ 123 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/Indexlist.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/Indexlist.ipp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | * 31 | * Implementation of inlined member functions of the Indexlist class designed 32 | * to manage index lists of constraints and bounds within a QProblem_SubjectTo. 33 | */ 34 | 35 | 36 | BEGIN_NAMESPACE_QPOASES 37 | 38 | 39 | /***************************************************************************** 40 | * P U B L I C * 41 | *****************************************************************************/ 42 | 43 | 44 | /* 45 | * g e t N u m b e r 46 | */ 47 | inline int_t Indexlist::getNumber( int_t physicalindex ) const 48 | { 49 | /* consistency check */ 50 | if ( ( physicalindex < 0 ) || ( physicalindex > length ) ) 51 | return -RET_INDEXLIST_OUTOFBOUNDS; 52 | 53 | return number[physicalindex]; 54 | } 55 | 56 | 57 | /* 58 | * g e t L e n g t h 59 | */ 60 | inline int_t Indexlist::getLength( ) const 61 | { 62 | return length; 63 | } 64 | 65 | 66 | /* 67 | * g e t L a s t N u m b e r 68 | */ 69 | inline int_t Indexlist::getLastNumber( ) const 70 | { 71 | return number[length-1]; 72 | } 73 | 74 | 75 | /* 76 | * g e t L a s t N u m b e r 77 | */ 78 | inline BooleanType Indexlist::isMember( int_t _number ) const 79 | { 80 | if ( getIndex( _number ) >= 0 ) 81 | return BT_TRUE; 82 | else 83 | return BT_FALSE; 84 | } 85 | 86 | 87 | END_NAMESPACE_QPOASES 88 | 89 | 90 | /* 91 | * end of file 92 | */ 93 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/SQProblem.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/SQProblem.ipp 27 | * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | * \version 3.2 29 | * \date 2007-2017 30 | * 31 | * Implementation of inlined member functions of the SQProblem class which 32 | * is able to use the newly developed online active set strategy for 33 | * parametric quadratic programming with varying matrices. 34 | */ 35 | 36 | 37 | 38 | /***************************************************************************** 39 | * P U B L I C * 40 | *****************************************************************************/ 41 | 42 | 43 | BEGIN_NAMESPACE_QPOASES 44 | 45 | 46 | END_NAMESPACE_QPOASES 47 | 48 | 49 | /* 50 | * end of file 51 | */ 52 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/SQProblemSchur.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2013 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/SQProblemSchur.ipp 27 | * \author Andreas Waechter, Dennis Janka 28 | * \version 3.2 29 | * \date 2012-2017 30 | * 31 | * Implementation of inlined member functions of the SQProblemSchur class which 32 | * is able to use the newly developed online active set strategy for 33 | * parametric quadratic programming. 34 | */ 35 | 36 | 37 | BEGIN_NAMESPACE_QPOASES 38 | 39 | 40 | /***************************************************************************** 41 | * P U B L I C * 42 | *****************************************************************************/ 43 | 44 | /* 45 | * g e t N u m F a c t o r i z a t i o n s 46 | */ 47 | inline int_t SQProblemSchur::getNumFactorizations( ) const 48 | { 49 | return numFactorizations; 50 | } 51 | 52 | END_NAMESPACE_QPOASES 53 | 54 | 55 | /* 56 | * end of file 57 | */ 58 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/UnitTesting.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/UnitTesting.hpp 27 | * \author Hans Joachim Ferreau 28 | * \version 3.2 29 | * \date 2014-2017 30 | * 31 | * Definition of auxiliary functions/macros for unit testing. 32 | */ 33 | 34 | 35 | #ifndef QPOASES_UNIT_TESTING_HPP 36 | #define QPOASES_UNIT_TESTING_HPP 37 | 38 | 39 | #ifndef TEST_TOL_FACTOR 40 | #define TEST_TOL_FACTOR 1 41 | #endif 42 | 43 | 44 | /** Return value for tests that passed. */ 45 | #define TEST_PASSED 0 46 | 47 | /** Return value for tests that failed. */ 48 | #define TEST_FAILED 1 49 | 50 | /** Return value for tests that could not run due to missing external data. */ 51 | #define TEST_DATA_NOT_FOUND 99 52 | 53 | 54 | /** Macro verifying that two numerical values are equal in order to pass unit test. */ 55 | #define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; } 56 | 57 | /** Macro verifying that two numerical values are close to each other in order to pass unit test. */ 58 | #define QPOASES_TEST_FOR_NEAR( x,y ) if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; } 59 | 60 | /** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */ 61 | #define QPOASES_TEST_FOR_TOL( x,tol ) if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; } 62 | 63 | /** Macro verifying that a logical expression holds in order to pass unit test. */ 64 | #define QPOASES_TEST_FOR_TRUE( x ) if ( (x) == false ) { return TEST_FAILED; } 65 | 66 | 67 | 68 | BEGIN_NAMESPACE_QPOASES 69 | 70 | 71 | END_NAMESPACE_QPOASES 72 | 73 | 74 | #endif /* QPOASES_UNIT_TESTING_HPP */ 75 | 76 | 77 | /* 78 | * end of file 79 | */ 80 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/include/qpOASES/extras/SolutionAnalysis.ipp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of qpOASES. 3 | * 4 | * qpOASES -- An Implementation of the Online Active Set Strategy. 5 | * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | * Christian Kirches et al. All rights reserved. 7 | * 8 | * qpOASES is free software; you can redistribute it and/or 9 | * modify it under the terms of the GNU Lesser General Public 10 | * License as published by the Free Software Foundation; either 11 | * version 2.1 of the License, or (at your option) any later version. 12 | * 13 | * qpOASES is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | * See the GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public 19 | * License along with qpOASES; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | */ 23 | 24 | 25 | /** 26 | * \file include/qpOASES/extras/SolutionAnalysis.ipp 27 | * \author Hans Joachim Ferreau (thanks to Boris Houska) 28 | * \version 3.2 29 | * \date 2008-2017 30 | * 31 | * Implementation of inlined member functions of the SolutionAnalysis class 32 | * designed to perform additional analysis after solving a QP with qpOASES. 33 | * 34 | */ 35 | 36 | 37 | 38 | /***************************************************************************** 39 | * P U B L I C * 40 | *****************************************************************************/ 41 | 42 | 43 | BEGIN_NAMESPACE_QPOASES 44 | 45 | 46 | END_NAMESPACE_QPOASES 47 | 48 | 49 | /* 50 | * end of file 51 | */ 52 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_control/third_party/qpOASES/make.mk: -------------------------------------------------------------------------------- 1 | ## 2 | ## This file is part of qpOASES. 3 | ## 4 | ## qpOASES -- An Implementation of the Online Active Set Strategy. 5 | ## Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka, 6 | ## Christian Kirches et al. All rights reserved. 7 | ## 8 | ## qpOASES is free software; you can redistribute it and/or 9 | ## modify it under the terms of the GNU Lesser General Public 10 | ## License as published by the Free Software Foundation; either 11 | ## version 2.1 of the License, or (at your option) any later version. 12 | ## 13 | ## qpOASES is distributed in the hope that it will be useful, 14 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 16 | ## See the GNU Lesser General Public License for more details. 17 | ## 18 | ## You should have received a copy of the GNU Lesser General Public 19 | ## License along with qpOASES; if not, write to the Free Software 20 | ## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 21 | ## 22 | 23 | 24 | 25 | ## 26 | ## Filename: make.mk 27 | ## Author: Hans Joachim Ferreau, Andreas Potschka, Christian Kirches 28 | ## Version: 3.2 29 | ## Date: 2007-2017 30 | ## 31 | 32 | 33 | TOP = $(realpath $(dir $(lastword $(MAKEFILE_LIST)))) 34 | 35 | include ${TOP}/make_linux.mk 36 | #include ${TOP}/make_cygwin.mk 37 | #include ${TOP}/make_windows.mk 38 | #include ${TOP}/make_osx.mk 39 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hector_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | hector_gazebo: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 1000 6 | 7 | # L Controllers --------------------------------------- 8 | L_hip_controller: 9 | type: unitree_legged_control/UnitreeJointController 10 | joint: L_hip_joint 11 | pid: {p: 100.0, i: 0.0, d: 5.0} 12 | 13 | L_hip2_controller: 14 | type: unitree_legged_control/UnitreeJointController 15 | joint: L_hip2_joint 16 | pid: {p: 100.0, i: 0.0, d: 5.0} 17 | 18 | L_thigh_controller: 19 | type: unitree_legged_control/UnitreeJointController 20 | joint: L_thigh_joint 21 | pid: {p: 300.0, i: 0.0, d: 8.0} 22 | 23 | L_calf_controller: 24 | type: unitree_legged_control/UnitreeJointController 25 | joint: L_calf_joint 26 | pid: {p: 300.0, i: 0.0, d: 8.0} 27 | 28 | L_toe_controller: 29 | type: unitree_legged_control/UnitreeJointController 30 | joint: L_toe_joint 31 | pid: {p: 300.0, i: 0.0, d: 8.0} 32 | 33 | # R Controllers --------------------------------------- 34 | R_hip_controller: 35 | type: unitree_legged_control/UnitreeJointController 36 | joint: R_hip_joint 37 | pid: {p: 100.0, i: 0.0, d: 5.0} 38 | 39 | R_hip2_controller: 40 | type: unitree_legged_control/UnitreeJointController 41 | joint: R_hip2_joint 42 | pid: {p: 100.0, i: 0.0, d: 5.0} 43 | 44 | R_thigh_controller: 45 | type: unitree_legged_control/UnitreeJointController 46 | joint: R_thigh_joint 47 | pid: {p: 300.0, i: 0.0, d: 8.0} 48 | 49 | R_calf_controller: 50 | type: unitree_legged_control/UnitreeJointController 51 | joint: R_calf_joint 52 | pid: {p: 300.0, i: 0.0, d: 8.0} 53 | 54 | R_toe_controller: 55 | type: unitree_legged_control/UnitreeJointController 56 | joint: R_toe_joint 57 | pid: {p: 300.0, i: 0.0, d: 8.0} 58 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/launch/hector_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/L_calf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/L_calf.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/L_foot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/L_foot.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/L_hip1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/L_hip1.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/L_hip2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/L_hip2.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/L_thigh.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/L_thigh.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/R_calf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/R_calf.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/R_foot.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/R_foot.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/R_hip1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/R_hip1.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/R_hip2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/R_hip2.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/R_thigh.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/R_thigh.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/body.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/body.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/calf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/calf.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/hip.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/hip.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/hip2_L.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/hip2_L.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/hip2_R.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/hip2_R.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/left leg hip_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/left leg hip_1.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/right leg hip_1.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/right leg hip_1.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/thigh.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/thigh.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/toe.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/toe.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/meshes/trunk.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/hector_description/meshes/trunk.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hector_description 4 | 0.0.0 5 | The hector_description package 6 | 7 | yiyuc 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | 14 | 15 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/hector_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 | transmission_interface/SimpleTransmission 42 | 43 | hardware_interface/EffortJointInterface 44 | 45 | 46 | hardware_interface/EffortJointInterface 47 | 1 48 | 49 | 50 | 51 | 52 | transmission_interface/SimpleTransmission 53 | 54 | hardware_interface/EffortJointInterface 55 | 56 | 57 | hardware_interface/EffortJointInterface 58 | 1 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/mujoco_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mujoco_sim 4 | 0.0.0 5 | The mujoco_sim package 6 | 7 | 8 | 9 | 10 | lqk 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 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/mujoco_sim/script/__pycache__/mujoco_base.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/mujoco_sim/script/__pycache__/mujoco_base.cpython-38.pyc -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pai_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/config/robot_control.yaml: -------------------------------------------------------------------------------- 1 | pai_gazebo: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 1000 6 | 7 | # L Controllers --------------------------------------- 8 | L_hip_controller: 9 | type: unitree_legged_control/UnitreeJointController 10 | joint: L_hip_joint 11 | pid: {p: 100.0, i: 0.0, d: 5.0} 12 | 13 | L_hip2_controller: 14 | type: unitree_legged_control/UnitreeJointController 15 | joint: L_hip2_joint 16 | pid: {p: 100.0, i: 0.0, d: 5.0} 17 | 18 | L_thigh_controller: 19 | type: unitree_legged_control/UnitreeJointController 20 | joint: L_thigh_joint 21 | pid: {p: 300.0, i: 0.0, d: 8.0} 22 | 23 | L_calf_controller: 24 | type: unitree_legged_control/UnitreeJointController 25 | joint: L_calf_joint 26 | pid: {p: 300.0, i: 0.0, d: 8.0} 27 | 28 | L_toe_controller: 29 | type: unitree_legged_control/UnitreeJointController 30 | joint: L_toe_joint 31 | pid: {p: 300.0, i: 0.0, d: 8.0} 32 | 33 | # R Controllers --------------------------------------- 34 | R_hip_controller: 35 | type: unitree_legged_control/UnitreeJointController 36 | joint: R_hip_joint 37 | pid: {p: 100.0, i: 0.0, d: 5.0} 38 | 39 | R_hip2_controller: 40 | type: unitree_legged_control/UnitreeJointController 41 | joint: R_hip2_joint 42 | pid: {p: 100.0, i: 0.0, d: 5.0} 43 | 44 | R_thigh_controller: 45 | type: unitree_legged_control/UnitreeJointController 46 | joint: R_thigh_joint 47 | pid: {p: 300.0, i: 0.0, d: 8.0} 48 | 49 | R_calf_controller: 50 | type: unitree_legged_control/UnitreeJointController 51 | joint: R_calf_joint 52 | pid: {p: 300.0, i: 0.0, d: 8.0} 53 | 54 | R_toe_controller: 55 | type: unitree_legged_control/UnitreeJointController 56 | joint: R_toe_joint 57 | pid: {p: 300.0, i: 0.0, d: 8.0} 58 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/launch/hector_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/L_calf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/L_calf.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/L_hip.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/L_hip.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/L_hip2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/L_hip2.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/L_thigh.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/L_thigh.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/L_toe.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/L_toe.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/R_calf.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/R_calf.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/R_hip.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/R_hip.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/R_hip2.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/R_hip2.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/R_thigh.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/R_thigh.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/R_toe.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/R_toe.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/meshes/base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/pai_description/meshes/base_link.STL -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pai_description 4 | 0.0.0 5 | The pai_description package 6 | 7 | yiyuc 8 | TODO 9 | 10 | catkin 11 | roscpp 12 | std_msgs 13 | 14 | 15 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/pai_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 | transmission_interface/SimpleTransmission 42 | 43 | hardware_interface/EffortJointInterface 44 | 45 | 46 | hardware_interface/EffortJointInterface 47 | 1 48 | 49 | 50 | 51 | 52 | transmission_interface/SimpleTransmission 53 | 54 | hardware_interface/EffortJointInterface 55 | 56 | 57 | hardware_interface/EffortJointInterface 58 | 1 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_controller) 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/external_force.cpp) 52 | target_link_libraries(unitree_external_force ${catkin_LIBRARIES}) 53 | 54 | add_executable(unitree_servo src/servo.cpp) 55 | target_link_libraries(unitree_servo ${PROJECT_NAME} ${catkin_LIBRARIES}) 56 | 57 | add_executable(unitree_move_kinetic src/move_publisher.cpp) 58 | target_link_libraries(unitree_move_kinetic ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/include/body.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 __BODY_H__ 7 | #define __BODY_H__ 8 | 9 | #include "ros/ros.h" 10 | #include "unitree_legged_msgs/LowCmd.h" 11 | #include "unitree_legged_msgs/LowState.h" 12 | #include "unitree_legged_msgs/HighState.h" 13 | #define PosStopF (2.146E+9f) 14 | #define VelStopF (16000.f) 15 | 16 | namespace unitree_model { 17 | 18 | extern ros::Publisher servo_pub[12]; 19 | extern ros::Publisher highState_pub; 20 | extern unitree_legged_msgs::LowCmd lowCmd; 21 | extern unitree_legged_msgs::LowState lowState; 22 | 23 | void stand(); 24 | void motion_init(); 25 | void sendServoCmd(); 26 | void moveAllPosition(double* jointPositions, double duration); 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/launch/set_ctrl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_controller 4 | 0.0.0 5 | The unitree_controller package. 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 | unitree_legged_msgs 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/src/body.cpp: -------------------------------------------------------------------------------- 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 "body.h" 7 | 8 | namespace unitree_model { 9 | 10 | ros::Publisher servo_pub[12]; 11 | unitree_legged_msgs::LowCmd lowCmd; 12 | unitree_legged_msgs::LowState lowState; 13 | 14 | // These parameters are only for reference. 15 | // Actual patameters need to be debugged if you want to run on real robot. 16 | void paramInit() 17 | { 18 | for(int i=0; i<4; i++){ 19 | lowCmd.motorCmd[i*3+0].mode = 0x0A; 20 | lowCmd.motorCmd[i*3+0].Kp = 70; 21 | lowCmd.motorCmd[i*3+0].dq = 0; 22 | lowCmd.motorCmd[i*3+0].Kd = 3; 23 | lowCmd.motorCmd[i*3+0].tau = 0; 24 | lowCmd.motorCmd[i*3+1].mode = 0x0A; 25 | lowCmd.motorCmd[i*3+1].Kp = 180; 26 | lowCmd.motorCmd[i*3+1].dq = 0; 27 | lowCmd.motorCmd[i*3+1].Kd = 8; 28 | lowCmd.motorCmd[i*3+1].tau = 0; 29 | lowCmd.motorCmd[i*3+2].mode = 0x0A; 30 | lowCmd.motorCmd[i*3+2].Kp = 300; 31 | lowCmd.motorCmd[i*3+2].dq = 0; 32 | lowCmd.motorCmd[i*3+2].Kd = 15; 33 | lowCmd.motorCmd[i*3+2].tau = 0; 34 | } 35 | for(int i=0; i<12; i++){ 36 | lowCmd.motorCmd[i].q = lowState.motorState[i].q; 37 | } 38 | } 39 | 40 | void stand() 41 | { 42 | double pos[12] = {0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 43 | 0.0, 0.67, -1.3, -0.0, 0.67, -1.3}; 44 | moveAllPosition(pos, 2*1000); 45 | } 46 | 47 | void motion_init() 48 | { 49 | paramInit(); 50 | stand(); 51 | } 52 | 53 | void sendServoCmd() 54 | { 55 | for(int m=0; m<12; m++){ 56 | servo_pub[m].publish(lowCmd.motorCmd[m]); 57 | } 58 | ros::spinOnce(); 59 | usleep(1000); 60 | } 61 | 62 | void moveAllPosition(double* targetPos, double duration) 63 | { 64 | double pos[12] ,lastPos[12], percent; 65 | for(int j=0; j<12; j++) lastPos[j] = lowState.motorState[j].q; 66 | for(int i=1; i<=duration; i++){ 67 | if(!ros::ok()) break; 68 | percent = (double)i/duration; 69 | for(int j=0; j<12; j++){ 70 | lowCmd.motorCmd[j].q = lastPos[j]*(1-percent) + targetPos[j]*percent; 71 | } 72 | sendServoCmd(); 73 | } 74 | } 75 | 76 | 77 | } 78 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_controller/src/move_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | // #include 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | enum coord 14 | { 15 | WORLD, 16 | ROBOT 17 | }; 18 | coord def_frame = coord::WORLD; 19 | 20 | ros::init(argc, argv, "move_publisher"); 21 | ros::NodeHandle nh; 22 | ros::Publisher move_publisher = nh.advertise("/gazebo/set_model_state", 1000); 23 | 24 | gazebo_msgs::ModelState model_state_pub; 25 | 26 | std::string robot_name; 27 | ros::param::get("/robot_name", robot_name); 28 | std::cout << "robot_name: " << robot_name << std::endl; 29 | 30 | model_state_pub.model_name = robot_name + "_gazebo"; 31 | ros::Rate loop_rate(1000); 32 | 33 | if(def_frame == coord::WORLD) 34 | { 35 | model_state_pub.pose.position.x = 0.0; 36 | model_state_pub.pose.position.y = 0.0; 37 | model_state_pub.pose.position.z = 0.5; 38 | 39 | model_state_pub.pose.orientation.x = 0.0; 40 | model_state_pub.pose.orientation.y = 0.0; 41 | model_state_pub.pose.orientation.z = 0.0; 42 | model_state_pub.pose.orientation.w = 1.0; 43 | 44 | model_state_pub.reference_frame = "world"; 45 | 46 | long long time_ms = 0; //time, ms 47 | const double period = 5000; //ms 48 | const double radius = 1.5; //m 49 | tf::Quaternion q; 50 | while(ros::ok()) 51 | { 52 | model_state_pub.pose.position.x = radius * sin(2*M_PI*(double)time_ms/period); 53 | model_state_pub.pose.position.y = radius * cos(2*M_PI*(double)time_ms/period); 54 | model_state_pub.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, - 2*M_PI*(double)time_ms/period); 55 | 56 | move_publisher.publish(model_state_pub); 57 | loop_rate.sleep(); 58 | time_ms += 1; 59 | } 60 | } 61 | else if(def_frame == coord::ROBOT) 62 | { 63 | model_state_pub.twist.linear.x= 0.02; //0.02: 2cm/sec 64 | model_state_pub.twist.linear.y= 0.0; 65 | model_state_pub.twist.linear.z= 0.08; 66 | 67 | model_state_pub.twist.angular.x= 0.0; 68 | model_state_pub.twist.angular.y= 0.0; 69 | model_state_pub.twist.angular.z= 0.0; 70 | 71 | model_state_pub.reference_frame = "base"; 72 | 73 | while(ros::ok()) 74 | { 75 | move_publisher.publish(model_state_pub); 76 | loop_rate.sleep(); 77 | } 78 | } 79 | 80 | } -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | 52 | 53 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/launch/biped.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 | 42 | 43 | 44 | 45 | 46 | 47 | 52 | 53 | 54 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | unitree_legged_msgs 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/big_map.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | model://big_map 25 | 0 0 0 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/house_obstacles.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | model://house_obstacles 25 | 0 0 0 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/maze.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | model://maze 25 | 0 0 0 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/big_map/meshes/big_map.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/big_map/meshes/big_map.stl -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/big_map/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | big_map 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/big_map/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 0 0 0 5 | true 6 | 7 | 8 | 9 | 10 | model://big_map/meshes/big_map.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | model://big_map/meshes/big_map.stl 20 | 1 1 1 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/house_obstacles/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | house_obstacles 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/maze/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | maze 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/simple_house/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | simple_house 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/models/two_storeys/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | two_storeys 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/normal.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 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_gazebo/worlds/simple_house.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | model://simple_house 25 | 0 0 0 0 0 0 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_control/lib/libunitree_joint_control_tool.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/unitree_ros/unitree_legged_control/lib/libunitree_joint_control_tool.so -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_control/lib/libunitree_joint_control_tool_arm64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/humarobot/Hector_Simulation/8fa28ce8e58e08851e96a9f529470ceeeeac40a6/Hector_ROS_Simulation/unitree_ros/unitree_legged_control/lib/libunitree_joint_control_tool_arm64.so -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_control/unitree_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The unitree joint controller. 7 | 8 | 9 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | BmsCmd.msg 16 | BmsState.msg 17 | Cartesian.msg 18 | IMU.msg 19 | LED.msg 20 | LowCmd.msg 21 | LowState.msg 22 | HighCmd.msg 23 | HighState.msg 24 | ) 25 | 26 | generate_messages( 27 | DEPENDENCIES 28 | std_msgs 29 | geometry_msgs 30 | sensor_msgs 31 | ) 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS 35 | message_runtime 36 | std_msgs 37 | geometry_msgs 38 | sensor_msgs 39 | ) 40 | 41 | ############# 42 | ## Install ## 43 | ############# 44 | 45 | # Mark topic names header files for installation 46 | install( 47 | DIRECTORY include/${PROJECT_NAME}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | FILES_MATCHING PATTERN "*.h" 50 | ) -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/BmsCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 off # off 0xA5 2 | uint8[3] reserve -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/BmsState.msg: -------------------------------------------------------------------------------- 1 | uint8 version_h 2 | uint8 version_l 3 | uint8 bms_status 4 | uint8 SOC # SOC 0-100% 5 | int32 current # mA 6 | uint16 cycle 7 | int8[2] BQ_NTC # x1 degrees centigrade 8 | int8[2] MCU_NTC # x1 degrees centigrade 9 | uint16[10] cell_vol # cell voltage mV -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/Cartesian.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/HighCmd.msg: -------------------------------------------------------------------------------- 1 | uint8[2] head 2 | uint8 levelFlag 3 | uint8 frameReserve 4 | 5 | uint32[2] SN 6 | uint32[2] version 7 | uint16 bandWidth 8 | uint8 mode 9 | 10 | uint8 gaitType 11 | uint8 speedLevel 12 | float32 footRaiseHeight 13 | float32 bodyHeight 14 | float32[2] position 15 | float32[3] euler 16 | float32[2] velocity 17 | float32 yawSpeed 18 | BmsCmd bms 19 | LED[4] led 20 | uint8[40] wirelessRemote 21 | uint32 reserve 22 | 23 | uint32 crc -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/HighState.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | IMU imu 10 | MotorState[20] motorState 11 | BmsState bms 12 | int16[4] footForce 13 | int16[4] footForceEst 14 | uint8 mode 15 | float32 progress 16 | uint8 gaitType 17 | float32 footRaiseHeight 18 | float32[3] position 19 | float32 bodyHeight 20 | float32[3] velocity 21 | float32 yawSpeed 22 | float32[4] rangeObstacle 23 | Cartesian[4] footPosition2Body 24 | Cartesian[4] footSpeed2Body 25 | uint8[40] wirelessRemote 26 | uint32 reserve 27 | 28 | uint32 crc -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/IMU.msg: -------------------------------------------------------------------------------- 1 | float32[4] quaternion 2 | float32[3] gyroscope 3 | float32[3] accelerometer 4 | float32[3] rpy 5 | int8 temperature -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/LED.msg: -------------------------------------------------------------------------------- 1 | uint8 r 2 | uint8 g 3 | uint8 b -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/LowCmd.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | MotorCmd[20] motorCmd 10 | BmsCmd bms 11 | uint8[40] wirelessRemote 12 | uint32 reserve 13 | 14 | uint32 crc -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/unitree_legged_msgs/msg/LowState.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | IMU imu 10 | MotorState[20] motorState 11 | BmsState bms 12 | int16[4] footForce 13 | int16[4] footForceEst 14 | uint32 tick 15 | uint8[40] wirelessRemote 16 | uint32 reserve 17 | uint32 crc 18 | 19 | # Old version Aliengo does not have: 20 | Cartesian[4] eeForceRaw 21 | Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization 22 | Cartesian position # will delete 23 | Cartesian velocity # will delete 24 | Cartesian velocity_w # will delete 25 | 26 | -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 -------------------------------------------------------------------------------- /Hector_ROS_Simulation/unitree_ros/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 | -------------------------------------------------------------------------------- /License.md: -------------------------------------------------------------------------------- 1 | # HECTOR Software - 3-Clause BSD License 2 | 3 | Copyright (c) 2023, USC Dynamic Robotics and Control Laboratory 4 | All rights reserved. 5 | 6 | ## Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions, and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions, and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 3. Neither the name of the USC Dynamic Robotics and Control Laboratory nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | 12 | ## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | --------------------------------------------------------------------------------