├── .vscode ├── c_cpp_properties.json └── settings.json ├── LICENSE ├── README.assets └── legged_robot.gif ├── README.md ├── g1_controllers ├── CMakeLists.txt ├── config │ └── controllers.yaml ├── g1_controllers_plugins.xml ├── include │ └── g1_controllers │ │ ├── SafetyChecker.h │ │ ├── TargetTrajectoriesPublisher.h │ │ ├── g1Controller.h │ │ └── visualization │ │ └── g1SelfCollisionVisualization.h ├── launch │ ├── load_cheat_controller.launch.py │ ├── load_joy_controller.launch.py │ └── load_normal_controller.launch.py ├── package.xml └── src │ ├── CheatControllerNode.cpp │ ├── JoyControllerNode.cpp │ ├── NormalControllerNode.cpp │ ├── TargetTrajectoriesPublisher.cpp │ └── g1Controller.cpp ├── g1_dummy ├── CMakeLists.txt ├── include │ └── g1_dummy │ │ ├── gait │ │ ├── GaitKeyboardPublisher.h │ │ ├── GaitReceiver.h │ │ └── ModeSequenceTemplateRos.h │ │ └── visualization │ │ └── g1Visualizer.h ├── launch │ ├── legged_robot_sqp.launch.py │ └── multiplot.launch.py ├── package.xml ├── rviz │ └── g1.rviz └── src │ ├── g1DdpMpcNode.cpp │ ├── g1DummyNode.cpp │ ├── g1GaitCommandNode.cpp │ ├── g1IpmMpcNode.cpp │ ├── g1PoseCommandNode.cpp │ ├── g1SqpMpcNode.cpp │ ├── gait │ ├── GaitKeyboardPublisher.cpp │ └── GaitReceiver.cpp │ └── visualization │ └── g1Visualizer.cpp ├── g1_estimation ├── CMakeLists.txt ├── include │ └── g1_estimation │ │ ├── FromTopiceEstimate.h │ │ ├── LinearKalmanFilter.h │ │ └── StateEstimateBase.h ├── package.xml └── src │ ├── FromTopicEstimate.cpp │ ├── LinearKalmanFilter.cpp │ └── StateEstimateBase.cpp ├── g1_interface ├── CMakeLists.txt ├── auto_generated │ └── .gitignore ├── config │ ├── command │ │ ├── gait.info │ │ ├── gait_.info │ │ ├── reference.info │ │ └── reference_.info │ ├── mpc │ │ └── task.info │ └── multiplot │ │ ├── friction_cone.xml │ │ └── zero_velocity.xml ├── include │ └── g1_interface │ │ ├── common │ │ ├── ModelSettings.h │ │ ├── Types.h │ │ └── utils.h │ │ ├── constraint │ │ ├── EndEffectorLinearConstraint.h │ │ ├── FootRollConstraint.h │ │ ├── FrictionConeConstraint.h │ │ ├── LeggedSelfCollisionConstraint.h │ │ ├── NormalVelocityConstraintCppAd.h │ │ ├── ZeroForceConstraint.h │ │ └── ZeroVelocityConstraintCppAd.h │ │ ├── cost │ │ └── g1QuadraticTrackingCost.h │ │ ├── dynamics │ │ └── g1DynamicsAD.h │ │ ├── foot_planner │ │ ├── CubicSpline.h │ │ ├── SplineCpg.h │ │ └── SwingTrajectoryPlanner.h │ │ ├── g1Interface.h │ │ ├── g1PreComputation.h │ │ ├── gait │ │ ├── Gait.h │ │ ├── GaitSchedule.h │ │ ├── LegLogic.h │ │ ├── ModeSequenceTemplate.h │ │ └── MotionPhaseDefinition.h │ │ ├── initialization │ │ └── g1Initializer.h │ │ ├── package_path.h.in │ │ └── reference_manager │ │ └── SwitchedModelReferenceManager.h ├── package.xml ├── src │ ├── common │ │ └── ModelSettings.cpp │ ├── constraint │ │ ├── EndEffectorLinearConstraint.cpp │ │ ├── FootRollContraint.cpp │ │ ├── FrictionConeConstraint.cpp │ │ ├── NormalVelocityConstraintCppAd.cpp │ │ ├── ZeroForceConstraint.cpp │ │ └── ZeroVelocityConstraintCppAd.cpp │ ├── dynamics │ │ └── g1DynamicsAD.cpp │ ├── foot_planner │ │ ├── CubicSpline.cpp │ │ ├── SplineCpg.cpp │ │ └── SwingTrajectoryPlanner.cpp │ ├── g1Interface.cpp │ ├── g1PreComputation.cpp │ ├── gait │ │ ├── Gait.cpp │ │ ├── GaitSchedule.cpp │ │ ├── LegLogic.cpp │ │ └── ModeSequenceTemplate.cpp │ ├── initialization │ │ └── g1Initializer.cpp │ └── reference_manager │ │ └── SwitchedModelReferenceManager.cpp └── test │ ├── AnymalFactoryFunctions.cpp │ ├── constraint │ ├── testEndEffectorLinearConstraint.cpp │ ├── testFrictionConeConstraint.cpp │ └── testZeroForceConstraint.cpp │ └── include │ └── g1_interface │ └── test │ └── AnymalFactoryFunctions.h ├── g1_legged_description ├── .gitignore ├── CMakeLists.txt ├── config │ └── joint_names_g1_legged.yaml ├── export.log ├── launch │ ├── visualize.launch.py │ └── visualize.launch_.py ├── meshes │ ├── head_link.STL │ ├── left_ankle_pitch_link.STL │ ├── left_ankle_roll_link.STL │ ├── left_elbow_link.STL │ ├── left_hand_index_0_link.STL │ ├── left_hand_index_1_link.STL │ ├── left_hand_middle_0_link.STL │ ├── left_hand_middle_1_link.STL │ ├── left_hand_palm_link.STL │ ├── left_hand_thumb_0_link.STL │ ├── left_hand_thumb_1_link.STL │ ├── left_hand_thumb_2_link.STL │ ├── left_hip_pitch_link.STL │ ├── left_hip_roll_link.STL │ ├── left_hip_yaw_link.STL │ ├── left_knee_link.STL │ ├── left_rubber_hand.STL │ ├── left_shoulder_pitch_link.STL │ ├── left_shoulder_roll_link.STL │ ├── left_shoulder_yaw_link.STL │ ├── left_wrist_pitch_link.STL │ ├── left_wrist_roll_link.STL │ ├── left_wrist_roll_rubber_hand.STL │ ├── left_wrist_yaw_link.STL │ ├── logo_link.STL │ ├── pelvis.STL │ ├── pelvis_contour_link.STL │ ├── right_ankle_pitch_link.STL │ ├── right_ankle_roll_link.STL │ ├── right_elbow_link.STL │ ├── right_hand_index_0_link.STL │ ├── right_hand_index_1_link.STL │ ├── right_hand_middle_0_link.STL │ ├── right_hand_middle_1_link.STL │ ├── right_hand_palm_link.STL │ ├── right_hand_thumb_0_link.STL │ ├── right_hand_thumb_1_link.STL │ ├── right_hand_thumb_2_link.STL │ ├── right_hip_pitch_link.STL │ ├── right_hip_roll_link.STL │ ├── right_hip_yaw_link.STL │ ├── right_knee_link.STL │ ├── right_rubber_hand.STL │ ├── right_shoulder_pitch_link.STL │ ├── right_shoulder_roll_link.STL │ ├── right_shoulder_yaw_link.STL │ ├── right_wrist_pitch_link.STL │ ├── right_wrist_roll_link.STL │ ├── right_wrist_roll_rubber_hand.STL │ ├── right_wrist_yaw_link.STL │ ├── torso_constraint_L_link.STL │ ├── torso_constraint_L_rod_link.STL │ ├── torso_constraint_R_link.STL │ ├── torso_constraint_R_rod_link.STL │ ├── torso_link.STL │ ├── torso_link_23dof_rev_1_0.STL │ ├── torso_link_rev_1_0.STL │ ├── waist_constraint_L.STL │ ├── waist_constraint_R.STL │ ├── waist_roll_link.STL │ ├── waist_roll_link_rev_1_0.STL │ ├── waist_support_link.STL │ ├── waist_yaw_link.STL │ └── waist_yaw_link_rev_1_0.STL ├── mjcf │ ├── asset │ │ └── block.png │ ├── g1_23dof_rev_1_0.xml │ ├── g1_legged_control_.xml │ └── humanoid_legged_control_.xml ├── package.xml ├── rviz │ ├── g1_legged_control.rviz │ └── humanoid_legged_control.rviz └── urdf │ ├── g1_12dof.urdf │ ├── g1_legged_control_.urdf │ └── g1_legged_origin.urdf ├── g1_mujoco_sim ├── g1_mujoco_sim │ ├── __init__.py │ ├── g1_sim.py │ ├── joy.py │ ├── mujoco_base.py │ └── teleop.py ├── package.xml ├── resource │ └── g1_mujoco_sim ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── g1_wbc ├── CMakeLists.txt ├── include │ └── g1_wbc │ │ ├── HierarchicalWbc.h │ │ ├── HoQp.h │ │ ├── Task.h │ │ ├── WbcBase.h │ │ └── WeightedWbc.h ├── package.xml ├── src │ ├── HierarchicalWbc.cpp │ ├── HoQp.cpp │ ├── WbcBase.cpp │ └── WeightedWbc.cpp └── test │ └── HoQp_test.cpp └── qpoases_catkin ├── CMakeLists.txt └── package.xml /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "ros.distro": "humble", 3 | "python.analysis.extraPaths": [ 4 | "/home/zou/Workspace/ros2_ws/install/ocs2_msgs/lib/python3.10/site-packages", 5 | "/home/zou/Workspace/ros2_ws/install/g1_mujoco_sim/lib/python3.10/site-packages", 6 | "/opt/ros/humble/lib/python3.10/site-packages", 7 | "/opt/ros/humble/local/lib/python3.10/dist-packages", 8 | "/opt/openrobots/lib/python3.10/site-packages", 9 | "/home/zou/Documents/GitHub/drake-build/install/lib/python3.10/site-packages", 10 | "" 11 | ], 12 | "files.associations": { 13 | "cctype": "cpp", 14 | "clocale": "cpp", 15 | "cmath": "cpp", 16 | "csetjmp": "cpp", 17 | "csignal": "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 | "any": "cpp", 27 | "array": "cpp", 28 | "atomic": "cpp", 29 | "strstream": "cpp", 30 | "bit": "cpp", 31 | "bitset": "cpp", 32 | "chrono": "cpp", 33 | "cinttypes": "cpp", 34 | "codecvt": "cpp", 35 | "compare": "cpp", 36 | "complex": "cpp", 37 | "concepts": "cpp", 38 | "condition_variable": "cpp", 39 | "coroutine": "cpp", 40 | "cstdint": "cpp", 41 | "deque": "cpp", 42 | "forward_list": "cpp", 43 | "list": "cpp", 44 | "map": "cpp", 45 | "set": "cpp", 46 | "string": "cpp", 47 | "unordered_map": "cpp", 48 | "unordered_set": "cpp", 49 | "vector": "cpp", 50 | "exception": "cpp", 51 | "algorithm": "cpp", 52 | "functional": "cpp", 53 | "iterator": "cpp", 54 | "memory": "cpp", 55 | "memory_resource": "cpp", 56 | "numeric": "cpp", 57 | "optional": "cpp", 58 | "random": "cpp", 59 | "ratio": "cpp", 60 | "regex": "cpp", 61 | "string_view": "cpp", 62 | "system_error": "cpp", 63 | "tuple": "cpp", 64 | "type_traits": "cpp", 65 | "utility": "cpp", 66 | "fstream": "cpp", 67 | "future": "cpp", 68 | "initializer_list": "cpp", 69 | "iomanip": "cpp", 70 | "iosfwd": "cpp", 71 | "iostream": "cpp", 72 | "istream": "cpp", 73 | "limits": "cpp", 74 | "mutex": "cpp", 75 | "new": "cpp", 76 | "numbers": "cpp", 77 | "ostream": "cpp", 78 | "ranges": "cpp", 79 | "scoped_allocator": "cpp", 80 | "semaphore": "cpp", 81 | "shared_mutex": "cpp", 82 | "span": "cpp", 83 | "sstream": "cpp", 84 | "stdexcept": "cpp", 85 | "stop_token": "cpp", 86 | "streambuf": "cpp", 87 | "thread": "cpp", 88 | "cfenv": "cpp", 89 | "typeindex": "cpp", 90 | "typeinfo": "cpp", 91 | "valarray": "cpp", 92 | "variant": "cpp", 93 | "__nullptr": "cpp" 94 | } 95 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 botianhao 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.assets/legged_robot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/README.assets/legged_robot.gif -------------------------------------------------------------------------------- /g1_controllers/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controllers: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 100 5 | g1_controller: 6 | type: g1_controller/g1Controller 7 | g1_cheater_controller: 8 | type: g1_controller/g1CheaterController 9 | -------------------------------------------------------------------------------- /g1_controllers/g1_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | The basic g1 controller. 7 | 8 | 9 | 10 | 12 | 13 | The basic g1 controller with cheat state estimator 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /g1_controllers/include/g1_controllers/SafetyChecker.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/26. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace ocs2 { 10 | namespace g1 { 11 | using namespace centroidal_model; 12 | class SafetyChecker { 13 | public: 14 | explicit SafetyChecker(const CentroidalModelInfo& info) : info_(info) {} 15 | 16 | bool check(const SystemObservation& observation, const vector_t& /*optimized_state*/, const vector_t& /*optimized_input*/) { 17 | return checkOrientation(observation); 18 | } 19 | 20 | protected: 21 | bool checkOrientation(const SystemObservation& observation) { 22 | vector_t pose = getBasePose(observation.state, info_); 23 | if (pose(5) > M_PI_2 || pose(5) < -M_PI_2) { 24 | std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl; 25 | //output pose 26 | // std::cerr << "pose: " << pose << std::endl; 27 | return false; 28 | } 29 | return true; 30 | } 31 | 32 | const CentroidalModelInfo& info_; 33 | }; 34 | 35 | } // namespace g1 36 | } // namespace ocs2 37 | -------------------------------------------------------------------------------- /g1_controllers/include/g1_controllers/g1Controller.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/6/24. 3 | // 4 | 5 | #pragma once 6 | 7 | 8 | #include "rclcpp/rclcpp.hpp" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "g1_controllers/SafetyChecker.h" 19 | #include "g1_controllers/visualization/g1SelfCollisionVisualization.h" 20 | 21 | #include "std_msgs/msg/int8_multi_array.hpp" 22 | #include "std_msgs/msg/float32_multi_array.hpp" 23 | #include "std_msgs/msg/bool.hpp" 24 | #include "sensor_msgs/msg/imu.hpp" 25 | 26 | 27 | 28 | namespace g1_controller{ 29 | using namespace ocs2; 30 | using namespace g1; 31 | 32 | class g1Controller{ 33 | public: 34 | g1Controller() = default; 35 | ~g1Controller(); 36 | bool init(rclcpp::Node::SharedPtr &controller_nh); 37 | void update(const rclcpp::Time& time, const rclcpp::Duration& period); 38 | void starting(const rclcpp::Time& time); 39 | void stopping(const rclcpp::Time& /*time*/) { mpcRunning_ = false; } 40 | 41 | protected: 42 | virtual void updateStateEstimation(const rclcpp::Time& time, const rclcpp::Duration& period); 43 | 44 | virtual void setupg1Interface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, 45 | bool verbose); 46 | virtual void setupMpc(); 47 | virtual void setupMrt(); 48 | virtual void setupStateEstimate(const std::string& taskFile, bool verbose); 49 | 50 | void jointStateCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg); 51 | void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); 52 | void HwSwitchCallback(const std_msgs::msg::Bool::SharedPtr msg); 53 | 54 | // Interface 55 | std::shared_ptr g1Interface_; 56 | std::shared_ptr eeKinematicsPtr_; 57 | 58 | // State Estimation 59 | SystemObservation currentObservation_; 60 | vector_t measuredRbdState_; 61 | std::shared_ptr stateEstimate_; 62 | std::shared_ptr rbdConversions_; 63 | 64 | // Whole Body Control 65 | std::shared_ptr wbc_; 66 | std::shared_ptr safetyChecker_; 67 | 68 | // Nonlinear MPC 69 | std::shared_ptr mpc_; 70 | std::shared_ptr mpcMrtInterface_; 71 | 72 | // Visualization 73 | std::shared_ptr robotVisualizer_; 74 | rclcpp::Publisher::SharedPtr observationPublisher_; 75 | 76 | //Controller Interface 77 | rclcpp::Publisher::SharedPtr targetTorquePub_; 78 | rclcpp::Publisher::SharedPtr targetPosPub_; 79 | rclcpp::Publisher::SharedPtr targetVelPub_; 80 | rclcpp::Publisher::SharedPtr targetKpPub_; 81 | rclcpp::Publisher::SharedPtr targetKdPub_; 82 | 83 | rclcpp::Publisher::SharedPtr vs_xianyanPub_; 84 | rclcpp::Publisher::SharedPtr vs_basePub_;//估计的足端速度 85 | rclcpp::Publisher::SharedPtr foot_vel_estimatePub_;//估计的足端速度 86 | rclcpp::Publisher::SharedPtr cmd_contactFlagPub_;//命令的接触状态 87 | 88 | 89 | rclcpp::Subscription::SharedPtr jointPosVelSub_; 90 | rclcpp::Subscription::SharedPtr imuSub_; 91 | rclcpp::Subscription::SharedPtr hwSwitchSub_; 92 | 93 | bool hwSwitch_ = false; 94 | 95 | // Node Handle 96 | rclcpp::Node::SharedPtr controllerNh_; 97 | 98 | 99 | private: 100 | std::thread mpcThread_; 101 | std::atomic_bool controllerRunning_{}, mpcRunning_{}; 102 | benchmark::RepeatedTimer mpcTimer_; 103 | benchmark::RepeatedTimer wbcTimer_; 104 | size_t jointNum_ = 12; 105 | vector_t jointPos_, jointVel_; 106 | Eigen::Quaternion quat_; 107 | contact_flag_t contactFlag_; 108 | vector3_t angularVel_, linearAccel_; 109 | matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; 110 | size_t plannedMode_ = 3; 111 | vector_t defalutJointPos_; 112 | vector_t joint_pos_bias_; 113 | }; 114 | 115 | class g1CheaterController : public g1Controller { 116 | protected: 117 | void setupStateEstimate(const std::string& taskFile, bool verbose) override; 118 | }; 119 | 120 | } // namespace g1_controller 121 | 122 | -------------------------------------------------------------------------------- /g1_controllers/include/g1_controllers/visualization/g1SelfCollisionVisualization.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 23-1-30. 3 | // 4 | 5 | #pragma once 6 | #include "rclcpp/rclcpp.hpp" 7 | 8 | #include 9 | 10 | #include 11 | 12 | namespace ocs2 { 13 | namespace g1 { 14 | 15 | 16 | class g1SelfCollisionVisualization : public GeometryInterfaceVisualization { 17 | public: 18 | g1SelfCollisionVisualization(PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface, 19 | const CentroidalModelPinocchioMapping& mapping, rclcpp::Node::SharedPtr &node, scalar_t maxUpdateFrequency = 50.0) 20 | : mappingPtr_(mapping.clone()), 21 | GeometryInterfaceVisualization(std::move(pinocchioInterface), std::move(geometryInterface), "odom"), 22 | lastTime_(std::numeric_limits::lowest()), 23 | minPublishTimeDifference_(1.0 / maxUpdateFrequency) {} 24 | void update(const SystemObservation& observation) { 25 | if (observation.time - lastTime_ > minPublishTimeDifference_) { 26 | lastTime_ = observation.time; 27 | 28 | publishDistances(mappingPtr_->getPinocchioJointPosition(observation.state)); 29 | } 30 | } 31 | 32 | private: 33 | std::unique_ptr mappingPtr_; 34 | 35 | scalar_t lastTime_; 36 | scalar_t minPublishTimeDifference_; 37 | }; 38 | 39 | } // namespace g1 40 | } // namespace ocs2 41 | -------------------------------------------------------------------------------- /g1_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_controllers 4 | 0.0.0 5 | The g1_controllers packages. 6 | Qiayuan Liao 7 | BSD 8 | Qiayuan Liao 9 | 10 | 11 | ament_cmake 12 | cmake_clang_tools 13 | 14 | 15 | rclcpp 16 | g1_interface 17 | g1_estimation 18 | g1_wbc 19 | geometry_msgs 20 | ocs2_msgs 21 | ocs2_ros_interfaces 22 | std_msgs 23 | g1_dummy 24 | ocs2_self_collision 25 | ocs2_self_collision_visualization 26 | 27 | angles 28 | hpp-fcl 29 | ament_lint_auto 30 | ament_lint_common 31 | controller_manager 32 | 33 | ament_cmake 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /g1_controllers/src/CheatControllerNode.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by pocket on 24-2-11. 3 | // 4 | 5 | #include "g1_controllers/g1Controller.h" 6 | #include 7 | 8 | using Duration = std::chrono::duration; 9 | using Clock = std::chrono::high_resolution_clock; 10 | 11 | bool pause_flag = false; 12 | 13 | void pauseCallback(const std_msgs::msg::Bool::SharedPtr msg){ 14 | pause_flag = msg->data; 15 | std::cerr << "pause_flag: " << pause_flag << std::endl; 16 | } 17 | 18 | int main(int argc, char** argv){ 19 | rclcpp::Duration elapsedTime_ = rclcpp::Duration::from_seconds(0.002); 20 | 21 | 22 | // Initialize ros node 23 | rclcpp::init(argc, argv); 24 | rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( 25 | "g1_controller_node", 26 | rclcpp::NodeOptions() 27 | .allow_undeclared_parameters(true) 28 | .automatically_declare_parameters_from_overrides(true)); 29 | 30 | //create a subscriber to pauseFlag 31 | //这个订阅者监听名为"pauseFlag"的话题（topic），该话题发布布尔类型的消息。 32 | //当收到消息时，会调用pauseCallback回调函数处理这些消息, 只保留最新的一条消息. 33 | auto pause_sub = node->create_subscription("pauseFlag", 1, pauseCallback); 34 | g1_controller::g1CheaterController controller; 35 | if (!controller.init(node)) { 36 | RCLCPP_ERROR(node->get_logger(),"Failed to initialize the g1 controller!"); 37 | return -1; 38 | } 39 | 40 | auto startTime = Clock::now(); //系统高精度时钟,精确测量控制循环的时间间隔 41 | auto startTimeROS = node->get_clock()->now(); //保持与ROS系统同步 42 | controller.starting(startTimeROS); 43 | auto lastTime = startTime; 44 | 45 | //create a thread to spin the node 46 | std::thread spin_thread([node](){ 47 | rclcpp::spin(node); 48 | }); 49 | spin_thread.detach(); 50 | 51 | while(rclcpp::ok()){ 52 | if (!pause_flag) 53 | { 54 | const auto currentTime = Clock::now(); 55 | // Compute desired duration rounded to clock decimation 56 | const Duration desiredDuration(1.0 / 500); 57 | 58 | // Get change in time 59 | Duration time_span = std::chrono::duration_cast(currentTime - lastTime); 60 | elapsedTime_ = rclcpp::Duration::from_seconds(time_span.count()); 61 | lastTime = currentTime; 62 | 63 | // Check cycle time for excess delay 64 | // const double cycle_time_error = (elapsedTime_ - ros::Duration(desiredDuration.count())).toSec(); 65 | // if (cycle_time_error > cycleTimeErrorThreshold_) { 66 | // ROS_WARN_STREAM("Cycle time exceeded error threshold by: " << cycle_time_error - cycleTimeErrorThreshold_ << "s, " 67 | // << "cycle time: " << elapsedTime_ << "s, " 68 | // << "threshold: " << cycleTimeErrorThreshold_ << "s"); 69 | // } 70 | 71 | // Control 72 | // let the controller compute the new command (via the controller manager) 73 | controller.update(node->get_clock()->now(), elapsedTime_); 74 | 75 | // Sleep 76 | const auto sleepTill = currentTime + std::chrono::duration_cast(desiredDuration); 77 | std::this_thread::sleep_until(sleepTill); 78 | } 79 | } 80 | 81 | 82 | 83 | return 0; 84 | } -------------------------------------------------------------------------------- /g1_controllers/src/NormalControllerNode.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by pocket on 24-2-11. 3 | // 4 | 5 | #include "g1_controllers/g1Controller.h" 6 | #include 7 | #include "rclcpp/rclcpp.hpp" 8 | 9 | using Duration = std::chrono::duration; 10 | using Clock = std::chrono::high_resolution_clock; 11 | 12 | bool pause_flag = false; 13 | 14 | void pauseCallback(const std_msgs::msg::Bool::SharedPtr msg){ 15 | pause_flag = msg->data; 16 | std::cerr << "pause_flag: " << pause_flag << std::endl; 17 | } 18 | 19 | int main(int argc, char** argv){ 20 | rclcpp::Duration elapsedTime_ = rclcpp::Duration::from_seconds(0.002); 21 | 22 | rclcpp::init(argc, argv); 23 | rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( 24 | "g1_controller_node", 25 | rclcpp::NodeOptions() 26 | .allow_undeclared_parameters(true) 27 | .automatically_declare_parameters_from_overrides(true)); 28 | 29 | 30 | 31 | //create a subscriber to pauseFlag 32 | auto pause_sub = node->create_subscription("pauseFlag", 1, pauseCallback); 33 | g1_controller::g1Controller controller; 34 | 35 | if (!controller.init(node)) { 36 | RCLCPP_ERROR(node->get_logger(),"Failed to initialize the g1 controller!"); 37 | return -1; 38 | } 39 | 40 | auto startTime = Clock::now(); 41 | auto startTimeROS = node->get_clock()->now(); 42 | controller.starting(startTimeROS); 43 | auto lastTime = startTime; 44 | 45 | //create a thread to spin the node 46 | std::thread spin_thread([node](){ 47 | rclcpp::spin(node); 48 | }); 49 | spin_thread.detach(); 50 | 51 | while(rclcpp::ok()){ 52 | if (!pause_flag) 53 | { 54 | const auto currentTime = Clock::now(); 55 | // Compute desired duration rounded to clock decimation 56 | const Duration desiredDuration(1.0 / 500); 57 | 58 | // Get change in time 59 | Duration time_span = std::chrono::duration_cast(currentTime - lastTime); 60 | elapsedTime_ = rclcpp::Duration::from_seconds(time_span.count()); 61 | lastTime = currentTime; 62 | 63 | // Check cycle time for excess delay 64 | // const double cycle_time_error = (elapsedTime_ - ros::Duration(desiredDuration.count())).toSec(); 65 | // if (cycle_time_error > cycleTimeErrorThreshold_) { 66 | // ROS_WARN_STREAM("Cycle time exceeded error threshold by: " << cycle_time_error - cycleTimeErrorThreshold_ << "s, " 67 | // << "cycle time: " << elapsedTime_ << "s, " 68 | // << "threshold: " << cycleTimeErrorThreshold_ << "s"); 69 | // } 70 | 71 | // Control 72 | // let the controller compute the new command (via the controller manager) 73 | controller.update(node->get_clock()->now(), elapsedTime_); 74 | 75 | // Sleep 76 | const auto sleepTill = currentTime + std::chrono::duration_cast(desiredDuration); 77 | std::this_thread::sleep_until(sleepTill); 78 | } 79 | } 80 | 81 | 82 | 83 | return 0; 84 | } -------------------------------------------------------------------------------- /g1_dummy/include/g1_dummy/gait/GaitKeyboardPublisher.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace ocs2 { 44 | namespace g1 { 45 | 46 | /** This class implements ModeSequence communication using ROS. */ 47 | class GaitKeyboardPublisher { 48 | public: 49 | GaitKeyboardPublisher(const rclcpp::Node::SharedPtr &node, const std::string& gaitFile, const std::string& robotName, bool verbose = false); 50 | 51 | /** Prints the command line interface and responds to user input. Function returns after one user input. */ 52 | void getKeyboardCommand(); 53 | void publishGaitCommandFromString(const std::string& gaitCommand); 54 | 55 | private: 56 | /** Prints the list of available gaits. */ 57 | void printGaitList(const std::vector& gaitList) const; 58 | 59 | std::vector gaitList_; 60 | std::map gaitMap_; 61 | 62 | rclcpp::Publisher::SharedPtr modeSequenceTemplatePublisher_; 63 | rclcpp::Publisher::SharedPtr modeSequencePublisher_;//发送的是string 64 | }; 65 | 66 | } // namespace g1 67 | } // end of namespace ocs2 68 | -------------------------------------------------------------------------------- /g1_dummy/include/g1_dummy/gait/GaitReceiver.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace ocs2 { 45 | namespace g1 { 46 | 47 | class GaitReceiver : public SolverSynchronizedModule { 48 | public: 49 | GaitReceiver(const rclcpp::Node::SharedPtr &node, std::shared_ptr gaitSchedulePtr, const std::string& robotName); 50 | 51 | void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, 52 | const ReferenceManagerInterface& referenceManager) override; 53 | 54 | void postSolverRun(const PrimalSolution& primalSolution) override{}; 55 | 56 | private: 57 | void mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg); 58 | 59 | std::shared_ptr gaitSchedulePtr_; 60 | 61 | rclcpp::Subscription::SharedPtr mpcModeSequenceSubscriber_; 62 | 63 | std::mutex receivedGaitMutex_; 64 | std::atomic_bool gaitUpdated_; 65 | ModeSequenceTemplate receivedGait_; 66 | }; 67 | 68 | } // namespace g1 69 | } // namespace ocs2 70 | -------------------------------------------------------------------------------- /g1_dummy/include/g1_dummy/gait/ModeSequenceTemplateRos.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | namespace ocs2 { 39 | namespace g1 { 40 | 41 | /** Convert mode sequence template to ROS message */ 42 | inline ocs2_msgs::msg::ModeSchedule createModeSequenceTemplateMsg(const ModeSequenceTemplate& modeSequenceTemplate) { 43 | ::ocs2_msgs::msg::ModeSchedule modeScheduleMsg; 44 | modeScheduleMsg.event_times.assign(modeSequenceTemplate.switchingTimes.begin(), modeSequenceTemplate.switchingTimes.end()); 45 | modeScheduleMsg.mode_sequence.assign(modeSequenceTemplate.modeSequence.begin(), modeSequenceTemplate.modeSequence.end()); 46 | return modeScheduleMsg; 47 | } 48 | 49 | /** Convert ROS message to mode sequence template */ 50 | inline ModeSequenceTemplate readModeSequenceTemplateMsg(const ocs2_msgs::msg::ModeSchedule& modeScheduleMsg) { 51 | std::vector switchingTimes(modeScheduleMsg.event_times.begin(), modeScheduleMsg.event_times.end()); 52 | std::vector modeSequence(modeScheduleMsg.mode_sequence.begin(), modeScheduleMsg.mode_sequence.end()); 53 | return {switchingTimes, modeSequence}; 54 | } 55 | 56 | } // namespace g1 57 | } // namespace ocs2 58 | -------------------------------------------------------------------------------- /g1_dummy/launch/multiplot.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | import launch 5 | import launch_ros.actions 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | 9 | def generate_launch_description(): 10 | ld = launch.LaunchDescription([ 11 | launch.actions.DeclareLaunchArgument( 12 | name='metrics_config', 13 | default_value=get_package_share_directory( 14 | 'g1_interface') + '/config/multiplot/zero_velocity.xml' 15 | ), 16 | launch_ros.actions.Node( 17 | package='rqt_multiplot', 18 | executable='rqt_multiplot', 19 | name='mpc_metrics', 20 | output='screen' 21 | ), 22 | launch.actions.IncludeLaunchDescription( 23 | launch.launch_description_sources.PythonLaunchDescriptionSource( 24 | os.path.join(get_package_share_directory( 25 | 'ocs2_ros_interfaces'), 'launch/performance_indices.launch.py') 26 | ), 27 | launch_arguments={ 28 | 'mpc_policy_topic_name': 'g1_mpc_policy' 29 | }.items() 30 | ) 31 | ]) 32 | return ld 33 | 34 | 35 | if __name__ == '__main__': 36 | generate_launch_description() 37 | -------------------------------------------------------------------------------- /g1_dummy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_dummy 4 | 0.0.1 5 | The g1_dummy package 6 | 7 | Farbod Farshidian 8 | Ruben Grandia 9 | Jean-Pierre Sleiman 10 | 11 | BSD-3 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | tf2_ros 17 | urdf 18 | kdl_parser 19 | robot_state_publisher 20 | 21 | 22 | hpp-fcl 23 | ocs2_core 24 | ocs2_oc 25 | ocs2_ddp 26 | ocs2_mpc 27 | ocs2_sqp 28 | ocs2_ipm 29 | ocs2_robotic_tools 30 | ocs2_pinocchio_interface 31 | ocs2_centroidal_model 32 | ocs2_robotic_assets 33 | ocs2_msgs 34 | g1_interface 35 | ocs2_ros_interfaces 36 | 37 | pinocchio 38 | 39 | 40 | ament_cmake_gtest 41 | ament_lint_auto 42 | ament_lint_common 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /g1_dummy/src/g1DummyNode.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "g1_dummy/visualization/g1Visualizer.h" 39 | 40 | using namespace ocs2; 41 | using namespace g1; 42 | 43 | int main(int argc, char** argv) { 44 | const std::string robotName = "g1"; 45 | 46 | // Initialize ros node 47 | rclcpp::init(argc, argv); 48 | rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( 49 | robotName + "_mrt", 50 | rclcpp::NodeOptions() 51 | .allow_undeclared_parameters(true) 52 | .automatically_declare_parameters_from_overrides(true)); 53 | // Get node parameters 54 | const std::string taskFile = node->get_parameter("taskFile").as_string(); 55 | const std::string urdfFile = node->get_parameter("urdfFile").as_string(); 56 | const std::string referenceFile = node->get_parameter("referenceFile").as_string(); 57 | 58 | // Robot interface 59 | g1Interface interface(taskFile, urdfFile, referenceFile); 60 | 61 | // MRT 62 | MRT_ROS_Interface mrt(robotName); 63 | mrt.initRollout(&interface.getRollout()); 64 | mrt.launchNodes(node); 65 | 66 | // Visualization 67 | CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); 68 | PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, 69 | interface.modelSettings().contactNames3DoF); 70 | auto G1Visualizer = std::make_shared( 71 | interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, node); 72 | 73 | // Dummy legged robot 74 | MRT_ROS_Dummy_Loop g1DummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, 75 | interface.mpcSettings().mpcDesiredFrequency_); 76 | g1DummySimulator.subscribeObservers({G1Visualizer}); 77 | 78 | // Initial state 79 | SystemObservation initObservation; 80 | initObservation.state = interface.getInitialState(); 81 | initObservation.input = vector_t::Zero(interface.getCentroidalModelInfo().inputDim); 82 | initObservation.mode = ModeNumber::STANCE; 83 | 84 | // Initial command 85 | TargetTrajectories initTargetTrajectories({0.0}, {initObservation.state}, {initObservation.input}); 86 | 87 | // run dummy 88 | g1DummySimulator.run(initObservation, initTargetTrajectories); 89 | 90 | // Successful exit 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /g1_dummy/src/g1GaitCommandNode.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | #include "g1_dummy/gait/GaitKeyboardPublisher.h" 33 | 34 | using namespace ocs2; 35 | using namespace g1; 36 | 37 | int main(int argc, char* argv[]) { 38 | const std::string robotName = "g1"; 39 | 40 | // Initialize ros node 41 | rclcpp::init(argc, argv); 42 | rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared( 43 | robotName + "_mpc_mode_schedule", 44 | rclcpp::NodeOptions() 45 | .allow_undeclared_parameters(true) 46 | .automatically_declare_parameters_from_overrides(true)); 47 | // Get node parameters 48 | const std::string gaitCommandFile = node->get_parameter("gaitCommandFile").as_string(); 49 | std::cerr << "Loading gait file: " << gaitCommandFile << std::endl; 50 | 51 | GaitKeyboardPublisher gaitCommand(node, gaitCommandFile, robotName, true); 52 | 53 | while (rclcpp::ok()) { 54 | gaitCommand.getKeyboardCommand(); 55 | } 56 | 57 | // Successful exit 58 | return 0; 59 | } 60 | -------------------------------------------------------------------------------- /g1_dummy/src/gait/GaitReceiver.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_dummy/gait/GaitReceiver.h" 31 | 32 | #include "g1_dummy/gait/ModeSequenceTemplateRos.h" 33 | 34 | namespace ocs2 { 35 | namespace g1 { 36 | 37 | /******************************************************************************************************/ 38 | /******************************************************************************************************/ 39 | /******************************************************************************************************/ 40 | GaitReceiver::GaitReceiver(const rclcpp::Node::SharedPtr &node, std::shared_ptr gaitSchedulePtr, const std::string& robotName) 41 | : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) { 42 | mpcModeSequenceSubscriber_ = node->create_subscription(robotName + "_mpc_mode_schedule", 1, std::bind(&GaitReceiver::mpcModeSequenceCallback, this, 43 | std::placeholders::_1)); 44 | } 45 | 46 | /******************************************************************************************************/ 47 | /******************************************************************************************************/ 48 | /******************************************************************************************************/ 49 | void GaitReceiver::preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, 50 | const ReferenceManagerInterface& referenceManager) { 51 | if (gaitUpdated_) { 52 | std::lock_guard lock(receivedGaitMutex_); 53 | std::cerr << "[GaitReceiver]: Setting new gait after time " << finalTime << "\n"; 54 | std::cerr << receivedGait_; 55 | const auto timeHorizon = finalTime - initTime; 56 | gaitSchedulePtr_->insertModeSequenceTemplate(receivedGait_, finalTime, timeHorizon); 57 | gaitUpdated_ = false; 58 | } 59 | } 60 | 61 | /******************************************************************************************************/ 62 | /******************************************************************************************************/ 63 | /******************************************************************************************************/ 64 | void GaitReceiver::mpcModeSequenceCallback(const ocs2_msgs::msg::ModeSchedule::ConstSharedPtr& msg) { 65 | std::lock_guard lock(receivedGaitMutex_); 66 | receivedGait_ = readModeSequenceTemplateMsg(*msg); 67 | gaitUpdated_ = true; 68 | } 69 | 70 | } // namespace g1 71 | } // namespace ocs2 72 | -------------------------------------------------------------------------------- /g1_estimation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | set(CMAKE_CXX_STANDARD 17) 3 | project(g1_estimation) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | set(dependencies 9 | rclcpp 10 | hardware_interface 11 | tf2_ros 12 | nav_msgs 13 | g1_interface 14 | geometry_msgs 15 | tf2_geometry_msgs 16 | ) 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(geometry_msgs REQUIRED) 20 | find_package(tf2_ros REQUIRED) 21 | find_package(nav_msgs REQUIRED) 22 | find_package(g1_interface REQUIRED) 23 | find_package(PkgConfig REQUIRED) 24 | find_package(hardware_interface REQUIRED) 25 | find_package(tf2_geometry_msgs REQUIRED) 26 | pkg_check_modules(pinocchio REQUIRED pinocchio) 27 | 28 | ########### 29 | ## Build ## 30 | ########### 31 | 32 | # Resolve for the package path at compile time. 33 | 34 | 35 | set(FLAGS 36 | ${OCS2_CXX_FLAGS} 37 | ${pinocchio_CFLAGS_OTHER} 38 | -Wno-ignored-attributes 39 | -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor 40 | -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR 41 | -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR 42 | ) 43 | 44 | # Legged robot interface library 45 | add_library(${PROJECT_NAME} 46 | src/StateEstimateBase.cpp 47 | src/FromTopicEstimate.cpp 48 | src/LinearKalmanFilter.cpp 49 | ) 50 | 51 | target_include_directories(${PROJECT_NAME} 52 | PUBLIC 53 | "$" 54 | "$") 55 | 56 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 57 | target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) 58 | 59 | ######################### 60 | ### CLANG TOOLING ### 61 | ######################### 62 | 63 | find_package(cmake_clang_tools QUIET) 64 | if (cmake_clang_tools_FOUND) 65 | message(STATUS "Run clang tooling for target ocs2_legged_robot") 66 | add_clang_tooling( 67 | TARGETS ${PROJECT_NAME} 68 | SOURCE_DIRS src include test 69 | CT_HEADER_DIRS include 70 | CF_WERROR 71 | ) 72 | endif (cmake_clang_tools_FOUND) 73 | 74 | ############# 75 | ## Install ## 76 | ############# 77 | 78 | install( 79 | DIRECTORY include/ 80 | DESTINATION include/${PROJECT_NAME} 81 | ) 82 | 83 | install( 84 | TARGETS ${PROJECT_NAME} 85 | EXPORT export_${PROJECT_NAME} 86 | ARCHIVE DESTINATION lib 87 | LIBRARY DESTINATION lib 88 | RUNTIME DESTINATION bin 89 | ) 90 | 91 | install( 92 | DIRECTORY 93 | DESTINATION share/${PROJECT_NAME}/ 94 | ) 95 | 96 | ament_export_dependencies(${dependencies}) 97 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 98 | 99 | ############# 100 | ## Testing ## 101 | ############# 102 | 103 | 104 | ament_package() 105 | 106 | -------------------------------------------------------------------------------- /g1_estimation/include/g1_estimation/FromTopiceEstimate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/24. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include "g1_estimation/StateEstimateBase.h" 13 | 14 | 15 | 16 | #pragma once 17 | namespace ocs2 18 | { 19 | namespace g1 20 | { 21 | 22 | class FromTopicStateEstimate : public StateEstimateBase 23 | { 24 | public: 25 | FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, 26 | const PinocchioEndEffectorKinematics& eeKinematics, const rclcpp::Node::SharedPtr &node); 27 | 28 | void updateImu(const Eigen::Quaternion& quat, const vector3_t& angularVelLocal, 29 | const vector3_t& linearAccelLocal, const matrix3_t& orientationCovariance, 30 | const matrix3_t& angularVelCovariance, const matrix3_t& linearAccelCovariance) override{}; 31 | 32 | vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) override; 33 | 34 | private: 35 | void callback(const nav_msgs::msg::Odometry::SharedPtr msg); 36 | 37 | 38 | rclcpp::Subscription::SharedPtr sub_; 39 | nav_msgs::msg::Odometry buffer_; 40 | }; 41 | 42 | } // namespace g1 43 | } // namespace ocs2 44 | -------------------------------------------------------------------------------- /g1_estimation/include/g1_estimation/LinearKalmanFilter.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/24. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "g1_estimation/StateEstimateBase.h" 15 | 16 | #include 17 | #include 18 | 19 | 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace ocs2 27 | { 28 | namespace g1 29 | { 30 | 31 | class KalmanFilterEstimate : public StateEstimateBase 32 | { 33 | public: 34 | KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, 35 | const PinocchioEndEffectorKinematics& eeKinematics , const rclcpp::Node::SharedPtr &node); 36 | 37 | vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) override; 38 | 39 | void loadSettings(const std::string& taskFile, bool verbose); 40 | 41 | 42 | 43 | 44 | 45 | protected: 46 | void updateFromTopic(); 47 | 48 | void callback(const nav_msgs::msg::Odometry::SharedPtr msg); 49 | 50 | nav_msgs::msg::Odometry getOdomMsg(); 51 | 52 | vector_t feetHeights_; 53 | 54 | // Config 55 | scalar_t footRadius_ = 0.02; 56 | scalar_t imuProcessNoisePosition_ = 0.02; 57 | scalar_t imuProcessNoiseVelocity_ = 0.02; 58 | scalar_t footProcessNoisePosition_ = 0.002; 59 | scalar_t footSensorNoisePosition_ = 0.005; 60 | scalar_t footSensorNoiseVelocity_ = 0.1; 61 | scalar_t footHeightSensorNoise_ = 0.01; 62 | 63 | private: 64 | Eigen::Matrix xHat_; 65 | Eigen::Matrix ps_; 66 | Eigen::Matrix vs_; 67 | Eigen::Matrix a_; 68 | Eigen::Matrix q_; 69 | Eigen::Matrix p_; 70 | Eigen::Matrix r_; 71 | Eigen::Matrix b_; 72 | Eigen::Matrix c_; 73 | 74 | // Topic 75 | 76 | rclcpp::Subscription::SharedPtr sub_; 77 | nav_msgs::msg::Odometry buffer_; 78 | tf2_ros::Buffer tfBuffer_; 79 | tf2_ros::TransformListener tfListener_; 80 | tf2::Transform world2odom_; 81 | std::string frameOdom_, frameGuess_; 82 | bool topicUpdated_ = false; 83 | }; 84 | 85 | } // namespace g1 86 | } // namespace ocs2 87 | -------------------------------------------------------------------------------- /g1_estimation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_estimation 4 | 0.0.0 5 | The g1_estimation packages. 6 | bridgedp 7 | MIT 8 | 9 | ament_cmake 10 | 11 | cmake_clang_tools 12 | nav_msgs 13 | tf2_ros 14 | tf2_geometry_msgs 15 | rclcpp 16 | geometry_msgs 17 | g1_interface 18 | hardware_interface 19 | 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /g1_estimation/src/FromTopicEstimate.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/24. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include "g1_estimation/FromTopiceEstimate.h" 13 | #include 14 | 15 | 16 | namespace ocs2 17 | { 18 | namespace g1 19 | { 20 | FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, 21 | const PinocchioEndEffectorKinematics& eeKinematics, const rclcpp::Node::SharedPtr &node) 22 | : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics , node) 23 | { 24 | sub_ = node->create_subscription("/ground_truth/state", 10, std::bind(&FromTopicStateEstimate::callback, this, std::placeholders::_1)); 25 | } 26 | 27 | void FromTopicStateEstimate::callback(const nav_msgs::msg::Odometry::SharedPtr msg) 28 | { 29 | buffer_ = *msg; 30 | } 31 | 32 | vector_t FromTopicStateEstimate::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) 33 | { 34 | nav_msgs::msg::Odometry odom = buffer_; 35 | 36 | Eigen::Quaternion quat(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, 37 | odom.pose.pose.orientation.z); 38 | auto angularVelLocal = Eigen::Matrix(odom.twist.twist.angular.x, odom.twist.twist.angular.y, 39 | odom.twist.twist.angular.z); 40 | auto linearVelLocal = Eigen::Matrix(odom.twist.twist.linear.x, odom.twist.twist.linear.y, 41 | odom.twist.twist.linear.z); 42 | 43 | Eigen::Matrix linearVelWorld = quat.matrix() * linearVelLocal; 44 | 45 | vector3_t zyx = quatToZyx(quat) - zyxOffset_; 46 | vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( 47 | zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat), angularVelLocal)); 48 | updateAngular(zyx, angularVelGlobal); 49 | 50 | // updateAngular(quatToZyx(Eigen::Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, 51 | // odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)), 52 | // Eigen::Matrix(odom.twist.twist.angular.x, odom.twist.twist.angular.y, 53 | // odom.twist.twist.angular.z)); 54 | updateLinear( 55 | Eigen::Matrix(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), 56 | Eigen::Matrix(odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z)); 57 | //linearVelWorld); 58 | 59 | publishMsgs(odom); 60 | 61 | return rbdState_; 62 | } 63 | 64 | } // namespace g1 65 | } // namespace ocs2 -------------------------------------------------------------------------------- /g1_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | set(CMAKE_CXX_STANDARD 17) 3 | project(g1_interface) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | set(dependencies 9 | ocs2_core 10 | ocs2_oc 11 | ocs2_ddp 12 | ocs2_mpc 13 | ocs2_sqp 14 | ocs2_ipm 15 | ocs2_robotic_tools 16 | ocs2_pinocchio_interface 17 | ocs2_centroidal_model 18 | ocs2_robotic_assets 19 | ocs2_self_collision 20 | ) 21 | 22 | find_package(ament_cmake REQUIRED) 23 | find_package(ocs2_core REQUIRED) 24 | find_package(ocs2_oc REQUIRED) 25 | find_package(ocs2_ddp REQUIRED) 26 | find_package(ocs2_mpc REQUIRED) 27 | find_package(ocs2_sqp REQUIRED) 28 | find_package(ocs2_ipm REQUIRED) 29 | find_package(ocs2_centroidal_model REQUIRED) 30 | find_package(ocs2_pinocchio_interface REQUIRED) 31 | find_package(ocs2_robotic_assets REQUIRED) 32 | find_package(ocs2_robotic_tools REQUIRED) 33 | find_package(ocs2_self_collision REQUIRED) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | 39 | # Resolve for the package path at compile time. 40 | configure_file( 41 | "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" 42 | "${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY 43 | ) 44 | 45 | set(FLAGS 46 | ${OCS2_CXX_FLAGS} 47 | ${pinocchio_CFLAGS_OTHER} 48 | -Wno-ignored-attributes 49 | -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor 50 | -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR 51 | -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR 52 | ) 53 | 54 | # Legged robot interface library 55 | add_library(${PROJECT_NAME} 56 | src/common/ModelSettings.cpp 57 | src/dynamics/g1DynamicsAD.cpp 58 | src/constraint/EndEffectorLinearConstraint.cpp 59 | src/constraint/FrictionConeConstraint.cpp 60 | src/constraint/ZeroForceConstraint.cpp 61 | src/constraint/NormalVelocityConstraintCppAd.cpp 62 | src/constraint/ZeroVelocityConstraintCppAd.cpp 63 | src/constraint/FootRollContraint.cpp 64 | src/initialization/g1Initializer.cpp 65 | src/reference_manager/SwitchedModelReferenceManager.cpp 66 | src/foot_planner/CubicSpline.cpp 67 | src/foot_planner/SplineCpg.cpp 68 | src/foot_planner/SwingTrajectoryPlanner.cpp 69 | src/gait/Gait.cpp 70 | src/gait/GaitSchedule.cpp 71 | src/gait/LegLogic.cpp 72 | src/gait/ModeSequenceTemplate.cpp 73 | src/g1Interface.cpp 74 | src/g1PreComputation.cpp 75 | ) 76 | 77 | target_include_directories(${PROJECT_NAME} 78 | PUBLIC 79 | "$" 80 | "$") 81 | 82 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 83 | target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) 84 | 85 | ######################### 86 | ### CLANG TOOLING ### 87 | ######################### 88 | 89 | find_package(cmake_clang_tools QUIET) 90 | if (cmake_clang_tools_FOUND) 91 | message(STATUS "Run clang tooling for target ocs2_legged_robot") 92 | add_clang_tooling( 93 | TARGETS ${PROJECT_NAME} 94 | SOURCE_DIRS src include test 95 | CT_HEADER_DIRS include 96 | CF_WERROR 97 | ) 98 | endif (cmake_clang_tools_FOUND) 99 | 100 | ############# 101 | ## Install ## 102 | ############# 103 | 104 | install( 105 | DIRECTORY include/ 106 | DESTINATION include/${PROJECT_NAME} 107 | ) 108 | 109 | install( 110 | TARGETS ${PROJECT_NAME} 111 | EXPORT export_${PROJECT_NAME} 112 | ARCHIVE DESTINATION lib 113 | LIBRARY DESTINATION lib 114 | RUNTIME DESTINATION bin 115 | ) 116 | 117 | install( 118 | DIRECTORY config 119 | DESTINATION share/${PROJECT_NAME}/ 120 | ) 121 | 122 | ament_export_dependencies(${dependencies}) 123 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 124 | 125 | ############# 126 | ## Testing ## 127 | ############# 128 | find_package(ament_lint_auto REQUIRED) 129 | ament_lint_auto_find_test_dependencies() 130 | 131 | if (BUILD_TESTING) 132 | find_package(ament_cmake_gtest) 133 | 134 | ament_add_gtest(${PROJECT_NAME}_test 135 | test/AnymalFactoryFunctions.cpp 136 | test/constraint/testEndEffectorLinearConstraint.cpp 137 | test/constraint/testFrictionConeConstraint.cpp 138 | test/constraint/testZeroForceConstraint.cpp 139 | ) 140 | target_include_directories(${PROJECT_NAME}_test PRIVATE 141 | test/include 142 | ${PROJECT_BINARY_DIR}/include 143 | ) 144 | ament_target_dependencies(${PROJECT_NAME}_test ${dependencies}) 145 | target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) 146 | target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) 147 | endif () 148 | 149 | ament_package() -------------------------------------------------------------------------------- /g1_interface/auto_generated/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | 6 | -------------------------------------------------------------------------------- /g1_interface/config/command/gait.info: -------------------------------------------------------------------------------- 1 | list 2 | { 3 | [0] stance 4 | [1] trot 5 | [2] walk 6 | [3] quick_trot 7 | } 8 | 9 | stance 10 | { 11 | modeSequence 12 | { 13 | [0] STANCE 14 | } 15 | switchingTimes 16 | { 17 | [0] 0.0 18 | [1] 1000.0 19 | } 20 | } 21 | 22 | trot 23 | { 24 | modeSequence 25 | { 26 | [0] LCONTACT 27 | [1] RCONTACT 28 | } 29 | switchingTimes 30 | { 31 | [0] 0.0 32 | [1] 0.45 33 | [2] 0.9 34 | } 35 | } 36 | 37 | walk 38 | { 39 | modeSequence 40 | { 41 | [0] LCONTACT 42 | [1] STANCE 43 | [2] RCONTACT 44 | [3] STANCE 45 | } 46 | switchingTimes 47 | { 48 | [0] 0.0 ; 49 | [1] 0.4 ;0.45 50 | [2] 0.6 ;0.6 51 | [3] 1.0 ;1.05 52 | [4] 1.2 ;1.2 53 | } 54 | } 55 | 56 | quick_trot 57 | { 58 | modeSequence 59 | { 60 | [0] LCONTACT 61 | [1] RCONTACT 62 | } 63 | switchingTimes 64 | { 65 | [0] 0.0 66 | [1] 0.3 67 | [2] 0.6 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /g1_interface/config/command/gait_.info: -------------------------------------------------------------------------------- 1 | list 2 | { 3 | [0] stance 4 | [1] trot 5 | [2] walk 6 | [3] quick_trot 7 | } 8 | 9 | stance 10 | { 11 | modeSequence 12 | { 13 | [0] STANCE 14 | } 15 | switchingTimes 16 | { 17 | [0] 0.0 18 | [1] 1000.0 19 | } 20 | } 21 | 22 | trot 23 | { 24 | modeSequence 25 | { 26 | [0] LCONTACT 27 | [1] RCONTACT 28 | } 29 | switchingTimes 30 | { 31 | [0] 0.0 32 | [1] 0.45 33 | [2] 0.9 34 | } 35 | } 36 | 37 | walk 38 | { 39 | modeSequence 40 | { 41 | [0] LCONTACT 42 | [1] STANCE 43 | [2] RCONTACT 44 | [3] STANCE 45 | } 46 | switchingTimes 47 | { 48 | [0] 0.0 49 | [1] 0.45 50 | [2] 0.6 51 | [3] 1.05 52 | [4] 1.2 53 | } 54 | } 55 | 56 | quick_trot 57 | { 58 | modeSequence 59 | { 60 | [0] LCONTACT 61 | [1] RCONTACT 62 | } 63 | switchingTimes 64 | { 65 | [0] 0.0 66 | [1] 0.3 67 | [2] 0.6 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /g1_interface/config/command/reference.info: -------------------------------------------------------------------------------- 1 | targetDisplacementVelocity 0.5; 2 | targetRotationVelocity 0.3; 3 | 4 | comHeight 0.71 ;0.608 5 | 6 | defaultJointState 7 | { 8 | (0,0) -0.4 ; left_hip_pitch_joint 9 | (1,0) 0.0 ; left_hip_roll_joint 10 | (2,0) 0.0 ; left_hip_yaw_joint 11 | (3,0) 0.87 ; left_knee_joint 12 | (4,0) -0.52 ; left_ankle_pitch_joint 13 | (5,0) 0.0 ; left_ankle_roll_joint 14 | (6,0) -0.4 ; right_hip_pitch_joint 15 | (7,0) 0.0 ; right_hip_roll_joint 16 | (8,0) 0.0 ; right_hip_yaw_joint 17 | (9,0) 0.87 ; right_knee_joint 18 | (10,0) -0.52 ; right_ankle_pitch_joint 19 | (11,0) 0.00 ; right_ankle_roll_joint 20 | } 21 | 22 | initialModeSchedule 23 | { 24 | modeSequence 25 | { 26 | [0] STANCE 27 | [1] STANCE 28 | } 29 | eventTimes 30 | { 31 | [0] 1000.0 ; not used 32 | } 33 | } 34 | 35 | defaultModeSequenceTemplate 36 | { 37 | modeSequence 38 | { 39 | [0] STANCE 40 | } 41 | switchingTimes 42 | { 43 | [0] 0.0 44 | [1] 1000.0 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /g1_interface/config/command/reference_.info: -------------------------------------------------------------------------------- 1 | targetDisplacementVelocity 0.7; 2 | targetRotationVelocity 0.5; 3 | 4 | comHeight 0.71 5 | 6 | defaultJointState 7 | { 8 | (0,0) -0.4 ; left_hip_pitch_joint 9 | (1,0) 0.0 ; left_hip_roll_joint 10 | (2,0) 0.0 ; left_hip_yaw_joint 11 | (3,0) 0.87 ; left_knee_joint 12 | (4,0) -0.52 ; left_ankle_pitch_joint 13 | (5,0) 0.0 ; left_ankle_roll_joint 14 | (6,0) -0.4 ; right_hip_pitch_joint 15 | (7,0) 0.0 ; right_hip_roll_joint 16 | (8,0) 0.0 ; right_hip_yaw_joint 17 | (9,0) 0.87 ; right_knee_joint 18 | (10,0) -0.52 ; right_ankle_pitch_joint 19 | (11,0) 0.00 ; right_ankle_roll_joint 20 | } 21 | 22 | initialModeSchedule 23 | { 24 | modeSequence 25 | { 26 | [0] STANCE 27 | [1] STANCE 28 | } 29 | eventTimes 30 | { 31 | [0] 1000.0 ; not used 32 | } 33 | } 34 | 35 | defaultModeSequenceTemplate 36 | { 37 | modeSequence 38 | { 39 | [0] STANCE 40 | } 41 | switchingTimes 42 | { 43 | [0] 0.0 44 | [1] 1000.0 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/common/ModelSettings.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace ocs2 { 39 | namespace g1 { 40 | 41 | struct ModelSettings { 42 | scalar_t positionErrorGain = 0.0; 43 | 44 | scalar_t phaseTransitionStanceTime = 0.4; 45 | 46 | bool verboseCppAd = true; 47 | bool recompileLibrariesCppAd = true; 48 | std::string modelFolderCppAd = "/tmp/ocs2"; 49 | 50 | // This is only used to get names for the knees and to check urdf for extra joints that need to be fixed. 51 | std::vector jointNames{ 52 | "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", 53 | "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", 54 | "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", 55 | "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint" 56 | }; 57 | 58 | std::vector contactNames6DoF{}; 59 | std::vector contactNames3DoF{"l_foot_toe", "r_foot_toe", "l_foot_heel", "r_foot_heel"}; 60 | }; 61 | 62 | ModelSettings loadModelSettings(const std::string& filename, const std::string& fieldName = "model_settings", bool verbose = "true"); 63 | 64 | } // namespace g1 65 | } // namespace ocs2 66 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/common/Types.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | template 41 | using feet_array_t = std::array; 42 | using contact_flag_t = feet_array_t; 43 | 44 | using vector3_t = Eigen::Matrix; 45 | using vector5_t = Eigen::Matrix; 46 | using vector6_t = Eigen::Matrix; 47 | 48 | using matrix3_t = Eigen::Matrix; 49 | using matrix35_t = Eigen::Matrix; 50 | using matrix65_t = Eigen::Matrix; 51 | using matrix56_t = Eigen::Matrix; 52 | using matrix66_t = Eigen::Matrix; 53 | using quaternion_t = Eigen::Quaternion; 54 | 55 | using vectorf_t = Eigen::Matrix; 56 | using vector3f_t = Eigen::Matrix; 57 | using vector5f_t = Eigen::Matrix; 58 | using vector6f_t = Eigen::Matrix; 59 | 60 | using matrixf_t = Eigen::Matrix; 61 | using matrix3f_t = Eigen::Matrix; 62 | using matrix35f_t = Eigen::Matrix; 63 | using matrix65f_t = Eigen::Matrix; 64 | 65 | } // namespace g1 66 | } // namespace ocs2 67 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/common/utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include "g1_interface/common/Types.h" 41 | 42 | namespace ocs2 { 43 | namespace g1 { 44 | 45 | /******************************************************************************************************/ 46 | /******************************************************************************************************/ 47 | /******************************************************************************************************/ 48 | /** Counts contact feet */ 49 | inline size_t numberOfClosedContacts(const contact_flag_t& contactFlags) { 50 | size_t numStanceLegs = 0; 51 | for (auto legInContact : contactFlags) { 52 | if (legInContact) { 53 | ++numStanceLegs; 54 | } 55 | } 56 | return numStanceLegs; 57 | } 58 | 59 | /******************************************************************************************************/ 60 | /******************************************************************************************************/ 61 | /******************************************************************************************************/ 62 | /** Computes an input with zero joint velocity and forces which equally distribute the robot weight between contact feet. */ 63 | inline vector_t weightCompensatingInput(const CentroidalModelInfoTpl& info, const contact_flag_t& contactFlags) { 64 | const auto numStanceLegs = numberOfClosedContacts(contactFlags); 65 | vector_t input = vector_t::Zero(info.inputDim); 66 | if (numStanceLegs > 0) { 67 | const scalar_t totalWeight = info.robotMass * 9.81; 68 | const vector3_t forceInInertialFrame(0.0, 0.0, totalWeight / numStanceLegs); 69 | for (size_t i = 0; i < contactFlags.size(); i++) { 70 | if (contactFlags[i]) { 71 | centroidal_model::getContactForces(input, i, info) = forceInInertialFrame; 72 | } 73 | } // end of i loop 74 | } 75 | return input; 76 | } 77 | 78 | } // namespace g1 79 | } // namespace ocs2 80 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/EndEffectorLinearConstraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | namespace ocs2 { 39 | namespace g1 { 40 | 41 | /** 42 | * Defines a linear constraint on an end-effector position (xee) and linear velocity (vee). 43 | * g(xee, vee) = Ax * xee + Av * vee + b 44 | * - For defining constraint of type g(xee), set Av to matrix_t(0, 0) 45 | * - For defining constraint of type g(vee), set Ax to matrix_t(0, 0) 46 | */ 47 | class EndEffectorLinearConstraint final : public StateInputConstraint { 48 | public: 49 | /** 50 | * Coefficients of the linear constraints of the form: 51 | * g(xee, vee) = Ax * xee + Av * vee + b 52 | */ 53 | struct Config { 54 | vector_t b; 55 | matrix_t Ax; 56 | matrix_t Av; 57 | }; 58 | 59 | /** 60 | * Constructor 61 | * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. 62 | * @param [in] numConstraints: The number of constraints {1, 2, 3} 63 | * @param [in] config: The constraint coefficients, g(xee, vee) = Ax * xee + Av * vee + b 64 | */ 65 | EndEffectorLinearConstraint(const EndEffectorKinematics& endEffectorKinematics, size_t numConstraints, 66 | Config config = Config()); 67 | 68 | ~EndEffectorLinearConstraint() override = default; 69 | EndEffectorLinearConstraint* clone() const override { return new EndEffectorLinearConstraint(*this); } 70 | 71 | /** Sets a new constraint coefficients. */ 72 | void configure(Config&& config); 73 | /** Sets a new constraint coefficients. */ 74 | void configure(const Config& config) { this->configure(Config(config)); } 75 | 76 | /** Gets the underlying end-effector kinematics interface. */ 77 | EndEffectorKinematics& getEndEffectorKinematics() { return *endEffectorKinematicsPtr_; } 78 | 79 | size_t getNumConstraints(scalar_t time) const override { return numConstraints_; } 80 | vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; 81 | VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 82 | const PreComputation& preComp) const override; 83 | 84 | private: 85 | EndEffectorLinearConstraint(const EndEffectorLinearConstraint& rhs); 86 | 87 | std::unique_ptr> endEffectorKinematicsPtr_; 88 | const size_t numConstraints_; 89 | Config config_; 90 | }; 91 | 92 | } // namespace g1 93 | } // namespace ocs2 94 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/FootRollConstraint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file FootRollConstraint.h 3 | * @author xuejialong 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 15 | 16 | 17 | 18 | namespace ocs2 { 19 | namespace g1 { 20 | 21 | /** 22 | * Defines a linear constraint on an end-effector position (xee) and linear velocity (vee). 23 | * g(xee, vee) = Ax * xee + Av * vee + b 24 | * - For defining constraint of type g(xee), set Av to matrix_t(0, 0) 25 | * - For defining constraint of type g(vee), set Ax to matrix_t(0, 0) 26 | */ 27 | class FootRollConstraint final : public StateInputConstraint { 28 | public: 29 | 30 | /** 31 | * Constructor 32 | * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. 33 | * @param [in] numConstraints: The number of constraints {1, 2, 3} 34 | */ 35 | FootRollConstraint(const SwitchedModelReferenceManager& referenceManager, size_t contactPointIndex, 36 | CentroidalModelInfo info); 37 | 38 | ~FootRollConstraint() override = default; 39 | FootRollConstraint* clone() const override { return new FootRollConstraint(*this); } 40 | 41 | bool isActive(scalar_t time) const override; 42 | size_t getNumConstraints(scalar_t time) const override { return 1; } 43 | vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; 44 | VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 45 | const PreComputation& preComp) const override; 46 | 47 | private: 48 | FootRollConstraint(const FootRollConstraint& rhs); 49 | 50 | const SwitchedModelReferenceManager* referenceManagerPtr_; 51 | const size_t contactPointIndex_; 52 | const CentroidalModelInfo info_; 53 | }; 54 | 55 | } // namespace g1 56 | } // namespace ocs2 57 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/LeggedSelfCollisionConstraint.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by jialong xue on 23-1-29. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include "g1_interface/g1PreComputation.h" 10 | 11 | namespace ocs2 { 12 | namespace g1 { 13 | using namespace ocs2; 14 | 15 | class LeggedSelfCollisionConstraint final : public SelfCollisionConstraint { 16 | public: 17 | LeggedSelfCollisionConstraint(const CentroidalModelPinocchioMapping& mapping, PinocchioGeometryInterface pinocchioGeometryInterface, 18 | scalar_t minimumDistance) 19 | : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface), minimumDistance) {} 20 | ~LeggedSelfCollisionConstraint() override = default; 21 | LeggedSelfCollisionConstraint(const LeggedSelfCollisionConstraint& other) = default; 22 | LeggedSelfCollisionConstraint* clone() const override { return new LeggedSelfCollisionConstraint(*this); } 23 | 24 | const PinocchioInterface& getPinocchioInterface(const PreComputation& preComputation) const override { 25 | return cast(preComputation).getPinocchioInterface(); 26 | } 27 | }; 28 | 29 | } // namespace legged 30 | } // namespace ocs2 31 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/NormalVelocityConstraintCppAd.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include "g1_interface/constraint/EndEffectorLinearConstraint.h" 35 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | /** 41 | * Specializes the CppAd version of normal velocity constraint on an end-effector position and linear velocity. 42 | * Constructs the member EndEffectorLinearConstraint object with number of constraints of 1. 43 | * 44 | * See also EndEffectorLinearConstraint for the underlying computation. 45 | */ 46 | class NormalVelocityConstraintCppAd final : public StateInputConstraint { 47 | public: 48 | /** 49 | * Constructor 50 | * @param [in] referenceManager : Switched model ReferenceManager 51 | * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. 52 | * @param [in] contactPointIndex : The 3 DoF contact index. 53 | */ 54 | NormalVelocityConstraintCppAd(const SwitchedModelReferenceManager& referenceManager, 55 | const EndEffectorKinematics& endEffectorKinematics, size_t contactPointIndex); 56 | 57 | ~NormalVelocityConstraintCppAd() override = default; 58 | NormalVelocityConstraintCppAd* clone() const override { return new NormalVelocityConstraintCppAd(*this); } 59 | 60 | bool isActive(scalar_t time) const override; 61 | size_t getNumConstraints(scalar_t time) const override { return 1; } 62 | vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; 63 | VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 64 | const PreComputation& preComp) const override; 65 | 66 | private: 67 | NormalVelocityConstraintCppAd(const NormalVelocityConstraintCppAd& rhs); 68 | 69 | const SwitchedModelReferenceManager* referenceManagerPtr_; 70 | std::unique_ptr eeLinearConstraintPtr_; 71 | const size_t contactPointIndex_; 72 | }; 73 | 74 | } // namespace g1 75 | } // namespace ocs2 76 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/ZeroForceConstraint.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | class ZeroForceConstraint final : public StateInputConstraint { 41 | public: 42 | /* 43 | * Constructor 44 | * @param [in] referenceManager : Switched model ReferenceManager. 45 | * @param [in] contactPointIndex : The 3 DoF contact index. 46 | * @param [in] info : The centroidal model information. 47 | */ 48 | ZeroForceConstraint(const SwitchedModelReferenceManager& referenceManager, size_t contactPointIndex, CentroidalModelInfo info); 49 | 50 | ~ZeroForceConstraint() override = default; 51 | ZeroForceConstraint* clone() const override { return new ZeroForceConstraint(*this); } 52 | 53 | bool isActive(scalar_t time) const override; 54 | size_t getNumConstraints(scalar_t time) const override { return 3; } 55 | vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; 56 | VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 57 | const PreComputation& preComp) const override; 58 | 59 | private: 60 | ZeroForceConstraint(const ZeroForceConstraint& other) = default; 61 | 62 | const SwitchedModelReferenceManager* referenceManagerPtr_; 63 | const size_t contactPointIndex_; 64 | const CentroidalModelInfo info_; 65 | }; 66 | 67 | } // namespace g1 68 | } // namespace ocs2 69 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/constraint/ZeroVelocityConstraintCppAd.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include "g1_interface/constraint/EndEffectorLinearConstraint.h" 35 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | /** 41 | * Specializes the CppAd version of zero velocity constraint on an end-effector position and linear velocity. 42 | * Constructs the member EndEffectorLinearConstraint object with number of constraints of 3. 43 | * 44 | * See also EndEffectorLinearConstraint for the underlying computation. 45 | */ 46 | class ZeroVelocityConstraintCppAd final : public StateInputConstraint { 47 | public: 48 | /** 49 | * Constructor 50 | * @param [in] referenceManager : Switched model ReferenceManager 51 | * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. 52 | * @param [in] contactPointIndex : The 3 DoF contact index. 53 | * @param [in] config: The constraint coefficients 54 | */ 55 | ZeroVelocityConstraintCppAd(const SwitchedModelReferenceManager& referenceManager, 56 | const EndEffectorKinematics& endEffectorKinematics, size_t contactPointIndex, 57 | EndEffectorLinearConstraint::Config config = EndEffectorLinearConstraint::Config()); 58 | 59 | ~ZeroVelocityConstraintCppAd() override = default; 60 | ZeroVelocityConstraintCppAd* clone() const override { return new ZeroVelocityConstraintCppAd(*this); } 61 | 62 | bool isActive(scalar_t time) const override; 63 | size_t getNumConstraints(scalar_t time) const override { return 3; } 64 | vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; 65 | VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 66 | const PreComputation& preComp) const override; 67 | 68 | private: 69 | ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd& rhs); 70 | 71 | const SwitchedModelReferenceManager* referenceManagerPtr_; 72 | std::unique_ptr eeLinearConstraintPtr_; 73 | const size_t contactPointIndex_; 74 | }; 75 | 76 | } // namespace g1 77 | } // namespace ocs2 78 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/cost/g1QuadraticTrackingCost.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "g1_interface/common/utils.h" 37 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 38 | 39 | namespace ocs2 { 40 | namespace g1 { 41 | 42 | /** 43 | * State-input tracking cost used for intermediate times 44 | */ 45 | class g1StateInputQuadraticCost final : public QuadraticStateInputCost { 46 | public: 47 | g1StateInputQuadraticCost(matrix_t Q, matrix_t R, CentroidalModelInfo info, 48 | const SwitchedModelReferenceManager& referenceManager) 49 | : QuadraticStateInputCost(std::move(Q), std::move(R)), info_(std::move(info)), referenceManagerPtr_(&referenceManager) {} 50 | 51 | ~g1StateInputQuadraticCost() override = default; 52 | g1StateInputQuadraticCost* clone() const override { return new g1StateInputQuadraticCost(*this); } 53 | 54 | private: 55 | g1StateInputQuadraticCost(const g1StateInputQuadraticCost& rhs) = default; 56 | 57 | std::pair getStateInputDeviation(scalar_t time, const vector_t& state, const vector_t& input, 58 | const TargetTrajectories& targetTrajectories) const override { 59 | const auto contactFlags = referenceManagerPtr_->getContactFlags(time); 60 | const vector_t xNominal = targetTrajectories.getDesiredState(time); 61 | const vector_t uNominal = weightCompensatingInput(info_, contactFlags); 62 | return {state - xNominal, input - uNominal}; 63 | } 64 | 65 | const CentroidalModelInfo info_; 66 | const SwitchedModelReferenceManager* referenceManagerPtr_; 67 | }; 68 | 69 | /** 70 | * State tracking cost used for the final time 71 | */ 72 | class g1StateQuadraticCost final : public QuadraticStateCost { 73 | public: 74 | g1StateQuadraticCost(matrix_t Q, CentroidalModelInfo info, const SwitchedModelReferenceManager& referenceManager) 75 | : QuadraticStateCost(std::move(Q)), info_(std::move(info)), referenceManagerPtr_(&referenceManager) {} 76 | 77 | ~g1StateQuadraticCost() override = default; 78 | g1StateQuadraticCost* clone() const override { return new g1StateQuadraticCost(*this); } 79 | 80 | private: 81 | g1StateQuadraticCost(const g1StateQuadraticCost& rhs) = default; 82 | 83 | vector_t getStateDeviation(scalar_t time, const vector_t& state, const TargetTrajectories& targetTrajectories) const override { 84 | const auto contactFlags = referenceManagerPtr_->getContactFlags(time); 85 | const vector_t xNominal = targetTrajectories.getDesiredState(time); 86 | return state - xNominal; 87 | } 88 | 89 | const CentroidalModelInfo info_; 90 | const SwitchedModelReferenceManager* referenceManagerPtr_; 91 | }; 92 | 93 | } // namespace g1 94 | } // namespace ocs2 95 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/dynamics/g1DynamicsAD.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include "g1_interface/common/ModelSettings.h" 38 | 39 | namespace ocs2 { 40 | namespace g1 { 41 | 42 | class g1DynamicsAD final : public SystemDynamicsBase { 43 | public: 44 | g1DynamicsAD(const PinocchioInterface& pinocchioInterface, const CentroidalModelInfo& info, const std::string& modelName, 45 | const ModelSettings& modelSettings); 46 | 47 | ~g1DynamicsAD() override = default; 48 | g1DynamicsAD* clone() const override { return new g1DynamicsAD(*this); } 49 | 50 | vector_t computeFlowMap(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) override; 51 | VectorFunctionLinearApproximation linearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 52 | const PreComputation& preComp) override; 53 | 54 | private: 55 | g1DynamicsAD(const g1DynamicsAD& rhs) = default; 56 | 57 | PinocchioCentroidalDynamicsAD pinocchioCentroidalDynamicsAd_; 58 | }; 59 | 60 | } // namespace g1 61 | } // namespace ocs2 62 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/foot_planner/CubicSpline.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | namespace ocs2 { 35 | namespace g1 { 36 | 37 | class CubicSpline { 38 | public: 39 | struct Node { 40 | scalar_t time; 41 | scalar_t position; 42 | scalar_t velocity; 43 | }; 44 | 45 | CubicSpline(Node start, Node end); 46 | 47 | scalar_t position(scalar_t time) const; 48 | 49 | scalar_t velocity(scalar_t time) const; 50 | 51 | scalar_t acceleration(scalar_t time) const; 52 | 53 | scalar_t startTimeDerivative(scalar_t t) const; 54 | 55 | scalar_t finalTimeDerivative(scalar_t t) const; 56 | 57 | private: 58 | scalar_t normalizedTime(scalar_t t) const; 59 | 60 | scalar_t t0_; 61 | scalar_t t1_; 62 | scalar_t dt_; 63 | 64 | scalar_t c0_; 65 | scalar_t c1_; 66 | scalar_t c2_; 67 | scalar_t c3_; 68 | 69 | scalar_t dc0_; // derivative w.r.t. dt_ 70 | scalar_t dc1_; // derivative w.r.t. dt_ 71 | scalar_t dc2_; // derivative w.r.t. dt_ 72 | scalar_t dc3_; // derivative w.r.t. dt_ 73 | }; 74 | 75 | } // namespace g1 76 | } // namespace ocs2 77 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/foot_planner/SplineCpg.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include "g1_interface/foot_planner/CubicSpline.h" 33 | 34 | namespace ocs2 { 35 | namespace g1 { 36 | 37 | class SplineCpg { 38 | public: 39 | SplineCpg(CubicSpline::Node liftOff, scalar_t midHeight, CubicSpline::Node touchDown); 40 | 41 | scalar_t position(scalar_t time) const; 42 | 43 | scalar_t velocity(scalar_t time) const; 44 | 45 | scalar_t acceleration(scalar_t time) const; 46 | 47 | scalar_t startTimeDerivative(scalar_t time) const; 48 | 49 | scalar_t finalTimeDerivative(scalar_t time) const; 50 | 51 | private: 52 | scalar_t midTime_; 53 | CubicSpline leftSpline_; 54 | CubicSpline rightSpline_; 55 | }; 56 | 57 | } // namespace g1 58 | } // namespace ocs2 59 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/g1PreComputation.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2020, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include "g1_interface/common/ModelSettings.h" 41 | #include "g1_interface/constraint/EndEffectorLinearConstraint.h" 42 | #include "g1_interface/foot_planner/SwingTrajectoryPlanner.h" 43 | 44 | namespace ocs2 { 45 | namespace g1 { 46 | 47 | /** Callback for caching and reference update */ 48 | class g1PreComputation : public PreComputation { 49 | public: 50 | g1PreComputation(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, 51 | const SwingTrajectoryPlanner& swingTrajectoryPlanner, ModelSettings settings); 52 | ~g1PreComputation() override = default; 53 | 54 | g1PreComputation* clone() const override; 55 | 56 | void request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u) override; 57 | 58 | const std::vector& getEeNormalVelocityConstraintConfigs() const { return eeNormalVelConConfigs_; } 59 | 60 | PinocchioInterface& getPinocchioInterface() { return pinocchioInterface_; } 61 | const PinocchioInterface& getPinocchioInterface() const { return pinocchioInterface_; } 62 | 63 | private: 64 | g1PreComputation(const g1PreComputation& other) = default; 65 | 66 | PinocchioInterface pinocchioInterface_; 67 | CentroidalModelInfo info_; 68 | const SwingTrajectoryPlanner* swingTrajectoryPlannerPtr_; 69 | const ModelSettings settings_; 70 | 71 | std::vector eeNormalVelConConfigs_; 72 | }; 73 | 74 | } // namespace g1 75 | } // namespace ocs2 76 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/gait/Gait.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | /** 41 | * A gait is a periodic mode schedule parameterized by a "phase" variable. 42 | * 43 | * The eventPhases only indicate switches of modes, i.e. phase = 0 and phase = 1 are not part of the eventPhases. 44 | * The number of modes is therefore number of phases + 1 45 | * 46 | * The conversion to time is regulated by a duration 47 | */ 48 | struct Gait { 49 | /** time for one gait cycle*/ 50 | scalar_t duration; 51 | /** points in (0.0, 1.0) along the gait cycle where the contact mode changes, size N-1 */ 52 | std::vector eventPhases; 53 | /** sequence of contact modes, size: N */ 54 | std::vector modeSequence; 55 | }; 56 | 57 | /** 58 | * isValidGait checks the following properties 59 | * - positive duration 60 | * - eventPhases are all in (0.0, 1.0) 61 | * - eventPhases are sorted 62 | * - the size of the modeSequences is 1 more than the eventPhases. 63 | */ 64 | bool isValidGait(const Gait& gait); 65 | 66 | /** Check is if the phase is in [0.0, 1.0) */ 67 | bool isValidPhase(scalar_t phase); 68 | 69 | /** Wraps a phase to [0.0, 1.0) */ 70 | scalar_t wrapPhase(scalar_t phase); 71 | 72 | /** The modes are selected with a closed-open interval: [ ) */ 73 | int getModeIndexFromPhase(scalar_t phase, const Gait& gait); 74 | 75 | /** Gets the active mode from the phase variable */ 76 | size_t getModeFromPhase(scalar_t phase, const Gait& gait); 77 | 78 | /** Returns the time left in the gait based on the phase variable */ 79 | scalar_t timeLeftInGait(scalar_t phase, const Gait& gait); 80 | 81 | /** Returns the time left in the current based on the phase variable */ 82 | scalar_t timeLeftInMode(scalar_t phase, const Gait& gait); 83 | 84 | /** Print gait */ 85 | std::ostream& operator<<(std::ostream& stream, const Gait& gait); 86 | 87 | } // namespace g1 88 | } // namespace ocs2 89 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/gait/GaitSchedule.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include "g1_interface/gait/ModeSequenceTemplate.h" 38 | 39 | namespace ocs2 { 40 | namespace g1 { 41 | 42 | class GaitSchedule { 43 | public: 44 | GaitSchedule(ModeSchedule initModeSchedule, ModeSequenceTemplate initModeSequenceTemplate, scalar_t phaseTransitionStanceTime); 45 | 46 | /** 47 | * Sets the mode schedule. 48 | * 49 | * @param [in] modeSchedule: The mode schedule to be used. 50 | */ 51 | void setModeSchedule(const ModeSchedule& modeSchedule) { modeSchedule_ = modeSchedule; } 52 | 53 | /** 54 | * Gets the mode schedule. 55 | * 56 | * @param [in] lowerBoundTime: The smallest time for which the ModeSchedule should be defined. 57 | * @param [in] upperBoundTime: The greatest time for which the ModeSchedule should be defined. 58 | */ 59 | ModeSchedule getModeSchedule(scalar_t lowerBoundTime, scalar_t upperBoundTime); 60 | 61 | /** 62 | * Used to insert a new user defined logic in the given time period. 63 | * 64 | * @param [in] startTime: The initial time from which the new mode sequence template should start. 65 | * @param [in] finalTime: The final time until when the new mode sequence needs to be defined. 66 | */ 67 | void insertModeSequenceTemplate(const ModeSequenceTemplate& modeSequenceTemplate, scalar_t startTime, scalar_t finalTime); 68 | 69 | private: 70 | /** 71 | * Extends the switch information from lowerBoundTime to upperBoundTime based on the template mode sequence. 72 | * 73 | * @param [in] startTime: The initial time from which the mode schedule should be appended with the template. 74 | * @param [in] finalTime: The final time to which the mode schedule should be appended with the template. 75 | */ 76 | void tileModeSequenceTemplate(scalar_t startTime, scalar_t finalTime); 77 | 78 | private: 79 | ModeSchedule modeSchedule_; 80 | ModeSequenceTemplate modeSequenceTemplate_; 81 | scalar_t phaseTransitionStanceTime_; 82 | }; 83 | 84 | } // namespace g1 85 | } // namespace ocs2 86 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/initialization/g1Initializer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | class g1Initializer final : public Initializer { 41 | public: 42 | /* 43 | * Constructor 44 | * @param [in] info : The centroidal model information. 45 | * @param [in] referenceManager : Switched system reference manager. 46 | * @param [in] extendNormalizedMomentum: If true, it extrapolates the normalized momenta; otherwise sets them to zero. 47 | */ 48 | g1Initializer(CentroidalModelInfo info, const SwitchedModelReferenceManager& referenceManager, 49 | bool extendNormalizedMomentum = false); 50 | 51 | ~g1Initializer() override = default; 52 | g1Initializer* clone() const override; 53 | 54 | void compute(scalar_t time, const vector_t& state, scalar_t nextTime, vector_t& input, vector_t& nextState) override; 55 | 56 | private: 57 | g1Initializer(const g1Initializer& other) = default; 58 | 59 | const CentroidalModelInfo info_; 60 | const SwitchedModelReferenceManager* referenceManagerPtr_; 61 | const bool extendNormalizedMomentum_; 62 | }; 63 | 64 | } // namespace g1 65 | } // namespace ocs2 66 | -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/package_path.h.in: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | namespace ocs2 { 36 | namespace g1 { 37 | 38 | /** Gets the path to the package source directory. */ 39 | inline std::string getPath() { 40 | return "@PROJECT_SOURCE_DIR@"; 41 | } 42 | 43 | } // namespace g1 44 | } // namespace ocs2 -------------------------------------------------------------------------------- /g1_interface/include/g1_interface/reference_manager/SwitchedModelReferenceManager.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | #include 34 | 35 | #include "g1_interface/foot_planner/SwingTrajectoryPlanner.h" 36 | #include "g1_interface/gait/GaitSchedule.h" 37 | #include "g1_interface/gait/MotionPhaseDefinition.h" 38 | 39 | namespace ocs2 { 40 | namespace g1 { 41 | 42 | /** 43 | * Manages the ModeSchedule and the TargetTrajectories for switched model. 44 | */ 45 | class SwitchedModelReferenceManager : public ReferenceManager { 46 | public: 47 | SwitchedModelReferenceManager(std::shared_ptr gaitSchedulePtr, std::shared_ptr swingTrajectoryPtr); 48 | 49 | ~SwitchedModelReferenceManager() override = default; 50 | 51 | void setModeSchedule(const ModeSchedule& modeSchedule) override; 52 | 53 | contact_flag_t getContactFlags(scalar_t time) const; 54 | 55 | const std::shared_ptr& getGaitSchedule() { return gaitSchedulePtr_; } 56 | 57 | const std::shared_ptr& getSwingTrajectoryPlanner() { return swingTrajectoryPtr_; } 58 | 59 | private: 60 | void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t& initState, TargetTrajectories& targetTrajectories, 61 | ModeSchedule& modeSchedule) override; 62 | 63 | std::shared_ptr gaitSchedulePtr_; 64 | std::shared_ptr swingTrajectoryPtr_; 65 | }; 66 | 67 | } // namespace g1 68 | } // namespace ocs2 69 | -------------------------------------------------------------------------------- /g1_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_interface 4 | 0.0.1 5 | The g1_interface package 6 | 7 | Jialong Xue 8 | 9 | BSD-3 10 | 11 | ament_cmake 12 | 13 | ocs2_core 14 | ocs2_oc 15 | ocs2_ddp 16 | ocs2_mpc 17 | ocs2_sqp 18 | ocs2_ipm 19 | ocs2_robotic_assets 20 | ocs2_robotic_tools 21 | ocs2_pinocchio_interface 22 | ocs2_centroidal_model 23 | ocs2_self_collision 24 | pinocchio 25 | 26 | ament_cmake_gtest 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /g1_interface/src/common/ModelSettings.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_interface/common/ModelSettings.h" 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace ocs2 { 38 | namespace g1 { 39 | 40 | ModelSettings loadModelSettings(const std::string& filename, const std::string& fieldName, bool verbose) { 41 | ModelSettings modelSettings; 42 | 43 | boost::property_tree::ptree pt; 44 | boost::property_tree::read_info(filename, pt); 45 | 46 | if (verbose) { 47 | std::cerr << "\n #### Legged Robot Model Settings:"; 48 | std::cerr << "\n #### =============================================================================\n"; 49 | } 50 | 51 | loadData::loadPtreeValue(pt, modelSettings.positionErrorGain, fieldName + ".positionErrorGain", verbose); 52 | 53 | loadData::loadPtreeValue(pt, modelSettings.phaseTransitionStanceTime, fieldName + ".phaseTransitionStanceTime", verbose); 54 | loadData::loadPtreeValue(pt, modelSettings.verboseCppAd, fieldName + ".verboseCppAd", verbose); 55 | loadData::loadPtreeValue(pt, modelSettings.recompileLibrariesCppAd, fieldName + ".recompileLibrariesCppAd", verbose); 56 | loadData::loadPtreeValue(pt, modelSettings.modelFolderCppAd, fieldName + ".modelFolderCppAd", verbose); 57 | 58 | if (verbose) { 59 | std::cerr << " #### =============================================================================" << std::endl; 60 | } 61 | return modelSettings; 62 | } 63 | 64 | } // namespace g1 65 | } // namespace ocs2 66 | -------------------------------------------------------------------------------- /g1_interface/src/constraint/FootRollContraint.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by pocket on 24-2-4. 3 | // 4 | #include "g1_interface/constraint/FootRollConstraint.h" 5 | 6 | #include 7 | 8 | namespace ocs2 { 9 | namespace g1 { 10 | FootRollConstraint::FootRollConstraint(const SwitchedModelReferenceManager& referenceManager, size_t contactPointIndex,CentroidalModelInfo info) 11 | : StateInputConstraint(ConstraintOrder::Linear), 12 | referenceManagerPtr_(&referenceManager), 13 | contactPointIndex_(contactPointIndex), 14 | info_(std::move(info)) {} 15 | 16 | FootRollConstraint::FootRollConstraint(const FootRollConstraint& rhs) 17 | : StateInputConstraint(rhs), 18 | referenceManagerPtr_(rhs.referenceManagerPtr_), 19 | contactPointIndex_(rhs.contactPointIndex_), 20 | info_(rhs.info_) {} 21 | 22 | bool FootRollConstraint::isActive(scalar_t time) const { 23 | // return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; 24 | return true; 25 | } 26 | 27 | 28 | vector_t FootRollConstraint::getValue(scalar_t time, const vector_t& state, const vector_t& input, 29 | const PreComputation& preComp) const { 30 | //获取最后一个关节的速度 31 | long index = 5 + (contactPointIndex_) * 6; 32 | return centroidal_model::getJointVelocities(input, info_).block<-1, 1>(index, 0, 1, 1); 33 | } 34 | 35 | 36 | VectorFunctionLinearApproximation FootRollConstraint::getLinearApproximation(scalar_t time, const vector_t& state, 37 | const vector_t& input, 38 | const PreComputation& preComp) const { 39 | VectorFunctionLinearApproximation approx; 40 | approx.f = getValue(time, state, input, preComp); 41 | approx.dfdx = matrix_t::Zero(1, state.size()); 42 | approx.dfdu = matrix_t::Zero(1, input.size()); 43 | approx.dfdu.middleCols<1>(17 + (contactPointIndex_ ) * 6).diagonal() = vector_t::Ones(1); 44 | return approx; 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /g1_interface/src/constraint/ZeroForceConstraint.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_interface/constraint/ZeroForceConstraint.h" 31 | 32 | #include 33 | 34 | namespace ocs2 { 35 | namespace g1 { 36 | 37 | /******************************************************************************************************/ 38 | /******************************************************************************************************/ 39 | /******************************************************************************************************/ 40 | ZeroForceConstraint::ZeroForceConstraint(const SwitchedModelReferenceManager& referenceManager, size_t contactPointIndex, 41 | CentroidalModelInfo info) 42 | : StateInputConstraint(ConstraintOrder::Linear), 43 | referenceManagerPtr_(&referenceManager), 44 | contactPointIndex_(contactPointIndex), 45 | info_(std::move(info)) {} 46 | 47 | /******************************************************************************************************/ 48 | /******************************************************************************************************/ 49 | /******************************************************************************************************/ 50 | bool ZeroForceConstraint::isActive(scalar_t time) const { 51 | return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; 52 | } 53 | 54 | /******************************************************************************************************/ 55 | /******************************************************************************************************/ 56 | /******************************************************************************************************/ 57 | vector_t ZeroForceConstraint::getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const { 58 | return centroidal_model::getContactForces(input, contactPointIndex_, info_); 59 | } 60 | 61 | /******************************************************************************************************/ 62 | /******************************************************************************************************/ 63 | /******************************************************************************************************/ 64 | VectorFunctionLinearApproximation ZeroForceConstraint::getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 65 | const PreComputation& preComp) const { 66 | VectorFunctionLinearApproximation approx; 67 | approx.f = getValue(time, state, input, preComp); 68 | approx.dfdx = matrix_t::Zero(3, state.size()); 69 | approx.dfdu = matrix_t::Zero(3, input.size()); 70 | approx.dfdu.middleCols<3>(3 * contactPointIndex_).diagonal() = vector_t::Ones(3); 71 | return approx; 72 | } 73 | 74 | } // namespace g1 75 | } // namespace ocs2 76 | -------------------------------------------------------------------------------- /g1_interface/src/dynamics/g1DynamicsAD.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_interface/dynamics/g1DynamicsAD.h" 31 | 32 | namespace ocs2 { 33 | namespace g1 { 34 | 35 | /******************************************************************************************************/ 36 | /******************************************************************************************************/ 37 | /******************************************************************************************************/ 38 | g1DynamicsAD::g1DynamicsAD(const PinocchioInterface& pinocchioInterface, const CentroidalModelInfo& info, 39 | const std::string& modelName, const ModelSettings& modelSettings) 40 | : pinocchioCentroidalDynamicsAd_(pinocchioInterface, info, modelName, modelSettings.modelFolderCppAd, 41 | modelSettings.recompileLibrariesCppAd, modelSettings.verboseCppAd) {} 42 | 43 | /******************************************************************************************************/ 44 | /******************************************************************************************************/ 45 | /******************************************************************************************************/ 46 | vector_t g1DynamicsAD::computeFlowMap(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) { 47 | return pinocchioCentroidalDynamicsAd_.getValue(time, state, input); 48 | } 49 | 50 | /******************************************************************************************************/ 51 | /******************************************************************************************************/ 52 | /******************************************************************************************************/ 53 | VectorFunctionLinearApproximation g1DynamicsAD::linearApproximation(scalar_t time, const vector_t& state, const vector_t& input, 54 | const PreComputation& preComp) { 55 | return pinocchioCentroidalDynamicsAd_.getLinearApproximation(time, state, input); 56 | } 57 | 58 | } // namespace g1 59 | } // namespace ocs2 60 | -------------------------------------------------------------------------------- /g1_interface/src/g1PreComputation.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2020, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | namespace ocs2 { 41 | namespace g1 { 42 | 43 | /******************************************************************************************************/ 44 | /******************************************************************************************************/ 45 | /******************************************************************************************************/ 46 | g1PreComputation::g1PreComputation(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, 47 | const SwingTrajectoryPlanner& swingTrajectoryPlanner, ModelSettings settings) 48 | : pinocchioInterface_(std::move(pinocchioInterface)), 49 | info_(std::move(info)), 50 | swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner), 51 | settings_(std::move(settings)) { 52 | eeNormalVelConConfigs_.resize(info_.numThreeDofContacts); 53 | } 54 | 55 | /******************************************************************************************************/ 56 | /******************************************************************************************************/ 57 | /******************************************************************************************************/ 58 | g1PreComputation* g1PreComputation::clone() const { 59 | return new g1PreComputation(*this); 60 | } 61 | 62 | /******************************************************************************************************/ 63 | /******************************************************************************************************/ 64 | /******************************************************************************************************/ 65 | void g1PreComputation::request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u) { 66 | if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint)) { 67 | return; 68 | } 69 | 70 | // lambda to set config for normal velocity constraints 71 | auto eeNormalVelConConfig = [&](size_t footIndex) { 72 | EndEffectorLinearConstraint::Config config; 73 | config.b = (vector_t(1) << -swingTrajectoryPlannerPtr_->getZvelocityConstraint(footIndex, t)).finished(); 74 | config.Av = (matrix_t(1, 3) << 0.0, 0.0, 1.0).finished(); 75 | if (!numerics::almost_eq(settings_.positionErrorGain, 0.0)) { 76 | config.b(0) -= settings_.positionErrorGain * swingTrajectoryPlannerPtr_->getZpositionConstraint(footIndex, t); 77 | config.Ax = (matrix_t(1, 3) << 0.0, 0.0, settings_.positionErrorGain).finished(); 78 | } 79 | return config; 80 | }; 81 | 82 | if (request.contains(Request::Constraint)) { 83 | for (size_t i = 0; i < info_.numThreeDofContacts; i++) { 84 | eeNormalVelConConfigs_[i] = eeNormalVelConConfig(i); 85 | } 86 | } 87 | } 88 | 89 | } // namespace g1 90 | } // namespace ocs2 91 | -------------------------------------------------------------------------------- /g1_interface/src/initialization/g1Initializer.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_interface/initialization/g1Initializer.h" 31 | 32 | #include "g1_interface/common/utils.h" 33 | 34 | #include 35 | 36 | namespace ocs2 { 37 | namespace g1 { 38 | 39 | /******************************************************************************************************/ 40 | /******************************************************************************************************/ 41 | /******************************************************************************************************/ 42 | g1Initializer::g1Initializer(CentroidalModelInfo info, const SwitchedModelReferenceManager& referenceManager, 43 | bool extendNormalizedMomentum) 44 | : info_(std::move(info)), referenceManagerPtr_(&referenceManager), extendNormalizedMomentum_(extendNormalizedMomentum) {} 45 | 46 | /******************************************************************************************************/ 47 | /******************************************************************************************************/ 48 | /******************************************************************************************************/ 49 | g1Initializer* g1Initializer::clone() const { 50 | return new g1Initializer(*this); 51 | } 52 | 53 | /******************************************************************************************************/ 54 | /******************************************************************************************************/ 55 | /******************************************************************************************************/ 56 | void g1Initializer::compute(scalar_t time, const vector_t& state, scalar_t nextTime, vector_t& input, vector_t& nextState) { 57 | const auto contactFlags = referenceManagerPtr_->getContactFlags(time); 58 | input = weightCompensatingInput(info_, contactFlags); 59 | nextState = state; 60 | if (!extendNormalizedMomentum_) { 61 | centroidal_model::getNormalizedMomentum(nextState, info_).setZero(); 62 | } 63 | } 64 | 65 | } // namespace g1 66 | } // namespace ocs2 67 | -------------------------------------------------------------------------------- /g1_interface/src/reference_manager/SwitchedModelReferenceManager.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 31 | 32 | namespace ocs2 { 33 | namespace g1 { 34 | 35 | /******************************************************************************************************/ 36 | /******************************************************************************************************/ 37 | /******************************************************************************************************/ 38 | SwitchedModelReferenceManager::SwitchedModelReferenceManager(std::shared_ptr gaitSchedulePtr, 39 | std::shared_ptr swingTrajectoryPtr) 40 | : ReferenceManager(TargetTrajectories(), ModeSchedule()), 41 | gaitSchedulePtr_(std::move(gaitSchedulePtr)), 42 | swingTrajectoryPtr_(std::move(swingTrajectoryPtr)) {} 43 | 44 | /******************************************************************************************************/ 45 | /******************************************************************************************************/ 46 | /******************************************************************************************************/ 47 | void SwitchedModelReferenceManager::setModeSchedule(const ModeSchedule& modeSchedule) { 48 | ReferenceManager::setModeSchedule(modeSchedule); 49 | gaitSchedulePtr_->setModeSchedule(modeSchedule); 50 | } 51 | 52 | /******************************************************************************************************/ 53 | /******************************************************************************************************/ 54 | /******************************************************************************************************/ 55 | contact_flag_t SwitchedModelReferenceManager::getContactFlags(scalar_t time) const { 56 | return modeNumber2StanceLeg(this->getModeSchedule().modeAtTime(time)); 57 | } 58 | 59 | /******************************************************************************************************/ 60 | /******************************************************************************************************/ 61 | /******************************************************************************************************/ 62 | void SwitchedModelReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t& initState, 63 | TargetTrajectories& targetTrajectories, ModeSchedule& modeSchedule) { 64 | const auto timeHorizon = finalTime - initTime; 65 | modeSchedule = gaitSchedulePtr_->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon); 66 | 67 | const scalar_t terrainHeight = 0.0; 68 | swingTrajectoryPtr_->update(modeSchedule, terrainHeight); 69 | } 70 | 71 | } // namespace g1 72 | } // namespace ocs2 73 | -------------------------------------------------------------------------------- /g1_interface/test/include/g1_interface/test/AnymalFactoryFunctions.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Copyright (c) 2021, Farbod Farshidian. All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the copyright holder nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************/ 29 | 30 | #pragma once 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include "g1_interface/reference_manager/SwitchedModelReferenceManager.h" 38 | 39 | namespace ocs2 { 40 | namespace g1 { 41 | 42 | /** Returns a Pinocchio interface based on a defined URDF_FILE */ 43 | std::unique_ptr createAnymalPinocchioInterface(); 44 | 45 | /** Returns a Pinocchio interface based on a defined REFERENCE_FILE */ 46 | CentroidalModelInfo createAnymalCentroidalModelInfo(const PinocchioInterface& pinocchioInterface, CentroidalModelType centroidalType); 47 | 48 | /** Return a Switched model mode schedule manager based on TASK_FILE */ 49 | std::shared_ptr createReferenceManager(size_t numFeet); 50 | 51 | } // namespace g1 52 | } // namespace ocs2 53 | -------------------------------------------------------------------------------- /g1_legged_description/.gitignore: -------------------------------------------------------------------------------- 1 | /.vscode -------------------------------------------------------------------------------- /g1_legged_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | set(CMAKE_CXX_STANDARD 17) 3 | project(g1_legged_description) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | ## Find ament_cmake macros and libraries 9 | find_package(ament_cmake REQUIRED) 10 | 11 | #find_package(ocs2_switched_model_interface REQUIRED) 12 | #find_package(ocs2_pinocchio_interface REQUIRED) 13 | 14 | set(dependencies 15 | # ocs2_switched_model_interface 16 | # ocs2_pinocchio_interface 17 | ) 18 | 19 | ########### 20 | ## Build ## 21 | ########### 22 | 23 | # Resolve for the package path at compile time. 24 | #configure_file( 25 | # "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h.in" 26 | # "${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/package_path.h" @ONLY 27 | #) 28 | 29 | #add_library(${PROJECT_NAME} 30 | # src/AnymalModels.cpp 31 | # src/FrameDeclaration.cpp 32 | # src/QuadrupedPinocchioMapping.cpp 33 | # src/QuadrupedInverseKinematics.cpp 34 | # src/QuadrupedKinematics.cpp 35 | # src/QuadrupedCom.cpp 36 | #) 37 | #target_include_directories(${PROJECT_NAME} 38 | # PUBLIC 39 | # "$" 40 | # "$") 41 | 42 | #ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 43 | #target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) 44 | 45 | ############# 46 | ## Install ## 47 | ############# 48 | 49 | #install( 50 | # TARGETS ${PROJECT_NAME} 51 | # EXPORT export_${PROJECT_NAME} 52 | # ARCHIVE DESTINATION lib 53 | # LIBRARY DESTINATION lib 54 | # RUNTIME DESTINATION bin 55 | #) 56 | 57 | #install( 58 | # DIRECTORY include/ 59 | # DESTINATION include/${PROJECT_NAME} 60 | #) 61 | 62 | install( 63 | DIRECTORY config launch urdf rviz mjcf meshes 64 | DESTINATION share/${PROJECT_NAME}/ 65 | ) 66 | 67 | ament_export_dependencies(${dependencies}) 68 | #ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 69 | 70 | ############# 71 | ## Testing ## 72 | ############# 73 | 74 | 75 | if (BUILD_TESTING) 76 | 77 | 78 | 79 | endif () 80 | ament_package() 81 | -------------------------------------------------------------------------------- /g1_legged_description/config/joint_names_g1_legged.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['', 'left_hip_pitch_joint', 'left_hip_roll_joint', 'left_hip_yaw_joint', 'left_knee_joint', 'left_ankle_pitch_joint', 'left_ankle_roll_joint','right_hip_pitch_joint', 'right_hip_roll_joint', 'right_hip_yaw_joint', 'right_knee_joint', 'right_ankle_pitch_joint', 'right_ankle_roll_joint', ] 2 | -------------------------------------------------------------------------------- /g1_legged_description/export.log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/export.log -------------------------------------------------------------------------------- /g1_legged_description/launch/visualize.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription, LaunchContext 2 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | from ament_index_python.packages import get_package_share_directory 6 | 7 | 8 | def get_urdf_file(robot_name): 9 | 10 | urdf_file = get_package_share_directory('g1_legged_description') + "/urdf/g1_legged_control_.urdf" 11 | return urdf_file 12 | 13 | 14 | def launch_setup(context, *args, **kwargs): 15 | rviz_config_file = get_package_share_directory('g1_legged_description') + "/rviz/g1_legged_control.rviz" 16 | 17 | robot_name = LaunchConfiguration('robot_name').perform(context) 18 | print("Visualizing robot: " + robot_name) 19 | urdf_file = get_urdf_file(robot_name) 20 | 21 | return [ 22 | Node( 23 | package='robot_state_publisher', 24 | executable='robot_state_publisher', 25 | name='robot_state_publisher', 26 | output='screen', 27 | parameters=[ 28 | { 29 | 'publish_frequency': 100.0, 30 | 'use_tf_static': True, 31 | }, 32 | ], 33 | arguments=[urdf_file], 34 | ), 35 | Node( 36 | package='joint_state_publisher_gui', 37 | executable='joint_state_publisher_gui', 38 | name='joint_state_publisher', 39 | output='screen', 40 | parameters=[ 41 | { 42 | 'use_gui': True, 43 | 'rate': 100.0 44 | }, 45 | ] 46 | ), 47 | Node( 48 | package='rviz2', 49 | executable='rviz2', 50 | name='rviz_ocs2', 51 | output='screen', 52 | arguments=["-d", rviz_config_file] 53 | ) 54 | ] 55 | 56 | 57 | def generate_launch_description(): 58 | return LaunchDescription([ 59 | DeclareLaunchArgument( 60 | name='robot_name', 61 | default_value='g1_legged_description', 62 | description='Name of the robot to visualize' 63 | ), 64 | OpaqueFunction(function=launch_setup) 65 | ]) 66 | -------------------------------------------------------------------------------- /g1_legged_description/launch/visualize.launch_.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription, LaunchContext 2 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | from ament_index_python.packages import get_package_share_directory 6 | 7 | 8 | def get_urdf_file(robot_name): 9 | 10 | urdf_file = get_package_share_directory('g1_legged_description') + "/urdf/g1_legged_control_.urdf" 11 | return urdf_file 12 | 13 | 14 | def launch_setup(context, *args, **kwargs): 15 | rviz_config_file = get_package_share_directory('g1_legged_description') + "/rviz/g1_legged_control.rviz" 16 | 17 | robot_name = LaunchConfiguration('robot_name').perform(context) 18 | print("Visualizing robot: " + robot_name) 19 | urdf_file = get_urdf_file(robot_name) 20 | 21 | return [ 22 | Node( 23 | package='robot_state_publisher', 24 | executable='robot_state_publisher', 25 | name='robot_state_publisher', 26 | output='screen', 27 | parameters=[ 28 | { 29 | 'publish_frequency': 100.0, 30 | 'use_tf_static': True, 31 | }, 32 | ], 33 | arguments=[urdf_file], 34 | ), 35 | Node( 36 | package='joint_state_publisher_gui', 37 | executable='joint_state_publisher_gui', 38 | name='joint_state_publisher', 39 | output='screen', 40 | parameters=[ 41 | { 42 | 'use_gui': True, 43 | 'rate': 100.0 44 | }, 45 | ] 46 | ), 47 | Node( 48 | package='rviz2', 49 | executable='rviz2', 50 | name='rviz_ocs2', 51 | output='screen', 52 | arguments=["-d", rviz_config_file] 53 | ) 54 | ] 55 | 56 | 57 | def generate_launch_description(): 58 | return LaunchDescription([ 59 | DeclareLaunchArgument( 60 | name='robot_name', 61 | default_value='g1_legged_description', 62 | description='Name of the robot to visualize' 63 | ), 64 | OpaqueFunction(function=launch_setup) 65 | ]) 66 | -------------------------------------------------------------------------------- /g1_legged_description/meshes/head_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/head_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_ankle_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_ankle_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_ankle_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_ankle_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_elbow_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_elbow_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_index_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_index_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_index_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_index_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_middle_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_middle_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_middle_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_middle_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_palm_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_palm_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_thumb_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_thumb_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_thumb_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_thumb_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hand_thumb_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hand_thumb_2_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hip_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hip_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hip_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hip_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_hip_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_hip_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_knee_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_knee_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_rubber_hand.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_rubber_hand.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_shoulder_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_shoulder_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_shoulder_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_shoulder_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_shoulder_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_shoulder_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_wrist_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_wrist_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_wrist_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_wrist_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_wrist_roll_rubber_hand.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_wrist_roll_rubber_hand.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/left_wrist_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/left_wrist_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/logo_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/logo_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/pelvis.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/pelvis.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/pelvis_contour_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/pelvis_contour_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_ankle_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_ankle_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_ankle_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_ankle_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_elbow_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_elbow_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_index_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_index_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_index_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_index_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_middle_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_middle_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_middle_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_middle_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_palm_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_palm_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_thumb_0_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_thumb_0_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_thumb_1_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_thumb_1_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hand_thumb_2_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hand_thumb_2_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hip_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hip_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hip_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hip_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_hip_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_hip_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_knee_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_knee_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_rubber_hand.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_rubber_hand.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_shoulder_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_shoulder_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_shoulder_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_shoulder_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_shoulder_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_shoulder_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_wrist_pitch_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_wrist_pitch_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_wrist_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_wrist_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_wrist_roll_rubber_hand.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_wrist_roll_rubber_hand.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/right_wrist_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/right_wrist_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_constraint_L_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_constraint_L_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_constraint_L_rod_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_constraint_L_rod_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_constraint_R_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_constraint_R_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_constraint_R_rod_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_constraint_R_rod_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_link_23dof_rev_1_0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_link_23dof_rev_1_0.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/torso_link_rev_1_0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/torso_link_rev_1_0.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_constraint_L.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_constraint_L.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_constraint_R.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_constraint_R.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_roll_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_roll_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_roll_link_rev_1_0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_roll_link_rev_1_0.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_support_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_support_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_yaw_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_yaw_link.STL -------------------------------------------------------------------------------- /g1_legged_description/meshes/waist_yaw_link_rev_1_0.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/meshes/waist_yaw_link_rev_1_0.STL -------------------------------------------------------------------------------- /g1_legged_description/mjcf/asset/block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_legged_description/mjcf/asset/block.png -------------------------------------------------------------------------------- /g1_legged_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_legged_description 4 | 0.0.1 5 | The g1_legged_description package 6 | 7 | Farbod Farshidian 8 | Jan Carius 9 | Ruben Grandia 10 | 11 | TODO 12 | 13 | ament_cmake 14 | 15 | 16 | joint_state_publisher_gui 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /g1_mujoco_sim/g1_mujoco_sim/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_mujoco_sim/g1_mujoco_sim/__init__.py -------------------------------------------------------------------------------- /g1_mujoco_sim/g1_mujoco_sim/joy.py: -------------------------------------------------------------------------------- 1 | #!/home/zou/miniconda3/bin/python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from sensor_msgs.msg import Joy 6 | from geometry_msgs.msg import Twist 7 | import threading 8 | from std_msgs.msg import Bool 9 | from std_msgs.msg import String 10 | from rclpy.time import Time, Duration 11 | vx = 0.0 12 | vy = 0.0 13 | w = 0.0 14 | key_Y_pressed = False 15 | pre_key_Y_press_status = False 16 | key_X_pressed = False 17 | pre_key_X_press_status = False 18 | 19 | def callback(data): 20 | global vx, vy, w, key_Y_pressed, key_X_pressed 21 | vx = data.axes[1] * 0.1 22 | vy = data.axes[0] * 0.02 23 | w = data.axes[3] * 0.3 24 | if data.buttons[3] == 1: 25 | key_Y_pressed = True 26 | else: 27 | key_Y_pressed = False 28 | if data.buttons[0] == 1: 29 | key_X_pressed = True 30 | else: 31 | key_X_pressed = False 32 | 33 | # ros init 34 | rclpy.init() 35 | node = Node('joy11') 36 | 37 | cmd_pub = node.create_publisher(Twist,'/cmd_vel',1) 38 | node.create_subscription(Joy,'joy',callback,10) 39 | 40 | publisher = node.create_publisher(Twist, '/cmd_vel',1) 41 | 42 | rate = node.create_rate(150) 43 | hw_switch_bool = False 44 | hw_switch_publisher = node.create_publisher(Bool, '/hwswitch', 1) 45 | 46 | gait_str = "walk" 47 | gait_str_publisher = node.create_publisher(String, '/desired_gait_str', 1) 48 | 49 | 50 | acc_linear_x = 0.5#人形的移动加速度 51 | acc_linear_y = 0.5#人形的移动加速度 52 | acc_angular_z = 0.4#人形的旋转角加速度 53 | 54 | 55 | #实际输入的是加速度的变化量而非加速度,其中该值为正值 56 | def change_limit(cmd_vel,vel_real,acc): 57 | if abs(cmd_vel-vel_real) > acc: 58 | if cmd_vel > vel_real: 59 | cmd_vel = vel_real + acc 60 | else: 61 | cmd_vel = vel_real - acc 62 | 63 | return cmd_vel 64 | 65 | 66 | 67 | 68 | def ros_publish(): 69 | global key_Y_pressed 70 | global pre_key_Y_press_status 71 | global hw_switch_bool 72 | global key_X_pressed 73 | global gait_str 74 | 75 | twist_msg_last = Twist()#记录了上一次发送的twist数值 76 | twist_msg_last.linear.x = 0.0 77 | twist_msg_last.linear.y = 0.0 78 | twist_msg_last.angular.z = 0.0 79 | while rclpy.ok(): 80 | if( hw_switch_bool == True): 81 | 82 | twist_msg = Twist() 83 | twist_msg.linear.x = vx 84 | twist_msg.linear.y = vy 85 | twist_msg.angular.z = w 86 | 87 | twist_msg.linear.x = change_limit(vx,twist_msg_last.linear.x,acc_linear_x/150.0) 88 | twist_msg.linear.y = change_limit(vy,twist_msg_last.linear.y,acc_linear_y/150.0) 89 | twist_msg.angular.z = change_limit(w,twist_msg_last.angular.z,acc_angular_z/150.0) 90 | 91 | publisher.publish(twist_msg) 92 | twist_msg_last = twist_msg#存储上一次发送的速度 93 | else: 94 | twist_msg = Twist() 95 | twist_msg.linear.x = 0.0 96 | twist_msg.linear.y = 0.0 97 | twist_msg.angular.z = 0.0 98 | 99 | publisher.publish(twist_msg) 100 | twist_msg_last = twist_msg#存储上一次发送的速度 101 | 102 | 103 | 104 | 105 | if key_Y_pressed and not pre_key_Y_press_status: 106 | hw_switch_bool = not hw_switch_bool 107 | print(f'\n Switch the output status to {hw_switch_bool}') 108 | pre_key_Y_press_status = key_Y_pressed 109 | 110 | hw_switch_msg = Bool() 111 | hw_switch_msg.data = hw_switch_bool 112 | hw_switch_publisher.publish(hw_switch_msg) 113 | 114 | if key_X_pressed and not pre_key_X_press_status: 115 | if gait_str == "trot": 116 | gait_str = "walk" 117 | elif gait_str == "walk": 118 | gait_str = "trot" 119 | print(f'\n Switch the desire gait to {gait_str}') 120 | pre_key_X_press_status = key_X_pressed 121 | 122 | gait_str_msg = String() 123 | gait_str_msg.data = gait_str 124 | gait_str_publisher.publish(gait_str_msg) 125 | 126 | rate.sleep() 127 | 128 | thread = threading.Thread(target=ros_publish) 129 | thread.start() 130 | 131 | rclpy.spin(node) 132 | rclpy.shutdown() 133 | -------------------------------------------------------------------------------- /g1_mujoco_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | g1_mujoco_sim 5 | 0.0.0 6 | TODO: Package description 7 | bth 8 | TODO: License declaration 9 | 10 | rclpy 11 | 12 | ament_copyright 13 | ament_flake8 14 | ament_pep257 15 | python3-pytest 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /g1_mujoco_sim/resource/g1_mujoco_sim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cyugai/g1_control/70b099f9ae238e94309e1c9cd5405a29414d041d/g1_mujoco_sim/resource/g1_mujoco_sim -------------------------------------------------------------------------------- /g1_mujoco_sim/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/g1_mujoco_sim 3 | [install] 4 | install_scripts=$base/lib/g1_mujoco_sim 5 | -------------------------------------------------------------------------------- /g1_mujoco_sim/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = 'g1_mujoco_sim' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | #packages=find_packages(exclude=['test']), 9 | packages=[package_name], 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', 12 | ['resource/' + package_name]), 13 | ('share/' + package_name, ['package.xml']), 14 | ], 15 | install_requires=['setuptools'], 16 | zip_safe=True, 17 | maintainer='bth', 18 | maintainer_email='bth@todo.todo', 19 | description='TODO: Package description', 20 | license='TODO: License declaration', 21 | extras_require={ 22 | 'test': ['pytest'],}, 23 | entry_points={ 24 | 'console_scripts': [ 25 | "g1_sim=g1_mujoco_sim.g1_sim:main", 26 | "joy=g1_mujoco_sim.joy:main", 27 | "teleop=g1_mujoco_sim.teleop:main", 28 | ], 29 | }, 30 | ) 31 | -------------------------------------------------------------------------------- /g1_mujoco_sim/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /g1_mujoco_sim/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /g1_mujoco_sim/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /g1_wbc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | set(CMAKE_CXX_STANDARD 17) 3 | project(g1_wbc) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 7 | 8 | set(dependencies 9 | g1_interface 10 | qpoases_catkin 11 | ) 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(g1_interface REQUIRED) 15 | find_package(qpoases_catkin REQUIRED) 16 | 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | # Resolve for the package path at compile time. 23 | 24 | 25 | set(FLAGS 26 | ${OCS2_CXX_FLAGS} 27 | ${pinocchio_CFLAGS_OTHER} 28 | -Wno-ignored-attributes 29 | -Wno-invalid-partial-specialization # to silence warning with unsupported Eigen Tensor 30 | -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR 31 | -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR 32 | ) 33 | 34 | # Legged robot interface library 35 | add_library(${PROJECT_NAME} 36 | src/HoQp.cpp 37 | src/WbcBase.cpp 38 | src/HierarchicalWbc.cpp 39 | src/WeightedWbc.cpp 40 | ) 41 | 42 | 43 | 44 | 45 | 46 | target_include_directories(${PROJECT_NAME} 47 | PUBLIC 48 | "$" 49 | "$" 50 | 51 | ) 52 | 53 | ament_target_dependencies(${PROJECT_NAME} ${dependencies}) 54 | target_compile_options(${PROJECT_NAME} PUBLIC ${FLAGS}) 55 | 56 | ######################### 57 | ### CLANG TOOLING ### 58 | ######################### 59 | 60 | find_package(cmake_clang_tools QUIET) 61 | if (cmake_clang_tools_FOUND) 62 | message(STATUS "Run clang tooling for target ocs2_legged_robot") 63 | add_clang_tooling( 64 | TARGETS ${PROJECT_NAME} 65 | SOURCE_DIRS src include test 66 | CT_HEADER_DIRS include 67 | CF_WERROR 68 | ) 69 | endif (cmake_clang_tools_FOUND) 70 | 71 | ############# 72 | ## Install ## 73 | ############# 74 | 75 | install( 76 | DIRECTORY include/ 77 | DESTINATION include/${PROJECT_NAME} 78 | ) 79 | 80 | install( 81 | TARGETS ${PROJECT_NAME} 82 | EXPORT export_${PROJECT_NAME} 83 | ARCHIVE DESTINATION lib 84 | LIBRARY DESTINATION lib 85 | RUNTIME DESTINATION bin 86 | ) 87 | 88 | install( 89 | DIRECTORY 90 | DESTINATION share/${PROJECT_NAME}/ 91 | ) 92 | 93 | ament_export_dependencies(${dependencies}) 94 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 95 | 96 | ############# 97 | ## Testing ## 98 | ############# 99 | find_package(ament_lint_auto REQUIRED) 100 | ament_lint_auto_find_test_dependencies() 101 | 102 | if (BUILD_TESTING) 103 | find_package(ament_cmake_gtest) 104 | 105 | ament_add_gtest(${PROJECT_NAME}_test 106 | test/HoQp_test.cpp 107 | ) 108 | target_include_directories(${PROJECT_NAME}_test PRIVATE 109 | ${PROJECT_NAME} 110 | ${pinocchio_LIBRARIES} 111 | gtest_main 112 | ) 113 | ament_target_dependencies(${PROJECT_NAME}_test ${dependencies}) 114 | target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) 115 | target_compile_options(${PROJECT_NAME}_test PRIVATE ${FLAGS}) 116 | endif () 117 | 118 | ament_package() 119 | -------------------------------------------------------------------------------- /g1_wbc/include/g1_wbc/HierarchicalWbc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 22-12-23. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "g1_wbc/WbcBase.h" 15 | 16 | namespace ocs2 17 | { 18 | namespace g1 19 | { 20 | class HierarchicalWbc : public WbcBase 21 | { 22 | public: 23 | using WbcBase::WbcBase; 24 | 25 | vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured, 26 | size_t mode, scalar_t period) override; 27 | }; 28 | 29 | } // namespace g1 30 | } // namespace ocs2 31 | -------------------------------------------------------------------------------- /g1_wbc/include/g1_wbc/HoQp.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/6/28. 3 | // 4 | // 5 | // Ref: https://github.com/bernhardpg/quadruped_locomotion 6 | // 7 | 8 | /******************************************************************************** 9 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 10 | 11 | For further information, contact: contact@bridgedp.com or visit our website 12 | at www.bridgedp.com. 13 | ********************************************************************************/ 14 | 15 | #pragma once 16 | 17 | #include "g1_wbc/Task.h" 18 | 19 | #include 20 | 21 | namespace ocs2 22 | { 23 | namespace g1 24 | { 25 | // Hierarchical Optimization Quadratic Program 26 | class HoQp 27 | { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | using HoQpPtr = std::shared_ptr; 32 | 33 | explicit HoQp(const Task& task) : HoQp(task, nullptr){}; 34 | 35 | HoQp(Task task, HoQpPtr higherProblem); 36 | 37 | matrix_t getStackedZMatrix() const 38 | { 39 | return stackedZ_; 40 | } 41 | 42 | Task getStackedTasks() const 43 | { 44 | return stackedTasks_; 45 | } 46 | 47 | vector_t getStackedSlackSolutions() const 48 | { 49 | return stackedSlackVars_; 50 | } 51 | 52 | vector_t getSolutions() const 53 | { 54 | vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_; 55 | return x; 56 | } 57 | 58 | size_t getSlackedNumVars() const 59 | { 60 | return stackedTasks_.d_.rows(); 61 | } 62 | 63 | private: 64 | void initVars(); 65 | void formulateProblem(); 66 | void solveProblem(); 67 | 68 | void buildHMatrix(); 69 | void buildCVector(); 70 | void buildDMatrix(); 71 | void buildFVector(); 72 | 73 | void buildZMatrix(); 74 | void stackSlackSolutions(); 75 | 76 | Task task_, stackedTasksPrev_, stackedTasks_; 77 | HoQpPtr higherProblem_; 78 | 79 | bool hasEqConstraints_{}, hasIneqConstraints_{}; 80 | size_t numSlackVars_{}, numDecisionVars_{}; 81 | matrix_t stackedZPrev_, stackedZ_; 82 | vector_t stackedSlackSolutionsPrev_, xPrev_; 83 | size_t numPrevSlackVars_{}; 84 | 85 | Eigen::Matrix h_, d_; 86 | vector_t c_, f_; 87 | vector_t stackedSlackVars_, slackVarsSolutions_, decisionVarsSolutions_; 88 | 89 | matrix_t eyeNvNv_; 90 | matrix_t zeroNvNx_; 91 | }; 92 | 93 | } // namespace g1 94 | } // namespace ocs2 95 | -------------------------------------------------------------------------------- /g1_wbc/include/g1_wbc/Task.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/6/28. 3 | // 4 | // 5 | // Ref: https://github.com/bernhardpg/quadruped_locomotion 6 | // 7 | 8 | /******************************************************************************** 9 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 10 | 11 | For further information, contact: contact@bridgedp.com or visit our website 12 | at www.bridgedp.com. 13 | ********************************************************************************/ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace ocs2 22 | { 23 | namespace g1 24 | { 25 | // using namespace ocs2; 26 | 27 | // Ax -b = w 28 | // Dx - f <= v 29 | // w -> 0, v -> 0 30 | 31 | class Task 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | 36 | Task() = default; 37 | 38 | Task(matrix_t a, vector_t b, matrix_t d, vector_t f) 39 | : a_(std::move(a)), d_(std::move(d)), b_(std::move(b)), f_(std::move(f)) 40 | { 41 | } 42 | 43 | explicit Task(size_t numDecisionVars) 44 | : Task(matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0), matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0)) 45 | { 46 | } 47 | 48 | Task operator+(const Task& rhs) const 49 | { 50 | return { concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_), 51 | concatenateVectors(f_, rhs.f_) }; 52 | } 53 | 54 | Task operator*(scalar_t rhs) const 55 | { 56 | return {a_.cols() > 0 ? rhs * a_ : a_, 57 | b_.cols() > 0 ? rhs * b_ : b_, 58 | d_.cols() > 0 ? rhs * d_ : d_, 59 | f_.cols() > 0 ? rhs * f_ : f_}; // clang-format on 60 | } 61 | 62 | matrix_t a_, d_; 63 | vector_t b_, f_; 64 | 65 | static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) 66 | { 67 | if (m1.cols() <= 0) 68 | { 69 | return m2; 70 | } 71 | else if (m2.cols() <= 0) 72 | { 73 | return m1; 74 | } 75 | assert(m1.cols() == m2.cols()); 76 | matrix_t res(m1.rows() + m2.rows(), m1.cols()); 77 | res << m1, m2; 78 | return res; 79 | } 80 | 81 | static vector_t concatenateVectors(const vector_t& v1, const vector_t& v2) 82 | { 83 | if (v1.cols() <= 0) 84 | { 85 | return v2; 86 | } 87 | else if (v2.cols() <= 0) 88 | { 89 | return v1; 90 | } 91 | assert(v1.cols() == v2.cols()); 92 | vector_t res(v1.rows() + v2.rows()); 93 | res << v1, v2; 94 | return res; 95 | } 96 | }; 97 | 98 | } // namespace g1 99 | } // namespace ocs2 100 | -------------------------------------------------------------------------------- /g1_wbc/include/g1_wbc/WbcBase.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/7/1. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include "g1_wbc/Task.h" 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | namespace ocs2 23 | { 24 | namespace g1 25 | { 26 | // using namespace ocs2; 27 | // using namespace g1; 28 | 29 | // Ax -b = w 30 | // Dx - f <= v 31 | // w -> 0, v -> 0 32 | 33 | // Decision Variables: x = [\dot u^T, 3*F(3)^T, \tau^T]^T , \dot u in ocal frame 34 | class WbcBase 35 | { 36 | using Vector6 = Eigen::Matrix; 37 | using Matrix6 = Eigen::Matrix; 38 | using Matrix6x = Eigen::Matrix; 39 | 40 | public: 41 | WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info, 42 | const PinocchioEndEffectorKinematics& eeKinematics); 43 | 44 | virtual void loadTasksSetting(const std::string& taskFile, bool verbose); 45 | 46 | virtual vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured, 47 | size_t mode, scalar_t period); 48 | void setCmdBodyPosVel(const vector_t& cmd_body_pos, const vector_t& cmd_body_vel) 49 | { 50 | cmd_body_pos_ = cmd_body_pos; 51 | cmd_body_vel_ = cmd_body_vel; 52 | } 53 | 54 | void setEarlyLateContact(const std::array& early_late_contact) 55 | { 56 | earlyLatecontact_ = early_late_contact; 57 | } 58 | 59 | void setFootPosVelAccDesired(const std::array& footPosVelAccDesired) 60 | { 61 | footPosVelAccDesired_ = footPosVelAccDesired; 62 | } 63 | void setJointAccDesired(const vector_t& jointAccDesired) 64 | { 65 | jointAccDesired_ = jointAccDesired; 66 | } 67 | void setKpKd(scalar_t swingKp, scalar_t swingKd) 68 | { 69 | swingKp_ = swingKp; 70 | swingKd_ = swingKd; 71 | } 72 | size_t getContactForceSize() 73 | { 74 | return contact_force_size_; 75 | } 76 | 77 | protected: 78 | void updateMeasured(const vector_t& rbdStateMeasured); 79 | void updateDesired(const vector_t& stateDesired, const vector_t& inputDesired); 80 | 81 | size_t getNumDecisionVars() const 82 | { 83 | return numDecisionVars_; 84 | } 85 | 86 | Task formulateFloatingBaseEomTask(); 87 | Task formulateTorqueLimitsTask(); 88 | Task formulateNoContactMotionTask(); 89 | Task formulateFrictionConeTask(); 90 | Task formulateBaseAngularMotionTask(); 91 | Task formulateBaseAccelTask(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period); 92 | Task formulateSwingLegTask(); 93 | Task formulateContactForceTask(const vector_t& inputDesired) const; 94 | 95 | void compensateFriction(vector_t& x); 96 | 97 | size_t numDecisionVars_; 98 | PinocchioInterface pinocchioInterfaceMeasured_, pinocchioInterfaceDesired_; 99 | CentroidalModelInfo info_; 100 | 101 | std::unique_ptr eeKinematics_; 102 | CentroidalModelPinocchioMapping mapping_; 103 | CentroidalModelRbdConversions rbdConversions_; 104 | 105 | vector_t qMeasured_, vMeasured_, inputLast_; 106 | matrix_t j_, dj_; 107 | Matrix6x base_j_, base_dj_; 108 | contact_flag_t contactFlag_{}; 109 | size_t numContacts_{}; 110 | 111 | vector_t torqueLimits_; 112 | scalar_t frictionCoeff_{}, swingKp_{}, swingKd_{}; 113 | scalar_t baseHeightKp_{}, baseHeightKd_{}; 114 | scalar_t baseAngularKp_{}, baseAngularKd_{}; 115 | 116 | vector_t cmd_body_pos_; 117 | vector_t cmd_body_vel_; 118 | scalar_t com_kp_{}, com_kd_{}; 119 | Vector6 basePoseDes_, baseVelocityDes_, baseAccelerationDes_; 120 | 121 | std::array earlyLatecontact_; 122 | 123 | std::vector footPosDesired_, footVelDesired_; 124 | std::array footPosVelAccDesired_; 125 | 126 | vector_t jointAccDesired_; 127 | size_t contact_force_size_ = 0; 128 | scalar_t contactForceFilter_ = 0.5; 129 | }; 130 | 131 | } // namespace g1 132 | } // namespace ocs2 133 | -------------------------------------------------------------------------------- /g1_wbc/include/g1_wbc/WeightedWbc.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 22-12-23. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include "g1_wbc/WbcBase.h" 13 | 14 | namespace ocs2 15 | { 16 | namespace g1 17 | { 18 | class WeightedWbc : public WbcBase 19 | { 20 | public: 21 | using WbcBase::WbcBase; 22 | 23 | vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured, 24 | size_t mode, scalar_t period) override; 25 | 26 | void loadTasksSetting(const std::string& taskFile, bool verbose) override; 27 | 28 | protected: 29 | virtual Task formulateConstraints(); 30 | virtual Task formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period); 31 | 32 | private: 33 | scalar_t weightSwingLeg_, weightBaseAccel_, weightContactForce_; 34 | 35 | vector_t last_qpSol; 36 | }; 37 | 38 | } // namespace g1 39 | } // namespace ocs2 -------------------------------------------------------------------------------- /g1_wbc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | g1_wbc 4 | 0.0.0 5 | Whole body control 6 | bridgedp 7 | 8 | MIT 9 | 10 | ament_cmake 11 | 12 | cmake_clang_tools 13 | 14 | g1_interface 15 | qpoases_catkin 16 | pinocchio 17 | 18 | ament_cmake_gtest 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /g1_wbc/src/HierarchicalWbc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 22-12-23. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include "g1_wbc/HierarchicalWbc.h" 13 | 14 | #include "g1_wbc/HoQp.h" 15 | 16 | namespace ocs2 17 | { 18 | namespace g1 19 | { 20 | vector_t HierarchicalWbc::update(const vector_t& stateDesired, const vector_t& inputDesired, 21 | const vector_t& rbdStateMeasured, size_t mode, scalar_t period) 22 | { 23 | WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); 24 | 25 | Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + 26 | formulateNoContactMotionTask(); 27 | Task task1 = formulateBaseAccelTask(stateDesired, inputDesired, period); 28 | Task task2 = formulateContactForceTask(inputDesired) * 0.1 + formulateSwingLegTask() * 1; 29 | HoQp hoQp(task2, std::make_shared(task1, std::make_shared(task0))); 30 | 31 | return hoQp.getSolutions(); 32 | } 33 | 34 | } // namespace g1 35 | } // namespace ocs2 36 | 37 | -------------------------------------------------------------------------------- /g1_wbc/src/WeightedWbc.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 22-12-23. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include "g1_wbc/WeightedWbc.h" 13 | 14 | #include 15 | 16 | 17 | namespace ocs2 18 | { 19 | namespace g1 20 | { 21 | vector_t WeightedWbc::update(const vector_t& stateDesired, const vector_t& inputDesired, 22 | const vector_t& rbdStateMeasured, size_t mode, scalar_t period) 23 | { 24 | WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); 25 | 26 | // Constraints 27 | Task constraints = formulateConstraints(); 28 | size_t numConstraints = constraints.b_.size() + constraints.f_.size(); 29 | 30 | Eigen::Matrix A(numConstraints, getNumDecisionVars()); 31 | vector_t lbA(numConstraints), ubA(numConstraints); // clang-format off 32 | A << constraints.a_, 33 | constraints.d_; 34 | 35 | lbA << constraints.b_, 36 | -qpOASES::INFTY * vector_t::Ones(constraints.f_.size()); 37 | ubA << constraints.b_, 38 | constraints.f_; // clang-format on 39 | 40 | // Cost 41 | Task weighedTask = formulateWeightedTasks(stateDesired, inputDesired, period); 42 | Eigen::Matrix H = 43 | weighedTask.a_.transpose() * weighedTask.a_; 44 | vector_t g = -weighedTask.a_.transpose() * weighedTask.b_; 45 | 46 | // Solve 47 | auto qpProblem = qpOASES::QProblem(getNumDecisionVars(), numConstraints); 48 | qpOASES::Options options; 49 | options.setToMPC(); 50 | options.printLevel = qpOASES::PL_NONE; 51 | #ifdef DEBUG 52 | options.printLevel = qpOASES::PL_LOW; 53 | #endif 54 | options.enableEqualities = qpOASES::BT_TRUE; 55 | qpProblem.setOptions(options); 56 | int nWsr = 20; 57 | 58 | qpProblem.init(H.data(), g.data(), A.data(), nullptr, nullptr, lbA.data(), ubA.data(), nWsr); 59 | vector_t qpSol(getNumDecisionVars()); 60 | 61 | qpProblem.getPrimalSolution(qpSol.data()); 62 | 63 | if (!qpProblem.isSolved()) 64 | { 65 | #ifdef DEBUG 66 | std::cout << "ERROR: WeightWBC Not Solved!!!" << std::endl; 67 | #endif 68 | if (last_qpSol.size() > 0) 69 | qpSol = last_qpSol; 70 | } 71 | 72 | last_qpSol = qpSol; 73 | return qpSol; 74 | } 75 | 76 | Task WeightedWbc::formulateConstraints() 77 | { 78 | return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask(); 79 | } 80 | 81 | Task WeightedWbc::formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period) 82 | { 83 | return formulateSwingLegTask() * weightSwingLeg_ + 84 | formulateBaseAccelTask(stateDesired, inputDesired, period) * weightBaseAccel_ + 85 | formulateContactForceTask(inputDesired) * weightContactForce_; 86 | } 87 | 88 | 89 | void WeightedWbc::loadTasksSetting(const std::string& taskFile, bool verbose) 90 | { 91 | WbcBase::loadTasksSetting(taskFile, verbose); 92 | 93 | boost::property_tree::ptree pt; 94 | boost::property_tree::read_info(taskFile, pt); 95 | std::string prefix = "weight."; 96 | if (verbose) 97 | { 98 | std::cerr << "\n #### WBC weight:"; 99 | std::cerr << "\n #### =============================================================================\n"; 100 | } 101 | loadData::loadPtreeValue(pt, weightSwingLeg_, prefix + "swingLeg", verbose); 102 | loadData::loadPtreeValue(pt, weightBaseAccel_, prefix + "baseAccel", verbose); 103 | loadData::loadPtreeValue(pt, weightContactForce_, prefix + "contactForce", verbose); 104 | } 105 | 106 | } // namespace g1 107 | } // namespace ocs2 108 | -------------------------------------------------------------------------------- /g1_wbc/test/HoQp_test.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by qiayuan on 2022/6/29. 3 | // 4 | 5 | /******************************************************************************** 6 | Modified Copyright (c) 2023-2024, BridgeDP Robotics.Co.Ltd. All rights reserved. 7 | 8 | For further information, contact: contact@bridgedp.com or visit our website 9 | at www.bridgedp.com. 10 | ********************************************************************************/ 11 | 12 | #include 13 | 14 | #include "g1_wbc/HoQp.h" 15 | 16 | using namespace ocs2; 17 | using namespace g1; 18 | 19 | TEST(HoQP, twoTask) 20 | { 21 | srand(0); 22 | Task task0, task1; 23 | task0.a_ = matrix_t::Random(2, 4); 24 | task0.b_ = vector_t::Ones(2); 25 | task0.d_ = matrix_t::Random(2, 4); 26 | task0.f_ = vector_t::Ones(2); 27 | task1 = task0; 28 | task1.a_ = matrix_t::Ones(2, 4); 29 | std::shared_ptr hoQp0 = std::make_shared(task0); 30 | std::shared_ptr hoQp1 = std::make_shared(task1, hoQp0); 31 | 32 | vector_t x0 = hoQp0->getSolutions(), x_1 = hoQp1->getSolutions(); 33 | vector_t slack0 = hoQp0->getStackedSlackSolutions(), slack_1 = hoQp1->getStackedSlackSolutions(); 34 | 35 | std::cout << x0.transpose() << std::endl; 36 | std::cout << x_1.transpose() << std::endl; 37 | std::cout << slack0.transpose() << std::endl; 38 | std::cout << slack_1.transpose() << std::endl; 39 | 40 | scalar_t prec = 1e-6; 41 | 42 | if (slack0.isApprox(vector_t::Zero(slack0.size()))) 43 | EXPECT_TRUE((task0.a_ * x0).isApprox(task0.b_, prec)); 44 | if (slack_1.isApprox(vector_t::Zero(slack_1.size()))) 45 | { 46 | EXPECT_TRUE((task1.a_ * x_1).isApprox(task1.b_, prec)); 47 | EXPECT_TRUE((task0.a_ * x_1).isApprox(task0.b_, prec)); 48 | } 49 | 50 | vector_t y = task0.d_ * x0; 51 | for (int i = 0; i < y.size(); ++i) 52 | EXPECT_TRUE(y[i] <= task0.f_[i] + slack0[i]); 53 | y = task1.d_ * x_1; 54 | for (int i = 0; i < y.size(); ++i) 55 | EXPECT_TRUE(y[i] <= task1.f_[i] + slack_1[i]); 56 | } 57 | -------------------------------------------------------------------------------- /qpoases_catkin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(qpoases_catkin) 3 | 4 | # Find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | 7 | 8 | include(FetchContent) 9 | 10 | # Define directories 11 | set(QPOASES_DEVEL_PREFIX ${CMAKE_INSTALL_PREFIX} CACHE STRING "QPOASES install path") 12 | set(QPOASES_INCLUDE_DIR ${QPOASES_DEVEL_PREFIX}/include) 13 | set(QPOASES_LIB_DIR ${QPOASES_DEVEL_PREFIX}/lib) 14 | set(QPOASES_DOWNLOAD_DIR ${CMAKE_CURRENT_BINARY_DIR}/download) 15 | set(QPOASES_BUILD_DIR ${CMAKE_CURRENT_BINARY_DIR}/build) 16 | 17 | # Create directories if they do not exist 18 | file(MAKE_DIRECTORY ${QPOASES_INCLUDE_DIR}) 19 | file(MAKE_DIRECTORY ${QPOASES_LIB_DIR}) 20 | file(MAKE_DIRECTORY ${QPOASES_DOWNLOAD_DIR}) 21 | file(MAKE_DIRECTORY ${QPOASES_BUILD_DIR}) 22 | 23 | # HPIPM Settings 24 | set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries" FORCE) 25 | set(QPOASES_BUILD_EXAMPLES OFF CACHE BOOL "Examples disable") 26 | 27 | # Download & build source 28 | FetchContent_Declare(qpoasesDownload 29 | GIT_REPOSITORY https://github.com/coin-or/qpOASES 30 | GIT_TAG 268b2f2659604df27c82aa6e32aeddb8c1d5cc7f 31 | UPDATE_COMMAND "" 32 | SOURCE_DIR ${QPOASES_DOWNLOAD_DIR} 33 | BINARY_DIR ${QPOASES_BUILD_DIR} 34 | BUILD_COMMAND $(MAKE) 35 | INSTALL_COMMAND "$(MAKE) install" 36 | ) 37 | FetchContent_MakeAvailable(qpoasesDownload) 38 | 39 | # Copy header to where catkin expects them 40 | file(COPY ${QPOASES_DOWNLOAD_DIR}/include/qpOASES.hpp DESTINATION ${QPOASES_INCLUDE_DIR}) 41 | 42 | file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/*") 43 | foreach (HEADER_FILE ${HEADERS}) 44 | message(STATUS "FOUND HEADER: " ${HEADER_FILE}) 45 | file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES) 46 | endforeach () 47 | 48 | file(GLOB_RECURSE HEADERS "${QPOASES_DOWNLOAD_DIR}/include/qpOASES/extras/*") 49 | foreach (HEADER_FILE ${HEADERS}) 50 | message(STATUS "FOUND HEADER: " ${HEADER_FILE}) 51 | file(COPY ${HEADER_FILE} DESTINATION ${QPOASES_INCLUDE_DIR}/qpOASES/extras) 52 | endforeach () 53 | 54 | 55 | 56 | ament_export_include_directories(${QPOASES_INCLUDE_DIR}) 57 | ament_export_libraries(qpOASES) 58 | 59 | install(TARGETS qpOASES 60 | EXPORT export_${PROJECT_NAME} 61 | LIBRARY DESTINATION lib) 62 | 63 | # Declare a ROS 2 package 64 | ament_package() -------------------------------------------------------------------------------- /qpoases_catkin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | qpoases_catkin 4 | 0.0.0 5 | Catkin wrapper for qpoases. 6 | Qiayuan Liao 7 | 8 | See package 9 | 10 | ament_cmake 11 | 12 | 13 | 14 | 15 | ament_cmake 16 | 17 | 18 | 19 | --------------------------------------------------------------------------------