Merge pull request #2 from legubiao/ocs2

Ocs2

README.md

@@ -7,17 +7,38 @@ This repository contains the ros2-control based controllers for the quadruped ro
 * [Hardwares](hardwares): contains the ros2-control hardware interface for the robot
 
 Todo List:
-- [x] Mujoco Simulation
-- [x] Unitree Guide Controller
-- [x] Gazebo Simulation
-- [x] Leg PD Controller
-- [ ] Contact Sensor
-- [ ] OCS2 Legged Control
+- [x] [Mujoco Simulation](hardwares/hardware_unitree_mujoco)
+- [x] [Unitree Guide Controller](controllers/unitree_guide_controller)
+- [x] [Gazebo Simulation](descriptions/quadruped_gazebo)
+- [x] [Leg PD Controller](controllers/leg_pd_controller)
+- [x] [Contact Sensor Simulation](https://github.com/legubiao/unitree_mujoco)
+- [x] [OCS2 Quadruped Control](controllers/ocs2_quadruped_controller)
+- [ ] Learning-based Controller
+
+Video for Unitree Guide Controller:
+[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)
 
 
-## 1. Build
+
+## Quick Start
 * rosdep
 ```bash
 cd ~/ros2_ws
 rosdep install --from-paths src --ignore-src -r -y
-```
+```
+* Compile the package
+```bash
+colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input hardware_unitree_mujoco
+```
+* Launch the unitree mujoco go2 simulation
+* Launch the ros2-control
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch go2_description unitree_guide.launch.py
+```
+* Run the keyboard control node
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 run keyboard_input keyboard_input
+```
+For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/go2_description/).

commands/joystick_input/README.md

@@ -24,9 +24,9 @@ ros2 launch joystick_input joystick.launch.py
 * Passive Mode: LB + B
 * Fixed Stand: LB + A
     * Free Stand: LB + X
-    * Trot: start
-    * SwingTest: LT + A
-    * Balance: LT + X
+    * Trot: LB + Y
+    * SwingTest: LT + B
+    * Balance: LT + A
 
 ### 1.2 Control Input
 

commands/joystick_input/src/JoystickInput.cpp

@@ -19,14 +19,18 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
         inputs_.command = 2; // LB + A
     } else if (msg->buttons[2] && msg->buttons[4]) {
         inputs_.command = 3; // LB + X
-    } else if (msg->buttons[7]) {
-        inputs_.command = 4; // START
-    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
-        inputs_.command = 10; // LT + X
+    } else if (msg->buttons[3] && msg->buttons[4]) {
+        inputs_.command = 4; // LB + Y
+    } else if (msg->axes[2] != 1 && msg->buttons[1]) {
+        inputs_.command = 5; // LT + B
     } else if (msg->axes[2] != 1 && msg->buttons[0]) {
-        inputs_.command = 9; // LT + A
+        inputs_.command = 6; // LT + A
+    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
+        inputs_.command = 7; // LT + X
     } else if (msg->axes[2] != 1 && msg->buttons[3]) {
         inputs_.command = 8; // LT + Y
+    } else if (msg->buttons[7]) {
+        inputs_.command = 9; // START
     } else {
         inputs_.command = 0;
         inputs_.lx = -msg->axes[0];

commands/keyboard_input/README.md

@@ -24,8 +24,8 @@ ros2 run keyboard_input keyboard_input
 * Fixed Stand: Keyboard 2
     * Free Stand: Keyboard 3
     * Trot: Keyboard 4
-    * SwingTest: Keyboard 9
-    * Balance: Keyboard 0
+    * SwingTest: Keyboard 5
+    * Balance: Keyboard 6
 ### 1.2 Control Input
 * WASD IJKL: Move robot
 * Space: Reset Speed Input

commands/keyboard_input/include/keyboard_input/KeyboardInput.h

@@ -41,6 +41,9 @@ private:
     rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
 
+    bool just_published_ = false;
+    int reset_count_ = 0;
+
     float sensitivity_left_ = 0.05;
     float sensitivity_right_ = 0.05;
     termios old_tio_{}, new_tio_{};

commands/keyboard_input/src/KeyboardInput.cpp

@@ -26,8 +26,21 @@ void KeyboardInput::timer_callback() {
             inputs_.ly = 0;
             inputs_.rx = 0;
             inputs_.ry = 0;
+            reset_count_ = 100;
         }
         publisher_->publish(inputs_);
+        just_published_ = true;
+    } else {
+        if (just_published_) {
+            reset_count_ -= 1;
+            if (reset_count_ == 0) {
+                just_published_ = false;
+                if (inputs_.command != 0) {
+                    inputs_.command = 0;
+                    publisher_->publish(inputs_);
+                }
+            }
+        }
     }
 }
 
@@ -43,17 +56,26 @@ void KeyboardInput::check_command(const char key) {
             inputs_.command = 3; // L2_X
             break;
         case '4':
-            inputs_.command = 4; // START
+            inputs_.command = 4; // L2_Y
             break;
-        case '0':
-            inputs_.command = 10; // L1_X
+        case '5':
+            inputs_.command = 5; // L1_A
             break;
-        case '9':
-            inputs_.command = 9; // L1_A
+        case '6':
+            inputs_.command = 6; // L1_B
+            break;
+        case '7':
+            inputs_.command = 7; // L1_X
             break;
         case '8':
             inputs_.command = 8; // L1_Y
             break;
+        case '9':
+            inputs_.command = 9;
+            break;
+        case '0':
+            inputs_.command = 10;
+        break;
         case ' ':
             inputs_.lx = 0;
             inputs_.ly = 0;

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 3.8)
+project(ocs2_quadruped_controller)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set(CONTROLLER_INCLUDE_DEPENDS
+        pluginlib
+        rcpputils
+        controller_interface
+        realtime_tools
+
+        ocs2_legged_robot_ros
+        ocs2_self_collision
+        control_input_msgs
+        angles
+        nav_msgs
+        qpoases_colcon
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
+    find_package(${Dependency} REQUIRED)
+endforeach ()
+
+add_library(${PROJECT_NAME} SHARED
+        src/Ocs2QuadrupedController.cpp
+
+        #        src/estimator/FromTopicEstimate.cpp
+        src/estimator/LinearKalmanFilter.cpp
+        src/estimator/StateEstimateBase.cpp
+
+        src/wbc/HierarchicalWbc.cpp
+        src/wbc/HoQp.cpp
+        src/wbc/WbcBase.cpp
+        src/wbc/WeightedWbc.cpp
+
+        src/interface/constraint/EndEffectorLinearConstraint.cpp
+        src/interface/constraint/FrictionConeConstraint.cpp
+        src/interface/constraint/ZeroForceConstraint.cpp
+        src/interface/constraint/NormalVelocityConstraintCppAd.cpp
+        src/interface/constraint/ZeroVelocityConstraintCppAd.cpp
+        src/interface/constraint/SwingTrajectoryPlanner.cpp
+        src/interface/initialization/LeggedRobotInitializer.cpp
+        src/interface/SwitchedModelReferenceManager.cpp
+        src/interface/LeggedRobotPreComputation.cpp
+        src/interface/LeggedInterface.cpp
+
+        src/control/GaitManager.cpp
+        src/control/TargetManager.cpp
+
+)
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        ${qpOASES_INCLUDE_DIR}
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+        PRIVATE
+        src)
+ament_target_dependencies(${PROJECT_NAME}
+        ${CONTROLLER_INCLUDE_DEPENDS}
+)
+pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml)
+
+install(
+        DIRECTORY include/
+        DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+        TARGETS ${PROJECT_NAME}
+        EXPORT export_${PROJECT_NAME}
+        ARCHIVE DESTINATION lib/${PROJECT_NAME}
+        LIBRARY DESTINATION lib/${PROJECT_NAME}
+        RUNTIME DESTINATION bin
+)
+
+install(
+        DIRECTORY config
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+
+ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+if (BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # comment the line when a copyright and license is added to all source files
+    set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # comment the line when this package is in a git repo and when
+    # a copyright and license is added to all source files
+    set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+endif ()
+
+ament_package()

controllers/ocs2_quadruped_controller/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

controllers/ocs2_quadruped_controller/README.md

@@ -0,0 +1,43 @@
+# OCS2 Quadruped Controller
+
+This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control)
+and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
+
+## 2. Build
+
+### 2.1 Build Dependencies
+
+* OCS2 ROS2 Libraries
+  ```bash
+  colcon build --packages-up-to ocs2_legged_robot_ros
+  colcon build --packages-up-to ocs2_self_collision
+  ```
+
+### 2.2 Build OCS2 Quadruped Controller
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to ocs2_quadruped_controller
+```
+
+## 3. Launch
+* Unitree Go1 Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch go1_description ocs2_control.launch.py
+  ```
+* Unitree Aliengo Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description ocs2_control.launch.py
+  ```
+* Unitree Go2 Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch go2_description ocs2_control.launch.py
+  ```
+* Unitree B2 Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch b2_description ocs2_control.launch.py
+  ```

controllers/ocs2_quadruped_controller/config/visualize_ocs2.rviz

@@ -0,0 +1,607 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /Optimized State Trajectory1
+        - /Target Trajectories1/Target Feet Trajectories1/Marker1
+        - /Target Trajectories1/Target Feet Trajectories1/Marker2
+        - /Target Trajectories1/Target Feet Trajectories1/Marker3
+        - /Target Trajectories1/Target Feet Trajectories1/Marker4
+        - /Target Trajectories1/Target Base Trajectory1
+      Splitter Ratio: 0.5
+    Tree Height: 477
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.8294117450714111
+    Tree Height: 301
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Alpha: 0.4000000059604645
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        head_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tail_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        trunk:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        FL_calf:
+          Value: true
+        FL_calf_rotor:
+          Value: true
+        FL_foot:
+          Value: true
+        FL_hip:
+          Value: true
+        FL_hip_rotor:
+          Value: true
+        FL_thigh:
+          Value: true
+        FL_thigh_rotor:
+          Value: true
+        FR_calf:
+          Value: true
+        FR_calf_rotor:
+          Value: true
+        FR_foot:
+          Value: true
+        FR_hip:
+          Value: true
+        FR_hip_rotor:
+          Value: true
+        FR_thigh:
+          Value: true
+        FR_thigh_rotor:
+          Value: true
+        RL_calf:
+          Value: true
+        RL_calf_rotor:
+          Value: true
+        RL_foot:
+          Value: true
+        RL_hip:
+          Value: true
+        RL_hip_rotor:
+          Value: true
+        RL_thigh:
+          Value: true
+        RL_thigh_rotor:
+          Value: true
+        RR_calf:
+          Value: true
+        RR_calf_rotor:
+          Value: true
+        RR_foot:
+          Value: true
+        RR_hip:
+          Value: true
+        RR_hip_rotor:
+          Value: true
+        RR_thigh:
+          Value: true
+        RR_thigh_rotor:
+          Value: true
+        base:
+          Value: false
+        head_Link:
+          Value: true
+        imu_link:
+          Value: false
+        lidar_link:
+          Value: true
+        odom:
+          Value: false
+        tail_link:
+          Value: true
+        trunk:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: false
+      Show Names: false
+      Tree:
+        odom:
+          base:
+            trunk:
+              FL_hip:
+                FL_thigh:
+                  FL_calf:
+                    FL_foot:
+                      {}
+                  FL_calf_rotor:
+                    {}
+                FL_thigh_rotor:
+                  {}
+              FL_hip_rotor:
+                {}
+              FR_hip:
+                FR_thigh:
+                  FR_calf:
+                    FR_foot:
+                      {}
+                  FR_calf_rotor:
+                    {}
+                FR_thigh_rotor:
+                  {}
+              FR_hip_rotor:
+                {}
+              RL_hip:
+                RL_thigh:
+                  RL_calf:
+                    RL_foot:
+                      {}
+                  RL_calf_rotor:
+                    {}
+                RL_thigh_rotor:
+                  {}
+              RL_hip_rotor:
+                {}
+              RR_hip:
+                RR_thigh:
+                  RR_calf:
+                    RR_foot:
+                      {}
+                  RR_calf_rotor:
+                    {}
+                RR_thigh_rotor:
+                  {}
+              RR_hip_rotor:
+                {}
+              head_Link:
+                {}
+              imu_link:
+                {}
+              lidar_link:
+                {}
+              tail_link:
+                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: Optimized State Trajectory
+      Namespaces:
+        CoM Trajectory: true
+        EE Trajectories: true
+        Future footholds: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /legged_robot/optimizedStateTrajectory
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: Current State
+      Namespaces:
+        Center of Pressure: true
+        EE Forces: true
+        EE Positions: true
+        Support Polygon: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /legged_robot/currentState
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/Marker
+              Enabled: true
+              Name: Marker
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /legged_robot/desiredFeetTrajectory/LF
+              Value: true
+            - Class: rviz_default_plugins/Marker
+              Enabled: true
+              Name: Marker
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /legged_robot/desiredFeetTrajectory/LH
+              Value: true
+            - Class: rviz_default_plugins/Marker
+              Enabled: true
+              Name: Marker
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /legged_robot/desiredFeetTrajectory/RF
+              Value: true
+            - Class: rviz_default_plugins/Marker
+              Enabled: true
+              Name: Marker
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /legged_robot/desiredFeetTrajectory/RH
+              Value: true
+          Enabled: false
+          Name: Target Feet Trajectories
+        - Class: rviz_default_plugins/Marker
+          Enabled: true
+          Name: Target Base Trajectory
+          Namespaces:
+            "": true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /legged_robot/desiredBaseTrajectory
+          Value: true
+      Enabled: true
+      Name: Target Trajectories
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Class: grid_map_rviz_plugin/GridMap
+      Color: 200; 200; 200
+      Color Layer: elevation
+      Color Transformer: GridMapLayer
+      Enabled: true
+      Height Layer: elevation
+      Height Transformer: GridMapLayer
+      History Length: 1
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 10
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: GridMap
+      Show Grid Lines: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /raisim_heightmap
+      Use Rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 238; 238; 238
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /move_base_simple/goal
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 4.046566963195801
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.5368534326553345
+        Y: 0.8848452568054199
+        Z: 0.04236998409032822
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4303978383541107
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 5.236791610717773
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 655
+  Hide Left Dock: true
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000032c000002a7fc020000000afb0000001200530065006c0065006300740069006f006e000000006e000000b00000005d00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e000004770000005d00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003f000002a7000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e00000247000000cc00ffffff000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000004000000023100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1024
+  X: 828
+  Y: 212

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h

@@ -0,0 +1,71 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#ifndef INTERFACE_H
+#define INTERFACE_H
+
+#include <vector>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/loaned_state_interface.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+#include <ocs2_mpc/SystemObservation.h>
+#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
+
+#include "TargetManager.h"
+#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
+
+struct CtrlComponent {
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_torque_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kp_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kd_command_interface_;
+
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_effort_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_position_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_velocity_state_interface_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    imu_state_interface_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    foot_force_state_interface_;
+
+    control_input_msgs::msg::Inputs control_inputs_;
+    ocs2::SystemObservation observation_;
+    int frequency_{};
+
+    std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
+    std::shared_ptr<ocs2::legged_robot::TargetManager> target_manager_;
+    std::shared_ptr<ocs2::legged_robot::LeggedRobotVisualizer> visualizer_;
+
+    CtrlComponent() {
+    }
+
+    void clear() {
+        joint_torque_command_interface_.clear();
+        joint_position_command_interface_.clear();
+        joint_velocity_command_interface_.clear();
+        joint_kd_command_interface_.clear();
+        joint_kp_command_interface_.clear();
+
+        joint_effort_state_interface_.clear();
+        joint_position_state_interface_.clear();
+        joint_velocity_state_interface_.clear();
+
+        imu_state_interface_.clear();
+        foot_force_state_interface_.clear();
+    }
+};
+
+#endif //INTERFACE_H

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h

@@ -0,0 +1,43 @@
+//
+// Created by tlab-uav on 24-9-26.
+//
+
+#ifndef GAITMANAGER_H
+#define GAITMANAGER_H
+#include <ocs2_legged_robot/gait/GaitSchedule.h>
+#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+
+#include "CtrlComponent.h"
+
+namespace ocs2::legged_robot {
+    class GaitManager final : public SolverSynchronizedModule {
+    public:
+        GaitManager(CtrlComponent &ctrl_component,
+                    std::shared_ptr<GaitSchedule> gait_schedule_ptr);
+
+        void preSolverRun(scalar_t initTime, scalar_t finalTime,
+                          const vector_t &currentState,
+                          const ReferenceManagerInterface &referenceManager) override;
+
+        void postSolverRun(const PrimalSolution &primalSolution) override {
+        }
+
+        void init(const std::string &gait_file);
+
+    private:
+        void getTargetGait();
+
+        CtrlComponent &ctrl_component_;
+        std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
+
+        ModeSequenceTemplate target_gait_;
+        int last_command_ = 0;
+        bool gait_updated_{false};
+        bool verbose_{false};
+        std::vector<ModeSequenceTemplate> gait_list_;
+        std::vector<std::string> gait_name_list_;
+    };
+}
+
+
+#endif //GAITMANAGER_H

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h

@@ -0,0 +1,59 @@
+//
+// Created by tlab-uav on 24-9-30.
+//
+
+#ifndef TARGETMANAGER_H
+#define TARGETMANAGER_H
+
+
+#include <memory>
+#include <ocs2_mpc/SystemObservation.h>
+#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h>
+
+struct CtrlComponent;
+
+namespace ocs2::legged_robot {
+    class TargetManager {
+    public:
+        TargetManager(CtrlComponent &ctrl_component,
+                      const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
+                      const std::string& task_file,
+                      const std::string& reference_file);
+
+        ~TargetManager() = default;
+
+        void update();
+
+    private:
+        TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose,
+                                                           const SystemObservation &observation,
+                                                           const scalar_t &targetReachingTime) {
+            // desired time trajectory
+            const scalar_array_t timeTrajectory{observation.time, targetReachingTime};
+
+            // desired state trajectory
+            vector_t currentPose = observation.state.segment<6>(6);
+            currentPose(2) = command_height_;
+            currentPose(4) = 0;
+            currentPose(5) = 0;
+            vector_array_t stateTrajectory(2, vector_t::Zero(observation.state.size()));
+            stateTrajectory[0] << vector_t::Zero(6), currentPose, default_joint_state_;
+            stateTrajectory[1] << vector_t::Zero(6), targetPose, default_joint_state_;
+
+            // desired input trajectory (just right dimensions, they are not used)
+            const vector_array_t inputTrajectory(2, vector_t::Zero(observation.input.size()));
+
+            return {timeTrajectory, stateTrajectory, inputTrajectory};
+        }
+        CtrlComponent &ctrl_component_;
+        std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
+
+        vector_t default_joint_state_{};
+        scalar_t command_height_{};
+        scalar_t time_to_target_{};
+        scalar_t target_displacement_velocity_;
+        scalar_t target_rotation_velocity_;
+    };
+}
+
+#endif //TARGETMANAGER_H

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h

@@ -0,0 +1,33 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+#pragma once
+
+#include "StateEstimateBase.h"
+
+#include <realtime_tools/realtime_tools/realtime_buffer.h>
+
+#pragma once
+namespace ocs2::legged_robot {
+    using namespace ocs2;
+
+    class FromTopicStateEstimate : public StateEstimateBase {
+    public:
+        FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
+                               const PinocchioEndEffectorKinematics &eeKinematics);
+
+        void updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal,
+                       const vector3_t &linearAccelLocal,
+                       const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance,
+                       const matrix3_t &linearAccelCovariance) override {
+        };
+
+        vector_t update(const ros::Time &time, const ros::Duration &period) override;
+
+    private:
+        void callback(const nav_msgs::Odometry::ConstPtr &msg);
+
+        ros::Subscriber sub_;
+        realtime_tools::RealtimeBuffer<nav_msgs::Odometry> buffer_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h

@@ -0,0 +1,48 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+
+#pragma once
+
+#include "StateEstimateBase.h"
+
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace ocs2::legged_robot {
+    class KalmanFilterEstimate final : public StateEstimateBase {
+    public:
+        KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
+                             const PinocchioEndEffectorKinematics &ee_kinematics,
+                             CtrlComponent &ctrl_component,
+                             const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
+
+        vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+        void loadSettings(const std::string &taskFile, bool verbose);
+
+    protected:
+        nav_msgs::msg::Odometry getOdomMsg();
+
+        vector_t feetHeights_;
+
+        // Config
+        scalar_t footRadius_ = 0.02;
+        scalar_t imuProcessNoisePosition_ = 0.02;
+        scalar_t imuProcessNoiseVelocity_ = 0.02;
+        scalar_t footProcessNoisePosition_ = 0.002;
+        scalar_t footSensorNoisePosition_ = 0.005;
+        scalar_t footSensorNoiseVelocity_ = 0.1;
+        scalar_t footHeightSensorNoise_ = 0.01;
+
+    private:
+        size_t numContacts_, dimContacts_, numState_, numObserve_;
+
+        matrix_t a_, b_, c_, q_, p_, r_;
+        vector_t xHat_, ps_, vs_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h

@@ -0,0 +1,83 @@
+//
+// Created by qiayuan on 2021/11/15.
+//
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <realtime_tools/realtime_tools/realtime_publisher.h>
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_legged_robot/common/ModelSettings.h>
+#include <ocs2_legged_robot/common/Types.h>
+#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+
+struct CtrlComponent;
+
+namespace ocs2::legged_robot {
+    class StateEstimateBase {
+    public:
+        virtual ~StateEstimateBase() = default;
+
+        StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
+                          const PinocchioEndEffectorKinematics &ee_kinematics,
+                          CtrlComponent &ctrl_component,
+                          rclcpp_lifecycle::LifecycleNode::SharedPtr node);
+
+        virtual void updateJointStates();
+
+        virtual void updateContact();
+
+        virtual void updateImu();
+
+        virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0;
+
+        size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); }
+
+    protected:
+        void updateAngular(const vector3_t &zyx, const vector_t &angularVel);
+
+        void updateLinear(const vector_t &pos, const vector_t &linearVel);
+
+        void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
+
+        CtrlComponent &ctrl_component_;
+
+        PinocchioInterface pinocchio_interface_;
+        CentroidalModelInfo info_;
+        std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
+
+        vector3_t zyx_offset_ = vector3_t::Zero();
+        vector_t rbd_state_;
+        contact_flag_t contact_flag_{};
+        Eigen::Quaternion<scalar_t> quat_;
+        vector3_t angular_vel_local_, linear_accel_local_;
+        matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
+
+        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+        rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+        rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+    };
+
+    template<typename T>
+    T square(T a) {
+        return a * a;
+    }
+
+    template<typename SCALAR_T>
+    Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T> &q) {
+        Eigen::Matrix<SCALAR_T, 3, 1> zyx;
+
+        SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999);
+        zyx(0) = std::atan2(2 * (q.x() * q.y() + q.w() * q.z()),
+                            square(q.w()) + square(q.x()) - square(q.y()) - square(q.z()));
+        zyx(1) = std::asin(as);
+        zyx(2) = std::atan2(2 * (q.y() * q.z() + q.w() * q.x()),
+                            square(q.w()) - square(q.x()) - square(q.y()) + square(q.z()));
+        return zyx;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h

@@ -0,0 +1,127 @@
+#pragma once
+#pragma clang diagnostic push
+#pragma ide diagnostic ignored "misc-non-private-member-variables-in-classes"
+//
+// Created by qiayuan on 2022/7/16.
+//
+
+#pragma once
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/Types.h>
+#include <ocs2_core/initialization/Initializer.h>
+#include <ocs2_core/penalties/Penalties.h>
+#include <ocs2_ddp/DDP_Settings.h>
+#include <ocs2_ipm/IpmSettings.h>
+#include <ocs2_legged_robot/common/ModelSettings.h>
+#include <ocs2_mpc/MPC_Settings.h>
+#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
+#include <ocs2_pinocchio_interface/PinocchioInterface.h>
+#include <ocs2_robotic_tools/common/RobotInterface.h>
+#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+#include <ocs2_self_collision/PinocchioGeometryInterface.h>
+#include <ocs2_sqp/SqpSettings.h>
+
+#include "SwitchedModelReferenceManager.h"
+
+namespace ocs2::legged_robot {
+    class LeggedInterface : public RobotInterface {
+    public:
+        LeggedInterface(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file,
+                        bool use_hard_friction_cone_constraint = false);
+
+        ~LeggedInterface() override = default;
+
+        virtual void setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
+                                                const std::string &reference_file,
+                                                bool verbose);
+
+        const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; }
+
+        const ModelSettings &modelSettings() const { return model_settings_; }
+        const ddp::Settings &ddpSettings() const { return ddp_settings_; }
+        const mpc::Settings &mpcSettings() const { return mpc_settings_; }
+        const rollout::Settings &rolloutSettings() const { return rollout_settings_; }
+        const sqp::Settings &sqpSettings() { return sqp_settings_; }
+        const ipm::Settings &ipmSettings() { return ipm_settings_; }
+
+        const vector_t &getInitialState() const { return initial_state_; }
+        const RolloutBase &getRollout() const { return *rollout_ptr_; }
+        PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; }
+        const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; }
+        PinocchioGeometryInterface &getGeometryInterface() { return *geometry_interface_ptr_; }
+
+        std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const {
+            return reference_manager_ptr_;
+        }
+
+        const Initializer &getInitializer() const override { return *initializer_ptr_; }
+
+        std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override {
+            return reference_manager_ptr_;
+        }
+
+    protected:
+        virtual void setupModel(const std::string &task_file, const std::string &urdf_file,
+                                const std::string &reference_file);
+
+        virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
+                                           const std::string &referenceFile,
+                                           bool verbose);
+
+        virtual void setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
+                                         const std::string &referenceFile,
+                                         bool verbose);
+
+        std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;
+
+        std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile,
+                                                            const CentroidalModelInfo &info, bool verbose);
+
+        matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info);
+
+        static std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(
+            const std::string &taskFile, bool verbose);
+
+        std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex,
+                                                                        scalar_t frictionCoefficient);
+
+        std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex,
+                                                                      scalar_t frictionCoefficient,
+                                                                      const RelaxedBarrierPenalty::Config &
+                                                                      barrierPenaltyConfig);
+
+        std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &foot_names,
+                                                                             const std::string &model_name);
+
+        std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
+            const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
+            size_t contact_point_index);
+
+        std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
+                                                              const std::string &taskFile,
+                                                              const std::string &prefix, bool verbose);
+
+        ModelSettings model_settings_;
+        mpc::Settings mpc_settings_;
+        ddp::Settings ddp_settings_;
+        sqp::Settings sqp_settings_;
+        ipm::Settings ipm_settings_;
+        const bool use_hard_friction_cone_constraint_;
+
+        std::unique_ptr<PinocchioInterface> pinocchio_interface_ptr_;
+        CentroidalModelInfo centroidal_model_info_;
+        std::unique_ptr<PinocchioGeometryInterface> geometry_interface_ptr_;
+
+        std::unique_ptr<OptimalControlProblem> problem_ptr_;
+        std::shared_ptr<SwitchedModelReferenceManager> reference_manager_ptr_;
+
+        rollout::Settings rollout_settings_;
+        std::unique_ptr<RolloutBase> rollout_ptr_;
+        std::unique_ptr<Initializer> initializer_ptr_;
+
+        vector_t initial_state_;
+    };
+} // namespace legged
+
+#pragma clang diagnostic pop

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h

@@ -0,0 +1,76 @@
+/******************************************************************************
+Copyright (c) 2020, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <memory>
+
+#include <ocs2_core/PreComputation.h>
+#include <ocs2_pinocchio_interface/PinocchioInterface.h>
+
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+
+#include <ocs2_legged_robot/common/ModelSettings.h>
+
+#include "constraint/EndEffectorLinearConstraint.h"
+#include "constraint/SwingTrajectoryPlanner.h"
+
+namespace ocs2::legged_robot {
+    /** Callback for caching and reference update */
+    class LeggedRobotPreComputation : public PreComputation {
+    public:
+        LeggedRobotPreComputation(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
+                                  const SwingTrajectoryPlanner &swingTrajectoryPlanner, ModelSettings settings);
+
+        ~LeggedRobotPreComputation() override = default;
+
+        LeggedRobotPreComputation *clone() const override { return new LeggedRobotPreComputation(*this); }
+
+        void request(RequestSet request, scalar_t t, const vector_t &x, const vector_t &u) override;
+
+        const std::vector<EndEffectorLinearConstraint::Config> &getEeNormalVelocityConstraintConfigs() const {
+            return eeNormalVelConConfigs_;
+        }
+
+        PinocchioInterface &getPinocchioInterface() { return pinocchioInterface_; }
+        const PinocchioInterface &getPinocchioInterface() const { return pinocchioInterface_; }
+
+    protected:
+        LeggedRobotPreComputation(const LeggedRobotPreComputation &other);
+
+    private:
+        PinocchioInterface pinocchioInterface_;
+        CentroidalModelInfo info_;
+        const SwingTrajectoryPlanner *swingTrajectoryPlannerPtr_;
+        std::unique_ptr<CentroidalModelPinocchioMapping> mappingPtr_;
+        const ModelSettings settings_;
+
+        std::vector<EndEffectorLinearConstraint::Config> eeNormalVelConConfigs_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h

@@ -0,0 +1,67 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_core/thread_support/Synchronized.h>
+#include <ocs2_oc/synchronized_module/ReferenceManager.h>
+
+#include <ocs2_legged_robot/gait/GaitSchedule.h>
+#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
+
+#include "constraint/SwingTrajectoryPlanner.h"
+
+namespace ocs2::legged_robot {
+    /**
+    * Manages the ModeSchedule and the TargetTrajectories for switched model.
+    */
+    class SwitchedModelReferenceManager : public ReferenceManager {
+    public:
+        SwitchedModelReferenceManager(std::shared_ptr<GaitSchedule> gaitSchedulePtr,
+                                      std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr);
+
+        ~SwitchedModelReferenceManager() override = default;
+
+        void setModeSchedule(const ModeSchedule &modeSchedule) override;
+
+        contact_flag_t getContactFlags(scalar_t time) const;
+
+        const std::shared_ptr<GaitSchedule> &getGaitSchedule() { return gaitSchedulePtr_; }
+
+        const std::shared_ptr<SwingTrajectoryPlanner> &getSwingTrajectoryPlanner() { return swingTrajectoryPtr_; }
+
+    protected:
+        void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t &initState,
+                              TargetTrajectories &targetTrajectories,
+                              ModeSchedule &modeSchedule) override;
+
+        std::shared_ptr<GaitSchedule> gaitSchedulePtr_;
+        std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h

@@ -0,0 +1,95 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <memory>
+
+#include <ocs2_core/constraint/StateInputConstraint.h>
+
+#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+
+namespace ocs2::legged_robot {
+    /**
+    * Defines a linear constraint on an end-effector position (xee) and linear velocity (vee).
+    * g(xee, vee) = Ax * xee + Av * vee + b
+    * - For defining constraint of type g(xee), set Av to matrix_t(0, 0)
+    * - For defining constraint of type g(vee), set Ax to matrix_t(0, 0)
+    */
+    class EndEffectorLinearConstraint final : public StateInputConstraint {
+    public:
+        /**
+         * Coefficients of the linear constraints of the form:
+         * g(xee, vee) = Ax * xee + Av * vee + b
+         */
+        struct Config {
+            vector_t b;
+            matrix_t Ax;
+            matrix_t Av;
+        };
+
+        /**
+         * Constructor
+         * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector.
+         * @param [in] numConstraints: The number of constraints {1, 2, 3}
+         * @param [in] config: The constraint coefficients, g(xee, vee) = Ax * xee + Av * vee + b
+         */
+        EndEffectorLinearConstraint(const EndEffectorKinematics<scalar_t> &endEffectorKinematics, size_t numConstraints,
+                                    Config config = Config());
+
+        ~EndEffectorLinearConstraint() override = default;
+
+        EndEffectorLinearConstraint *clone() const override { return new EndEffectorLinearConstraint(*this); }
+
+        /** Sets a new constraint coefficients. */
+        void configure(Config &&config);
+
+        /** Sets a new constraint coefficients. */
+        void configure(const Config &config) { this->configure(Config(config)); }
+
+        /** Gets the underlying end-effector kinematics interface. */
+        EndEffectorKinematics<scalar_t> &getEndEffectorKinematics() { return *endEffectorKinematicsPtr_; }
+
+        size_t getNumConstraints(scalar_t time) const override { return numConstraints_; }
+
+        vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                          const PreComputation &preComp) const override;
+
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state,
+                                                                 const vector_t &input,
+                                                                 const PreComputation &preComp) const override;
+
+    private:
+        EndEffectorLinearConstraint(const EndEffectorLinearConstraint &rhs);
+
+        std::unique_ptr<EndEffectorKinematics<scalar_t> > endEffectorKinematicsPtr_;
+        const size_t numConstraints_;
+        Config config_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h

@@ -0,0 +1,160 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/constraint/StateInputConstraint.h>
+
+#include <ocs2_legged_robot/common/Types.h>
+
+#include "../SwitchedModelReferenceManager.h"
+
+namespace ocs2::legged_robot {
+    /**
+ * Implements the constraint h(t,x,u) >= 0
+ *
+ * frictionCoefficient * (Fz + gripperForce) - sqrt(Fx * Fx + Fy * Fy + regularization) >= 0
+ *
+ * The gripper force shifts the origin of the friction cone down in z-direction by the amount of gripping force available. This makes it
+ * possible to produce tangential forces without applying a regular normal force on that foot, or to "pull" on the foot with magnitude up to
+ * the gripping force.
+ *
+ * The regularization prevents the constraint gradient / hessian to go to infinity when Fx = Fz = 0. It also creates a parabolic safety
+ * margin to the friction cone. For example: when Fx = Fy = 0 the constraint zero-crossing will be at Fz = 1/frictionCoefficient *
+ * sqrt(regularization) instead of Fz = 0
+ *
+ */
+    class FrictionConeConstraint final : public StateInputConstraint {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /**
+   * frictionCoefficient: The coefficient of friction.
+   * regularization: A positive number to regulize the friction constraint. refer to the FrictionConeConstraint documentation.
+   * gripperForce: Gripper force in normal direction.
+   * hessianDiagonalShift: The Hessian shift to assure a strictly-convex quadratic constraint approximation.
+   */
+        struct Config {
+            explicit Config(scalar_t frictionCoefficientParam = 0.7, scalar_t regularizationParam = 25.0,
+                            scalar_t gripperForceParam = 0.0,
+                            scalar_t hessianDiagonalShiftParam = 1e-6)
+                : frictionCoefficient(frictionCoefficientParam),
+                  regularization(regularizationParam),
+                  gripperForce(gripperForceParam),
+                  hessianDiagonalShift(hessianDiagonalShiftParam) {
+                assert(frictionCoefficient > 0.0);
+                assert(regularization > 0.0);
+                assert(hessianDiagonalShift >= 0.0);
+            }
+
+            scalar_t frictionCoefficient;
+            scalar_t regularization;
+            scalar_t gripperForce;
+            scalar_t hessianDiagonalShift;
+        };
+
+        /**
+   * Constructor
+   * @param [in] referenceManager : Switched model ReferenceManager.
+   * @param [in] config : Friction model settings.
+   * @param [in] contactPointIndex : The 3 DoF contact index.
+   * @param [in] info : The centroidal model information.
+   */
+        FrictionConeConstraint(const SwitchedModelReferenceManager &referenceManager, Config config,
+                               size_t contactPointIndex,
+                               CentroidalModelInfo info);
+
+        ~FrictionConeConstraint() override = default;
+
+        FrictionConeConstraint *clone() const override { return new FrictionConeConstraint(*this); }
+
+        bool isActive(scalar_t time) const override;
+
+        size_t getNumConstraints(scalar_t time) const override { return 1; };
+
+        vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                          const PreComputation &preComp) const override;
+
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state,
+                                                                 const vector_t &input,
+                                                                 const PreComputation &preComp) const override;
+
+        VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t &state,
+                                                                       const vector_t &input,
+                                                                       const PreComputation &preComp) const override;
+
+        /** Sets the estimated terrain normal expressed in the world frame. */
+        void setSurfaceNormalInWorld(const vector3_t &surfaceNormalInWorld);
+
+    private:
+        struct LocalForceDerivatives {
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+            matrix3_t dF_du; // derivative local force w.r.t. forces in world frame
+        };
+
+        struct ConeLocalDerivatives {
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+            vector3_t dCone_dF; // derivative w.r.t local force
+            matrix3_t d2Cone_dF2; // second derivative w.r.t local force
+        };
+
+        struct ConeDerivatives {
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+            vector3_t dCone_du;
+            matrix3_t d2Cone_du2;
+        };
+
+        FrictionConeConstraint(const FrictionConeConstraint &other) = default;
+
+        vector_t coneConstraint(const vector3_t &localForces) const;
+
+        LocalForceDerivatives computeLocalForceDerivatives(const vector3_t &forcesInBodyFrame) const;
+
+        ConeLocalDerivatives computeConeLocalDerivatives(const vector3_t &localForces) const;
+
+        ConeDerivatives computeConeConstraintDerivatives(const ConeLocalDerivatives &coneLocalDerivatives,
+                                                         const LocalForceDerivatives &localForceDerivatives) const;
+
+        matrix_t frictionConeInputDerivative(size_t inputDim, const ConeDerivatives &coneDerivatives) const;
+
+        matrix_t frictionConeSecondDerivativeInput(size_t inputDim, const ConeDerivatives &coneDerivatives) const;
+
+        matrix_t frictionConeSecondDerivativeState(size_t stateDim, const ConeDerivatives &coneDerivatives) const;
+
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+
+        const Config config_;
+        const size_t contactPointIndex_;
+        const CentroidalModelInfo info_;
+
+        // rotation world to terrain
+        matrix3_t t_R_w = matrix3_t::Identity();
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h

@@ -0,0 +1,30 @@
+//
+// Created by qiayuan on 23-1-29.
+//
+
+#pragma once
+
+#include <ocs2_self_collision/SelfCollisionConstraint.h>
+
+#include "../LeggedRobotPreComputation.h"
+
+namespace ocs2::legged_robot {
+    class LeggedSelfCollisionConstraint final : public SelfCollisionConstraint {
+    public:
+        LeggedSelfCollisionConstraint(const CentroidalModelPinocchioMapping &mapping,
+                                      PinocchioGeometryInterface pinocchioGeometryInterface,
+                                      scalar_t minimumDistance)
+            : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface), minimumDistance) {
+        }
+
+        ~LeggedSelfCollisionConstraint() override = default;
+
+        LeggedSelfCollisionConstraint(const LeggedSelfCollisionConstraint &other) = default;
+
+        LeggedSelfCollisionConstraint *clone() const override { return new LeggedSelfCollisionConstraint(*this); }
+
+        const PinocchioInterface &getPinocchioInterface(const PreComputation &preComputation) const override {
+            return cast<LeggedRobotPreComputation>(preComputation).getPinocchioInterface();
+        }
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h

@@ -0,0 +1,78 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_core/constraint/StateInputConstraint.h>
+
+#include "../SwitchedModelReferenceManager.h"
+#include "EndEffectorLinearConstraint.h"
+
+namespace ocs2::legged_robot {
+    /**
+    * Specializes the CppAd version of normal velocity constraint on an end-effector position and linear velocity.
+    * Constructs the member EndEffectorLinearConstraint object with number of constraints of 1.
+    *
+    * See also EndEffectorLinearConstraint for the underlying computation.
+    */
+    class NormalVelocityConstraintCppAd final : public StateInputConstraint {
+    public:
+        /**
+         * Constructor
+         * @param [in] referenceManager : Switched model ReferenceManager
+         * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector.
+         * @param [in] contactPointIndex : The 3 DoF contact index.
+         */
+        NormalVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager,
+                                      const EndEffectorKinematics<scalar_t> &endEffectorKinematics,
+                                      size_t contactPointIndex);
+
+        ~NormalVelocityConstraintCppAd() override = default;
+
+        NormalVelocityConstraintCppAd *clone() const override { return new NormalVelocityConstraintCppAd(*this); }
+
+        bool isActive(scalar_t time) const override;
+
+        size_t getNumConstraints(scalar_t time) const override { return 1; }
+
+        vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                          const PreComputation &preComp) const override;
+
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state,
+                                                                 const vector_t &input,
+                                                                 const PreComputation &preComp) const override;
+
+    private:
+        NormalVelocityConstraintCppAd(const NormalVelocityConstraintCppAd &rhs);
+
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+        std::unique_ptr<EndEffectorLinearConstraint> eeLinearConstraintPtr_;
+        const size_t contactPointIndex_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h

@@ -0,0 +1,118 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_core/reference/ModeSchedule.h>
+
+#include <ocs2_legged_robot/common/Types.h>
+#include <ocs2_legged_robot/foot_planner/SplineCpg.h>
+
+namespace ocs2::legged_robot {
+    class SwingTrajectoryPlanner {
+    public:
+        struct Config {
+            scalar_t liftOffVelocity = 0.0;
+            scalar_t touchDownVelocity = 0.0;
+            scalar_t swingHeight = 0.1;
+            scalar_t swingTimeScale = 0.15;
+            // swing phases shorter than this time will be scaled down in height and velocity
+        };
+
+        SwingTrajectoryPlanner(Config config, size_t numFeet);
+
+        void update(const ModeSchedule &modeSchedule, scalar_t terrainHeight);
+
+        void update(const ModeSchedule &modeSchedule, const feet_array_t<scalar_array_t> &liftOffHeightSequence,
+                    const feet_array_t<scalar_array_t> &touchDownHeightSequence);
+
+        void update(const ModeSchedule &modeSchedule, const feet_array_t<scalar_array_t> &liftOffHeightSequence,
+                    const feet_array_t<scalar_array_t> &touchDownHeightSequence,
+                    const feet_array_t<scalar_array_t> &maxHeightSequence);
+
+        scalar_t getZvelocityConstraint(size_t leg, scalar_t time) const;
+
+        scalar_t getZpositionConstraint(size_t leg, scalar_t time) const;
+
+    private:
+        /**
+         * Extracts for each leg the contact sequence over the motion phase sequence.
+         * @param phaseIDsStock
+         * @return contactFlagStock
+         */
+        feet_array_t<std::vector<bool> > extractContactFlags(const std::vector<size_t> &phaseIDsStock) const;
+
+        /**
+         * Finds the take-off and touch-down times indices for a specific leg.
+         *
+         * @param index
+         * @param contactFlagStock
+         * @return {The take-off time index for swing legs, touch-down time index for swing legs}
+         */
+        static std::pair<int, int> findIndex(size_t index, const std::vector<bool> &contactFlagStock);
+
+        /**
+         * based on the input phaseIDsStock finds the start subsystem and final subsystem of the swing
+         * phases of the a foot in each subsystem.
+         *
+         * startTimeIndexStock: eventTimes[startTimesIndex] will be the take-off time for the requested leg.
+         * finalTimeIndexStock: eventTimes[finalTimesIndex] will be the touch-down time for the requested leg.
+         *
+         * @param [in] footIndex: Foot index
+         * @param [in] phaseIDsStock: The sequence of the motion phase IDs.
+         * @param [in] contactFlagStock: The sequence of the contact status for the requested leg.
+         * @return { startTimeIndexStock, finalTimeIndexStock}
+         */
+        static std::pair<std::vector<int>, std::vector<int> > updateFootSchedule(
+            const std::vector<bool> &contactFlagStock);
+
+        /**
+         * Check if event time indices are valid
+         * @param leg
+         * @param index : phase index
+         * @param startIndex : liftoff event time index
+         * @param finalIndex : touchdown event time index
+         * @param phaseIDsStock : mode sequence
+         */
+        static void checkThatIndicesAreValid(int leg, int index, int startIndex, int finalIndex,
+                                             const std::vector<size_t> &phaseIDsStock);
+
+        static scalar_t swingTrajectoryScaling(scalar_t startTime, scalar_t finalTime, scalar_t swingTimeScale);
+
+        const Config config_;
+        const size_t numFeet_;
+
+        feet_array_t<std::vector<SplineCpg> > feetHeightTrajectories_;
+        feet_array_t<std::vector<scalar_t> > feetHeightTrajectoriesEvents_;
+    };
+
+    SwingTrajectoryPlanner::Config loadSwingTrajectorySettings(const std::string &fileName,
+                                                               const std::string &fieldName = "swing_trajectory_config",
+                                                               bool verbose = true);
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h

@@ -0,0 +1,71 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/constraint/StateInputConstraint.h>
+
+#include "../SwitchedModelReferenceManager.h"
+
+namespace ocs2::legged_robot {
+    class ZeroForceConstraint final : public StateInputConstraint {
+    public:
+        /*
+         * Constructor
+         * @param [in] referenceManager : Switched model ReferenceManager.
+         * @param [in] contactPointIndex : The 3 DoF contact index.
+         * @param [in] info : The centroidal model information.
+         */
+        ZeroForceConstraint(const SwitchedModelReferenceManager &referenceManager, size_t contactPointIndex,
+                            CentroidalModelInfo info);
+
+        ~ZeroForceConstraint() override = default;
+
+        ZeroForceConstraint *clone() const override { return new ZeroForceConstraint(*this); }
+
+        bool isActive(scalar_t time) const override;
+
+        size_t getNumConstraints(scalar_t time) const override { return 3; }
+
+        vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                          const PreComputation &preComp) const override;
+
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state,
+                                                                 const vector_t &input,
+                                                                 const PreComputation &preComp) const override;
+
+    private:
+        ZeroForceConstraint(const ZeroForceConstraint &other) = default;
+
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+        const size_t contactPointIndex_;
+        const CentroidalModelInfo info_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h

@@ -0,0 +1,80 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_core/constraint/StateInputConstraint.h>
+
+#include "../SwitchedModelReferenceManager.h"
+#include "EndEffectorLinearConstraint.h"
+
+namespace ocs2::legged_robot {
+    /**
+    * Specializes the CppAd version of zero velocity constraint on an end-effector position and linear velocity.
+    * Constructs the member EndEffectorLinearConstraint object with number of constraints of 3.
+    *
+    * See also EndEffectorLinearConstraint for the underlying computation.
+    */
+    class ZeroVelocityConstraintCppAd final : public StateInputConstraint {
+    public:
+        /**
+         * Constructor
+         * @param [in] referenceManager : Switched model ReferenceManager
+         * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector.
+         * @param [in] contactPointIndex : The 3 DoF contact index.
+         * @param [in] config: The constraint coefficients
+         */
+        ZeroVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager,
+                                    const EndEffectorKinematics<scalar_t> &endEffectorKinematics,
+                                    size_t contactPointIndex,
+                                    EndEffectorLinearConstraint::Config config = EndEffectorLinearConstraint::Config());
+
+        ~ZeroVelocityConstraintCppAd() override = default;
+
+        ZeroVelocityConstraintCppAd *clone() const override { return new ZeroVelocityConstraintCppAd(*this); }
+
+        bool isActive(scalar_t time) const override;
+
+        size_t getNumConstraints(scalar_t time) const override { return 3; }
+
+        vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                          const PreComputation &preComp) const override;
+
+        VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state,
+                                                                 const vector_t &input,
+                                                                 const PreComputation &preComp) const override;
+
+    private:
+        ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd &rhs);
+
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+        std::unique_ptr<EndEffectorLinearConstraint> eeLinearConstraintPtr_;
+        const size_t contactPointIndex_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h

@@ -0,0 +1,101 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/cost/QuadraticStateCost.h>
+#include <ocs2_core/cost/QuadraticStateInputCost.h>
+#include <ocs2_legged_robot/common/utils.h>
+
+#include "../SwitchedModelReferenceManager.h"
+
+namespace ocs2::legged_robot {
+    /**
+         * State-input tracking cost used for intermediate times
+         */
+    class LeggedRobotStateInputQuadraticCost final : public QuadraticStateInputCost {
+    public:
+        LeggedRobotStateInputQuadraticCost(matrix_t Q, matrix_t R, CentroidalModelInfo info,
+                                           const SwitchedModelReferenceManager &referenceManager)
+            : QuadraticStateInputCost(std::move(Q), std::move(R)), info_(std::move(info)),
+              referenceManagerPtr_(&referenceManager) {
+        }
+
+        ~LeggedRobotStateInputQuadraticCost() override = default;
+
+        LeggedRobotStateInputQuadraticCost *clone() const override {
+            return new LeggedRobotStateInputQuadraticCost(*this);
+        }
+
+    private:
+        LeggedRobotStateInputQuadraticCost(const LeggedRobotStateInputQuadraticCost &rhs) = default;
+
+        std::pair<vector_t, vector_t> getStateInputDeviation(scalar_t time, const vector_t &state,
+                                                             const vector_t &input,
+                                                             const TargetTrajectories &targetTrajectories)
+        const override {
+            const auto contactFlags = referenceManagerPtr_->getContactFlags(time);
+            const vector_t xNominal = targetTrajectories.getDesiredState(time);
+            const vector_t uNominal = weightCompensatingInput(info_, contactFlags);
+            return {state - xNominal, input - uNominal};
+        }
+
+        const CentroidalModelInfo info_;
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+    };
+
+    /**
+         * State tracking cost used for the final time
+         */
+    class LeggedRobotStateQuadraticCost final : public QuadraticStateCost {
+    public:
+        LeggedRobotStateQuadraticCost(matrix_t Q, CentroidalModelInfo info,
+                                      const SwitchedModelReferenceManager &referenceManager)
+            : QuadraticStateCost(std::move(Q)), info_(std::move(info)), referenceManagerPtr_(&referenceManager) {
+        }
+
+        ~LeggedRobotStateQuadraticCost() override = default;
+
+        LeggedRobotStateQuadraticCost *clone() const override { return new LeggedRobotStateQuadraticCost(*this); }
+
+    private:
+        LeggedRobotStateQuadraticCost(const LeggedRobotStateQuadraticCost &rhs) = default;
+
+        vector_t getStateDeviation(scalar_t time, const vector_t &state,
+                                   const TargetTrajectories &targetTrajectories) const override {
+            const auto contactFlags = referenceManagerPtr_->getContactFlags(time);
+            const vector_t xNominal = targetTrajectories.getDesiredState(time);
+            return state - xNominal;
+        }
+
+        const CentroidalModelInfo info_;
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h

@@ -0,0 +1,63 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#pragma once
+
+#include <ocs2_centroidal_model/CentroidalModelInfo.h>
+#include <ocs2_core/initialization/Initializer.h>
+
+#include "../SwitchedModelReferenceManager.h"
+
+namespace ocs2::legged_robot {
+    class LeggedRobotInitializer final : public Initializer {
+    public:
+        /*
+         * Constructor
+         * @param [in] info : The centroidal model information.
+         * @param [in] referenceManager : Switched system reference manager.
+         * @param [in] extendNormalizedMomentum: If true, it extrapolates the normalized momenta; otherwise sets them to zero.
+         */
+        LeggedRobotInitializer(CentroidalModelInfo info, const SwitchedModelReferenceManager &referenceManager,
+                               bool extendNormalizedMomentum = false);
+
+        ~LeggedRobotInitializer() override = default;
+
+        LeggedRobotInitializer *clone() const override;
+
+        void compute(scalar_t time, const vector_t &state, scalar_t nextTime, vector_t &input,
+                     vector_t &nextState) override;
+
+    private:
+        LeggedRobotInitializer(const LeggedRobotInitializer &other) = default;
+
+        const CentroidalModelInfo info_;
+        const SwitchedModelReferenceManager *referenceManagerPtr_;
+        const bool extendNormalizedMomentum_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h

@@ -0,0 +1,17 @@
+//
+// Created by qiayuan on 22-12-23.
+//
+#pragma once
+
+#include "WbcBase.h"
+
+namespace ocs2::legged_robot {
+    class HierarchicalWbc final : public WbcBase {
+    public:
+        using WbcBase::WbcBase;
+
+        vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured,
+                        size_t mode,
+                        scalar_t period) override;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h

@@ -0,0 +1,76 @@
+//
+// Created by qiayuan on 2022/6/28.
+//
+//
+// Ref: https://github.com/bernhardpg/quadruped_locomotion
+//
+
+#pragma once
+
+#include "Task.h"
+
+#include <memory>
+
+namespace ocs2::legged_robot{
+    // Hierarchical Optimization Quadratic Program
+    class HoQp {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        using HoQpPtr = std::shared_ptr<HoQp>;
+
+        explicit HoQp(const Task &task) : HoQp(task, nullptr) {
+        }
+
+        HoQp(Task task, HoQpPtr higherProblem);
+
+        matrix_t getStackedZMatrix() const { return stackedZ_; }
+
+        Task getStackedTasks() const { return stackedTasks_; }
+
+        vector_t getStackedSlackSolutions() const { return stackedSlackVars_; }
+
+        vector_t getSolutions() const {
+            vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_;
+            return x;
+        }
+
+        size_t getSlackedNumVars() const { return stackedTasks_.d_.rows(); }
+
+    private:
+        void initVars();
+
+        void formulateProblem();
+
+        void solveProblem();
+
+        void buildHMatrix();
+
+        void buildCVector();
+
+        void buildDMatrix();
+
+        void buildFVector();
+
+        void buildZMatrix();
+
+        void stackSlackSolutions();
+
+        Task task_, stackedTasksPrev_, stackedTasks_;
+        HoQpPtr higherProblem_;
+
+        bool hasEqConstraints_{}, hasIneqConstraints_{};
+        size_t numSlackVars_{}, numDecisionVars_{};
+        matrix_t stackedZPrev_, stackedZ_;
+        vector_t stackedSlackSolutionsPrev_, xPrev_;
+        size_t numPrevSlackVars_{};
+
+        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> h_, d_;
+        vector_t c_, f_;
+        vector_t stackedSlackVars_, slackVarsSolutions_, decisionVarsSolutions_;
+
+        // Convenience matrices that are used multiple times
+        matrix_t eyeNvNv_;
+        matrix_t zeroNvNx_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h

@@ -0,0 +1,77 @@
+//
+// Created by qiayuan on 2022/6/28.
+//
+//
+// Ref: https://github.com/bernhardpg/quadruped_locomotion
+//
+
+#pragma once
+
+#include <ocs2_core/Types.h>
+
+#include <utility>
+
+namespace ocs2::legged_robot {
+
+    class Task {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        Task() = default;
+
+        Task(matrix_t a, vector_t b, matrix_t d, vector_t f) : a_(std::move(a)), d_(std::move(d)), b_(std::move(b)),
+                                                               f_(std::move(f)) {
+        }
+
+        explicit Task(size_t numDecisionVars)
+            : Task(matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0), matrix_t::Zero(0, numDecisionVars),
+                   vector_t::Zero(0)) {
+        }
+
+        Task operator+(const Task &rhs) const {
+            return {
+                concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_),
+                concatenateVectors(f_, rhs.f_)
+            };
+        }
+
+        Task operator*(scalar_t rhs) const {
+            // clang-format off
+            return {
+                a_.cols() > 0 ? rhs * a_ : a_,
+                b_.cols() > 0 ? rhs * b_ : b_,
+                d_.cols() > 0 ? rhs * d_ : d_,
+                f_.cols() > 0 ? rhs * f_ : f_
+            }; // clang-format on
+        }
+
+        matrix_t a_, d_;
+        vector_t b_, f_;
+
+        static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) {
+            if (m1.cols() <= 0) {
+                return m2;
+            }
+            if (m2.cols() <= 0) {
+                return m1;
+            }
+            assert(m1.cols() == m2.cols());
+            matrix_t res(m1.rows() + m2.rows(), m1.cols());
+            res << m1, m2;
+            return res;
+        }
+
+        static vector_t concatenateVectors(const vector_t &v1, const vector_t &v2) {
+            if (v1.cols() <= 0) {
+                return v2;
+            }
+            if (v2.cols() <= 0) {
+                return v1;
+            }
+            assert(v1.cols() == v2.cols());
+            vector_t res(v1.rows() + v2.rows());
+            res << v1, v2;
+            return res;
+        }
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h

@@ -0,0 +1,68 @@
+//
+// Created by qiayuan on 2022/7/1.
+//
+
+#pragma once
+
+#include "Task.h"
+
+#include <ocs2_centroidal_model/PinocchioCentroidalDynamics.h>
+#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
+
+namespace ocs2::legged_robot {
+    // Decision Variables: x = [\dot u^T, F^T, \tau^T]^T
+    class WbcBase {
+        using Vector6 = Eigen::Matrix<scalar_t, 6, 1>;
+        using Matrix6 = Eigen::Matrix<scalar_t, 6, 6>;
+
+    public:
+        virtual ~WbcBase() = default;
+
+        WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
+                const PinocchioEndEffectorKinematics &eeKinematics);
+
+        virtual void loadTasksSetting(const std::string &taskFile, bool verbose);
+
+        virtual vector_t update(const vector_t &stateDesired, const vector_t &inputDesired,
+                                const vector_t &rbdStateMeasured, size_t mode,
+                                scalar_t period);
+
+    protected:
+        void updateMeasured(const vector_t &rbdStateMeasured);
+
+        void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired);
+
+        size_t getNumDecisionVars() const { return num_decision_vars_; }
+
+        Task formulateFloatingBaseEomTask();
+
+        Task formulateTorqueLimitsTask();
+
+        Task formulateNoContactMotionTask();
+
+        Task formulateFrictionConeTask();
+
+        Task formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period);
+
+        Task formulateSwingLegTask();
+
+        Task formulateContactForceTask(const vector_t &inputDesired) const;
+
+        size_t num_decision_vars_;
+        PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_;
+        CentroidalModelInfo info_;
+
+        std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
+        CentroidalModelPinocchioMapping mapping_;
+
+        vector_t q_measured_, v_measured_, input_last_;
+        matrix_t j_, dj_;
+        contact_flag_t contact_flag_{};
+        size_t num_contacts_{};
+
+        // Task Parameters:
+        vector_t torque_limits_;
+        scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{};
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h

@@ -0,0 +1,28 @@
+//
+// Created by qiayuan on 22-12-23.
+//
+#pragma once
+
+#include "WbcBase.h"
+
+namespace ocs2::legged_robot {
+    class WeightedWbc : public WbcBase {
+    public:
+        using WbcBase::WbcBase;
+
+        vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured,
+                        size_t mode,
+                        scalar_t period) override;
+
+        void loadTasksSetting(const std::string &taskFile, bool verbose) override;
+
+    protected:
+        virtual Task formulateConstraints();
+
+        virtual Task formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired,
+                                            scalar_t period);
+
+    private:
+        scalar_t weightSwingLeg_, weightBaseAccel_, weightContactForce_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml

@@ -0,0 +1,9 @@
+<library path="ocs2_quadruped_controller">
+  <class name="ocs2_quadruped_controller/Ocs2QuadrupedController"
+         type="ocs2::legged_robot::Ocs2QuadrupedController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Quadruped Controller based on OCS2 Legged Control.
+    </description>
+  </class>
+</library>

controllers/ocs2_quadruped_controller/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ocs2_quadruped_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>control_input_msgs</depend>
+  <depend>qpoases_colcon</depend>
+  <depend>ocs2_self_collision</depend>
+  <depend>angles</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -0,0 +1,347 @@
+//
+// Created by tlab-uav on 24-9-24.
+//
+
+#include "Ocs2QuadrupedController.h"
+
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_core/thread_support/ExecuteAndSleep.h>
+#include <ocs2_core/thread_support/SetThreadPriority.h>
+#include <ocs2_legged_robot_ros/gait/GaitReceiver.h>
+#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
+#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
+#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
+#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
+#include <ocs2_sqp/SqpMpc.h>
+#include <angles/angles.h>
+#include <ocs2_quadruped_controller/control/GaitManager.h>
+
+namespace ocs2::legged_robot {
+    using config_type = controller_interface::interface_configuration_type;
+
+    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const {
+        controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+        conf.names.reserve(joint_names_.size() * command_interface_types_.size());
+        for (const auto &joint_name: joint_names_) {
+            for (const auto &interface_type: command_interface_types_) {
+                conf.names.push_back(joint_name + "/" += interface_type);
+            }
+        }
+
+        return conf;
+    }
+
+    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const {
+        controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+        conf.names.reserve(joint_names_.size() * state_interface_types_.size());
+        for (const auto &joint_name: joint_names_) {
+            for (const auto &interface_type: state_interface_types_) {
+                conf.names.push_back(joint_name + "/" += interface_type);
+            }
+        }
+
+        for (const auto &interface_type: imu_interface_types_) {
+            conf.names.push_back(imu_name_ + "/" += interface_type);
+        }
+
+        for (const auto &interface_type: foot_force_interface_types_) {
+            conf.names.push_back(foot_force_name_ + "/" += interface_type);
+        }
+
+        return conf;
+    }
+
+    controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
+                                                                      const rclcpp::Duration &period) {
+        // State Estimate
+        updateStateEstimation(time, period);
+
+        // Compute target trajectory
+        ctrl_comp_.target_manager_->update();
+
+        // Update the current state of the system
+        mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
+
+        // Load the latest MPC policy
+        mpc_mrt_interface_->updatePolicy();
+
+        // Evaluate the current policy
+        vector_t optimized_state, optimized_input;
+        size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
+        mpc_mrt_interface_->evaluatePolicy(ctrl_comp_.observation_.time, ctrl_comp_.observation_.state, optimized_state,
+                                           optimized_input, planned_mode);
+
+        // Whole body control
+        ctrl_comp_.observation_.input = optimized_input;
+
+        wbc_timer_.startTimer();
+        vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode,
+                                  period.seconds());
+        wbc_timer_.endTimer();
+
+        vector_t torque = x.tail(12);
+
+        vector_t pos_des = centroidal_model::getJointAngles(optimized_state,
+                                                            legged_interface_->getCentroidalModelInfo());
+        vector_t vel_des = centroidal_model::getJointVelocities(optimized_input,
+                                                                legged_interface_->getCentroidalModelInfo());
+
+        // Safety check, if failed, stop the controller
+        if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input)) {
+            RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
+            for (int i = 0; i < joint_names_.size(); i++) {
+                ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+                ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
+                ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.35);
+            }
+            return controller_interface::return_type::ERROR;
+        }
+
+        for (int i = 0; i < joint_names_.size(); i++) {
+            ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque(i));
+            ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i));
+            ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
+            ctrl_comp_.joint_kp_command_interface_[i].get().set_value(default_kp_);
+            ctrl_comp_.joint_kd_command_interface_[i].get().set_value(default_kd_);
+        }
+
+        // Visualization
+        ctrl_comp_.visualizer_->update(ctrl_comp_.observation_, mpc_mrt_interface_->getPolicy(),
+                                       mpc_mrt_interface_->getCommand());
+
+        observation_publisher_->publish(ros_msg_conversions::createObservationMsg(ctrl_comp_.observation_));
+
+        return controller_interface::return_type::OK;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
+        // Initialize OCS2
+        urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
+        task_file_ = auto_declare<std::string>("task_file", task_file_);
+        reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
+        gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
+
+        // Load verbose parameter from the task file
+        verbose_ = false;
+        loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
+
+        // Hardware Parameters
+        joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
+        command_interface_types_ =
+                auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
+        state_interface_types_ =
+                auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
+        imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
+        imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+        foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
+        foot_force_interface_types_ =
+                auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
+
+        // PD gains
+        default_kp_ = auto_declare<double>("default_kp", default_kp_);
+        default_kd_ = auto_declare<double>("default_kd", default_kd_);
+
+        setupLeggedInterface();
+        setupMpc();
+        setupMrt();
+
+        // Visualization
+        CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
+        eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
+            legged_interface_->getPinocchioInterface(), pinocchio_mapping,
+            legged_interface_->modelSettings().contactNames3DoF);
+
+        ctrl_comp_.visualizer_ = std::make_shared<LeggedRobotVisualizer>(
+            legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_,
+            get_node());
+
+        // selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
+        //                                                                        leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
+
+        // State estimation
+        setupStateEstimate();
+
+        // Whole body control
+        wbc_ = std::make_shared<WeightedWbc>(legged_interface_->getPinocchioInterface(),
+                                             legged_interface_->getCentroidalModelInfo(),
+                                             *eeKinematicsPtr_);
+        wbc_->loadTasksSetting(task_file_, verbose_);
+
+        // Safety Checker
+        safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
+            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
+                // Handle message
+                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_comp_.control_inputs_.ry = msg->ry;
+            });
+
+        observation_publisher_ = get_node()->create_publisher<ocs2_msgs::msg::MpcObservation>(
+            "legged_robot_mpc_observation", 10);
+
+        get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        // clear out vectors in case of restart
+        ctrl_comp_.clear();
+
+        // assign command interfaces
+        for (auto &interface: command_interfaces_) {
+            std::string interface_name = interface.get_interface_name();
+            if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
+                command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
+            } else {
+                command_interface_map_[interface_name]->push_back(interface);
+            }
+        }
+
+        // assign state interfaces
+        for (auto &interface: state_interfaces_) {
+            if (interface.get_prefix_name() == imu_name_) {
+                ctrl_comp_.imu_state_interface_.emplace_back(interface);
+            } else if (interface.get_prefix_name() == foot_force_name_) {
+                ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
+            } else {
+                state_interface_map_[interface.get_interface_name()]->push_back(interface);
+            }
+        }
+
+        if (mpc_running_ == false) {
+            // Initial state
+            ctrl_comp_.observation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
+            updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
+            ctrl_comp_.observation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
+            ctrl_comp_.observation_.mode = STANCE;
+
+            const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time}, {ctrl_comp_.observation_.state},
+                                                         {ctrl_comp_.observation_.input});
+
+            // Set the first observation and command and wait for optimization to finish
+            mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
+            mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
+            RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
+            while (!mpc_mrt_interface_->initialPolicyReceived()) {
+                mpc_mrt_interface_->advanceMpc();
+                rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
+            }
+            RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
+
+            mpc_running_ = true;
+        }
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        release_interfaces();
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        return CallbackReturn::SUCCESS;
+    }
+
+    void Ocs2QuadrupedController::setupLeggedInterface() {
+        legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
+        legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
+    }
+
+    void Ocs2QuadrupedController::setupMpc() {
+        mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
+                                        legged_interface_->getOptimalControlProblem(),
+                                        legged_interface_->getInitializer());
+        rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
+                                                                           legged_interface_->getCentroidalModelInfo());
+
+        // Initialize the reference manager
+        const auto gait_manager_ptr = std::make_shared<GaitManager>(
+            ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
+            getGaitSchedule());
+        gait_manager_ptr->init(gait_file_);
+        mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
+        mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
+
+        ctrl_comp_.target_manager_ = std::make_shared<TargetManager>(ctrl_comp_,
+                                                                     legged_interface_->getReferenceManagerPtr(),
+                                                                     task_file_, reference_file_);
+    }
+
+    void Ocs2QuadrupedController::setupMrt() {
+        mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
+        mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
+        mpc_timer_.reset();
+
+        controller_running_ = true;
+        mpc_thread_ = std::thread([&] {
+            while (controller_running_) {
+                try {
+                    executeAndSleep(
+                        [&] {
+                            if (mpc_running_) {
+                                mpc_timer_.startTimer();
+                                mpc_mrt_interface_->advanceMpc();
+                                mpc_timer_.endTimer();
+                            }
+                        },
+                        legged_interface_->mpcSettings().mpcDesiredFrequency_);
+                } catch (const std::exception &e) {
+                    controller_running_ = false;
+                    RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
+                }
+            }
+        });
+        setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
+        RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started.");
+    }
+
+    void Ocs2QuadrupedController::setupStateEstimate() {
+        ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
+                                                                       legged_interface_->getCentroidalModelInfo(),
+                                                                       *eeKinematicsPtr_, ctrl_comp_, this->get_node());
+        dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
+        ctrl_comp_.observation_.time = 0;
+    }
+
+    void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
+        measured_rbd_state_ = ctrl_comp_.estimator_->update(time, period);
+        ctrl_comp_.observation_.time += period.seconds();
+        const scalar_t yaw_last = ctrl_comp_.observation_.state(9);
+        ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
+        ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance(
+                                            yaw_last, ctrl_comp_.observation_.state(9));
+        ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
+    }
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface);

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -0,0 +1,137 @@
+//
+// Created by tlab-uav on 24-9-24.
+//
+
+#ifndef OCS2QUADRUPEDCONTROLLER_H
+#define OCS2QUADRUPEDCONTROLLER_H
+
+#include <controller_interface/controller_interface.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
+#include <ocs2_mpc/MPC_MRT_Interface.h>
+#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
+#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
+#include <ocs2_quadruped_controller/wbc/WbcBase.h>
+#include <ocs2_msgs/msg/mpc_observation.hpp>
+#include "SafetyChecker.h"
+#include "ocs2_quadruped_controller/control/CtrlComponent.h"
+
+namespace ocs2::legged_robot {
+    class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
+    public:
+        CONTROLLER_INTERFACE_PUBLIC
+        Ocs2QuadrupedController() = default;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::return_type update(
+            const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_init() override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_configure(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_activate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_deactivate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_cleanup(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_shutdown(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_error(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+    protected:
+        void setupLeggedInterface();
+
+        void setupMpc();
+
+        void setupMrt();
+
+        void setupStateEstimate();
+
+        void updateStateEstimation(const rclcpp::Time &time,
+                                   const rclcpp::Duration &period);
+
+        CtrlComponent ctrl_comp_;
+        std::vector<std::string> joint_names_;
+        std::vector<std::string> command_interface_types_;
+        std::vector<std::string> state_interface_types_;
+
+        std::unordered_map<
+            std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
+        command_interface_map_ = {
+            {"effort", &ctrl_comp_.joint_torque_command_interface_},
+            {"position", &ctrl_comp_.joint_position_command_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
+            {"kp", &ctrl_comp_.joint_kp_command_interface_},
+            {"kd", &ctrl_comp_.joint_kd_command_interface_}
+        };
+        std::unordered_map<
+            std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
+        state_interface_map_ = {
+            {"position", &ctrl_comp_.joint_position_state_interface_},
+            {"effort", &ctrl_comp_.joint_effort_state_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_state_interface_}
+        };
+
+        // IMU Sensor
+        std::string imu_name_;
+        std::vector<std::string> imu_interface_types_;
+        // Foot Force Sensor
+        std::string foot_force_name_;
+        std::vector<std::string> foot_force_interface_types_;
+        double default_kp_ = 0;
+        double default_kd_ = 6;
+
+        rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
+        rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
+
+        std::string task_file_;
+        std::string urdf_file_;
+        std::string reference_file_;
+        std::string gait_file_;
+
+        bool verbose_;
+
+        std::shared_ptr<LeggedInterface> legged_interface_;
+        std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
+
+        // Whole Body Control
+        std::shared_ptr<WbcBase> wbc_;
+        std::shared_ptr<SafetyChecker> safety_checker_;
+
+        // Nonlinear MPC
+        std::shared_ptr<MPC_BASE> mpc_;
+        std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
+
+        std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
+
+    private:
+        vector_t measured_rbd_state_;
+        std::thread mpc_thread_;
+        std::atomic_bool controller_running_{}, mpc_running_{};
+        benchmark::RepeatedTimer mpc_timer_;
+        benchmark::RepeatedTimer wbc_timer_;
+    };
+}
+
+#endif //OCS2QUADRUPEDCONTROLLER_H

controllers/ocs2_quadruped_controller/src/SafetyChecker.h

@@ -0,0 +1,33 @@
+//
+// Created by qiayuan on 2022/7/26.
+//
+
+#pragma once
+
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_mpc/SystemObservation.h>
+
+namespace ocs2::legged_robot {
+    class SafetyChecker {
+    public:
+        explicit SafetyChecker(const CentroidalModelInfo &info) : info_(info) {
+        }
+
+        [[nodiscard]] bool check(const SystemObservation &observation, const vector_t & /*optimized_state*/,
+                                 const vector_t & /*optimized_input*/) const {
+            return checkOrientation(observation);
+        }
+
+    protected:
+        [[nodiscard]] bool checkOrientation(const SystemObservation &observation) const {
+            if (vector_t pose = centroidal_model::getBasePose(observation.state, info_);
+                pose(5) > M_PI_2 || pose(5) < -M_PI_2) {
+                std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl;
+                return false;
+            }
+            return true;
+        }
+
+        const CentroidalModelInfo &info_;
+    };
+} // namespace legged

controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp

@@ -0,0 +1,52 @@
+//
+// Created by tlab-uav on 24-9-26.
+//
+
+#include <utility>
+
+#include "ocs2_quadruped_controller/control/GaitManager.h"
+
+#include <ocs2_core/misc/LoadData.h>
+
+namespace ocs2::legged_robot {
+    GaitManager::GaitManager(CtrlComponent &ctrl_component,
+                             std::shared_ptr<GaitSchedule> gait_schedule_ptr)
+        : ctrl_component_(ctrl_component),
+          gait_schedule_ptr_(std::move(gait_schedule_ptr)),
+          target_gait_({0.0, 1.0}, {STANCE}) {
+    }
+
+    void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime,
+                                   const vector_t &currentState,
+                                   const ReferenceManagerInterface &referenceManager) {
+        getTargetGait();
+        if (gait_updated_) {
+            const auto timeHorizon = finalTime - initTime;
+            gait_schedule_ptr_->insertModeSequenceTemplate(target_gait_, finalTime,
+                                                           timeHorizon);
+            gait_updated_ = false;
+        }
+    }
+
+    void GaitManager::init(const std::string &gait_file) {
+        gait_name_list_.clear();
+        loadData::loadStdVector(gait_file, "list", gait_name_list_, verbose_);
+
+        gait_list_.clear();
+        for (const auto &name: gait_name_list_) {
+            gait_list_.push_back(loadModeSequenceTemplate(gait_file, name, verbose_));
+        }
+
+        RCLCPP_INFO(rclcpp::get_logger("gait_manager"), "GaitManager is ready.");
+    }
+
+    void GaitManager::getTargetGait() {
+        if (ctrl_component_.control_inputs_.command == 0) return;
+        if (ctrl_component_.control_inputs_.command == last_command_) return;
+        last_command_ = ctrl_component_.control_inputs_.command;
+        target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1];
+        RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
+                    gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
+        gait_updated_ = true;
+    }
+}

controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp

@@ -0,0 +1,58 @@
+//
+// Created by tlab-uav on 24-9-30.
+//
+
+#include "ocs2_quadruped_controller/control/TargetManager.h"
+
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_robotic_tools/common/RotationTransforms.h>
+
+#include "ocs2_quadruped_controller/control/CtrlComponent.h"
+
+namespace ocs2::legged_robot {
+    TargetManager::TargetManager(CtrlComponent &ctrl_component,
+                                 const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
+                                 const std::string &task_file,
+                                 const std::string &reference_file)
+        : ctrl_component_(ctrl_component),
+          referenceManagerPtr_(referenceManagerPtr) {
+        default_joint_state_ = vector_t::Zero(12);
+        loadData::loadCppDataType(reference_file, "comHeight", command_height_);
+        loadData::loadEigenMatrix(reference_file, "defaultJointState", default_joint_state_);
+        loadData::loadCppDataType(task_file, "mpc.timeHorizon", time_to_target_);
+        loadData::loadCppDataType(reference_file, "targetRotationVelocity", target_rotation_velocity_);
+        loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
+    }
+
+    void TargetManager::update() {
+        vector_t cmdGoal = vector_t::Zero(6);
+        cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
+        cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;
+        cmdGoal[2] = ctrl_component_.control_inputs_.ry;
+        cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
+
+        const vector_t currentPose = ctrl_component_.observation_.state.segment<6>(6);
+        const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
+        vector_t cmdVelRot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
+
+        const scalar_t timeToTarget = time_to_target_;
+        const vector_t targetPose = [&]() {
+            vector_t target(6);
+            target(0) = currentPose(0) + cmdVelRot(0) * timeToTarget;
+            target(1) = currentPose(1) + cmdVelRot(1) * timeToTarget;
+            target(2) = command_height_;
+            target(3) = currentPose(3) + cmdGoal(3) * timeToTarget;
+            target(4) = 0;
+            target(5) = 0;
+            return target;
+        }();
+
+        const scalar_t targetReachingTime = ctrl_component_.observation_.time + timeToTarget;
+        auto trajectories =
+                targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime);
+        trajectories.stateTrajectory[0].head(3) = cmdVelRot;
+        trajectories.stateTrajectory[1].head(3) = cmdVelRot;
+
+        referenceManagerPtr_->setTargetTrajectories(std::move(trajectories));
+    }
+}

controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp

@@ -0,0 +1,36 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+
+#include "ocs2_quadruped_controller/estimator/FromTopiceEstimate.h"
+
+namespace ocs2::legged_robot {
+    FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
+                                                   const PinocchioEndEffectorKinematics &eeKinematics)
+        : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics) {
+        ros::NodeHandle nh;
+        sub_ = nh.subscribe<nav_msgs::Odometry>("/ground_truth/state", 10, &FromTopicStateEstimate::callback, this);
+    }
+
+    void FromTopicStateEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) {
+        buffer_.writeFromNonRT(*msg);
+    }
+
+    vector_t FromTopicStateEstimate::update(const ros::Time & /*time*/, const ros::Duration & /*period*/) {
+        nav_msgs::Odometry odom = *buffer_.readFromRT();
+
+        updateAngular(quatToZyx(Eigen::Quaternion<scalar_t>(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
+                                                            odom.pose.pose.orientation.y,
+                                                            odom.pose.pose.orientation.z)),
+                      Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.angular.x, odom.twist.twist.angular.y,
+                                                    odom.twist.twist.angular.z));
+        updateLinear(Eigen::Matrix<scalar_t, 3, 1>(odom.pose.pose.position.x, odom.pose.pose.position.y,
+                                                   odom.pose.pose.position.z),
+                     Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.linear.x, odom.twist.twist.linear.y,
+                                                   odom.twist.twist.linear.z));
+
+        publishMsgs(odom);
+
+        return rbdState_;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp

@@ -0,0 +1,210 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <utility>
+
+#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
+#include <ocs2_core/misc/LoadData.h>
+
+#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
+#include <ocs2_robotic_tools/common/RotationTransforms.h>
+
+namespace ocs2::legged_robot {
+    KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
+                                               const PinocchioEndEffectorKinematics &ee_kinematics,
+                                               CtrlComponent &ctrl_component,
+                                               const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
+        : StateEstimateBase(std::move(pinocchio_interface), std::move(info), ee_kinematics, ctrl_component,
+                            node),
+          numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts),
+          dimContacts_(3 * numContacts_),
+          numState_(6 + dimContacts_),
+          numObserve_(2 * dimContacts_ + numContacts_) {
+        xHat_.setZero(numState_);
+        ps_.setZero(dimContacts_);
+        vs_.setZero(dimContacts_);
+        a_.setIdentity(numState_, numState_);
+        b_.setZero(numState_, 3);
+
+        matrix_t c1(3, 6), c2(3, 6);
+        c1 << matrix3_t::Identity(), matrix3_t::Zero();
+        c2 << matrix3_t::Zero(), matrix3_t::Identity();
+        c_.setZero(numObserve_, numState_);
+        for (ssize_t i = 0; i < numContacts_; ++i) {
+            c_.block(3 * i, 0, 3, 6) = c1;
+            c_.block(3 * (numContacts_ + i), 0, 3, 6) = c2;
+            c_(2 * dimContacts_ + i, 6 + 3 * i + 2) = 1.0;
+        }
+        c_.block(0, 6, dimContacts_, dimContacts_) = -matrix_t::Identity(dimContacts_, dimContacts_);
+
+        q_.setIdentity(numState_, numState_);
+        p_ = 100. * q_;
+        r_.setIdentity(numObserve_, numObserve_);
+        feetHeights_.setZero(numContacts_);
+
+        ee_kinematics_->setPinocchioInterface(pinocchio_interface_);
+    }
+
+    vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
+        updateJointStates();
+        updateContact();
+        updateImu();
+
+        scalar_t dt = period.seconds();
+        a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity();
+        b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity();
+        b_.block(3, 0, 3, 3) = dt * matrix3_t::Identity();
+        q_.block(0, 0, 3, 3) = (dt / 20.f) * matrix3_t::Identity();
+        q_.block(3, 3, 3, 3) = (dt * 9.81f / 20.f) * matrix3_t::Identity();
+        q_.block(6, 6, dimContacts_, dimContacts_) = dt * matrix_t::Identity(dimContacts_, dimContacts_);
+
+        const auto &model = pinocchio_interface_.getModel();
+        auto &data = pinocchio_interface_.getData();
+        size_t actuatedDofNum = info_.actuatedDofNum;
+
+        vector_t qPino(info_.generalizedCoordinatesNum);
+        vector_t vPino(info_.generalizedCoordinatesNum);
+        qPino.setZero();
+        qPino.segment<3>(3) = rbd_state_.head<3>(); // Only set orientation, let position in origin.
+        qPino.tail(actuatedDofNum) = rbd_state_.segment(6, actuatedDofNum);
+
+        vPino.setZero();
+        vPino.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
+            qPino.segment<3>(3),
+            rbd_state_.segment<3>(info_.generalizedCoordinatesNum));
+        // Only set angular velocity, let linear velocity be zero
+        vPino.tail(actuatedDofNum) = rbd_state_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum);
+
+        forwardKinematics(model, data, qPino, vPino);
+        updateFramePlacements(model, data);
+
+        const auto eePos = ee_kinematics_->getPosition(vector_t());
+        const auto eeVel = ee_kinematics_->getVelocity(vector_t(), vector_t());
+
+        matrix_t q = matrix_t::Identity(numState_, numState_);
+        q.block(0, 0, 3, 3) = q_.block(0, 0, 3, 3) * imuProcessNoisePosition_;
+        q.block(3, 3, 3, 3) = q_.block(3, 3, 3, 3) * imuProcessNoiseVelocity_;
+        q.block(6, 6, dimContacts_, dimContacts_) =
+                q_.block(6, 6, dimContacts_, dimContacts_) * footProcessNoisePosition_;
+
+        matrix_t r = matrix_t::Identity(numObserve_, numObserve_);
+        r.block(0, 0, dimContacts_, dimContacts_) =
+                r_.block(0, 0, dimContacts_, dimContacts_) * footSensorNoisePosition_;
+        r.block(dimContacts_, dimContacts_, dimContacts_, dimContacts_) =
+                r_.block(dimContacts_, dimContacts_, dimContacts_, dimContacts_) * footSensorNoiseVelocity_;
+        r.block(2 * dimContacts_, 2 * dimContacts_, numContacts_, numContacts_) =
+                r_.block(2 * dimContacts_, 2 * dimContacts_, numContacts_, numContacts_) * footHeightSensorNoise_;
+
+        for (int i = 0; i < numContacts_; i++) {
+            int i1 = 3 * i;
+
+            int qIndex = 6 + i1;
+            int rIndex1 = i1;
+            int rIndex2 = dimContacts_ + i1;
+            int rIndex3 = 2 * dimContacts_ + i;
+            bool isContact = contact_flag_[i];
+
+            scalar_t high_suspect_number(100);
+            q.block(qIndex, qIndex, 3, 3) = (isContact ? 1. : high_suspect_number) * q.block(qIndex, qIndex, 3, 3);
+            r.block(rIndex1, rIndex1, 3, 3) = (isContact ? 1. : high_suspect_number) * r.block(rIndex1, rIndex1, 3, 3);
+            r.block(rIndex2, rIndex2, 3, 3) = (isContact ? 1. : high_suspect_number) * r.block(rIndex2, rIndex2, 3, 3);
+            r(rIndex3, rIndex3) = (isContact ? 1. : high_suspect_number) * r(rIndex3, rIndex3);
+
+            ps_.segment(3 * i, 3) = -eePos[i];
+            ps_.segment(3 * i, 3)[2] += footRadius_;
+            vs_.segment(3 * i, 3) = -eeVel[i];
+        }
+
+        vector3_t g(0, 0, -9.81);
+        vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linear_accel_local_ + g;
+
+        vector_t y(numObserve_);
+        y << ps_, vs_, feetHeights_;
+        xHat_ = a_ * xHat_ + b_ * accel;
+        matrix_t at = a_.transpose();
+        matrix_t pm = a_ * p_ * at + q;
+        matrix_t cT = c_.transpose();
+        matrix_t yModel = c_ * xHat_;
+        matrix_t ey = y - yModel;
+        matrix_t s = c_ * pm * cT + r;
+
+        vector_t sEy = s.lu().solve(ey);
+        xHat_ += pm * cT * sEy;
+
+        matrix_t sC = s.lu().solve(c_);
+        p_ = (matrix_t::Identity(numState_, numState_) - pm * cT * sC) * pm;
+
+        matrix_t pt = p_.transpose();
+        p_ = (p_ + pt) / 2.0;
+
+        //  if (p_.block(0, 0, 2, 2).determinant() > 0.000001) {
+        //    p_.block(0, 2, 2, 16).setZero();
+        //    p_.block(2, 0, 16, 2).setZero();
+        //    p_.block(0, 0, 2, 2) /= 10.;
+        //  }
+
+        updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3));
+
+        auto odom = getOdomMsg();
+        odom.header.stamp = time;
+        odom.header.frame_id = "odom";
+        odom.child_frame_id = "base";
+        publishMsgs(odom);
+
+        return rbd_state_;
+    }
+
+    nav_msgs::msg::Odometry KalmanFilterEstimate::getOdomMsg() {
+        nav_msgs::msg::Odometry odom;
+        odom.pose.pose.position.x = xHat_.segment<3>(0)(0);
+        odom.pose.pose.position.y = xHat_.segment<3>(0)(1);
+        odom.pose.pose.position.z = xHat_.segment<3>(0)(2);
+        odom.pose.pose.orientation.x = quat_.x();
+        odom.pose.pose.orientation.y = quat_.y();
+        odom.pose.pose.orientation.z = quat_.z();
+        odom.pose.pose.orientation.w = quat_.w();
+        odom.pose.pose.orientation.x = quat_.x();
+        for (int i = 0; i < 3; ++i) {
+            for (int j = 0; j < 3; ++j) {
+                odom.pose.covariance[i * 6 + j] = p_(i, j);
+                odom.pose.covariance[6 * (3 + i) + (3 + j)] = orientationCovariance_(i * 3 + j);
+            }
+        }
+        //  The twist in this message should be specified in the coordinate frame given by the child_frame_id: "base"
+        vector_t twist = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)).transpose() * xHat_.segment<3>(3);
+        odom.twist.twist.linear.x = twist.x();
+        odom.twist.twist.linear.y = twist.y();
+        odom.twist.twist.linear.z = twist.z();
+        odom.twist.twist.angular.x = angular_vel_local_.x();
+        odom.twist.twist.angular.y = angular_vel_local_.y();
+        odom.twist.twist.angular.z = angular_vel_local_.z();
+        for (int i = 0; i < 3; ++i) {
+            for (int j = 0; j < 3; ++j) {
+                odom.twist.covariance[i * 6 + j] = p_.block<3, 3>(3, 3)(i, j);
+                odom.twist.covariance[6 * (3 + i) + (3 + j)] = angularVelCovariance_(i * 3 + j);
+            }
+        }
+        return odom;
+    }
+
+    void KalmanFilterEstimate::loadSettings(const std::string &taskFile, bool verbose) {
+        boost::property_tree::ptree pt;
+        read_info(taskFile, pt);
+        const std::string prefix = "kalmanFilter.";
+        if (verbose) {
+            std::cerr << "\n #### Kalman Filter Noise:";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+
+        loadData::loadPtreeValue(pt, footRadius_, prefix + "footRadius", verbose);
+        loadData::loadPtreeValue(pt, imuProcessNoisePosition_, prefix + "imuProcessNoisePosition", verbose);
+        loadData::loadPtreeValue(pt, imuProcessNoiseVelocity_, prefix + "imuProcessNoiseVelocity", verbose);
+        loadData::loadPtreeValue(pt, footProcessNoisePosition_, prefix + "footProcessNoisePosition", verbose);
+        loadData::loadPtreeValue(pt, footSensorNoisePosition_, prefix + "footSensorNoisePosition", verbose);
+        loadData::loadPtreeValue(pt, footSensorNoiseVelocity_, prefix + "footSensorNoiseVelocity", verbose);
+        loadData::loadPtreeValue(pt, footHeightSensorNoise_, prefix + "footHeightSensorNoise", verbose);
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp

@@ -0,0 +1,98 @@
+//
+// Created by qiayuan on 2021/11/15.
+//
+
+#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
+
+#include <ocs2_centroidal_model/FactoryFunctions.h>
+#include <ocs2_legged_robot/common/Types.h>
+#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
+
+#include <memory>
+#include "ocs2_quadruped_controller/control/CtrlComponent.h"
+
+namespace ocs2::legged_robot {
+
+    StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
+                                         const PinocchioEndEffectorKinematics &ee_kinematics,
+                                         CtrlComponent &ctrl_component,
+                                         rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+        : ctrl_component_(ctrl_component),
+          pinocchio_interface_(std::move(pinocchio_interface)),
+          info_(std::move(info)),
+          ee_kinematics_(ee_kinematics.clone()),
+          rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
+        odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+        pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);
+    }
+
+    void StateEstimateBase::updateJointStates() {
+        const size_t size = ctrl_component_.joint_effort_state_interface_.size();
+        vector_t joint_pos(size), joint_vel(size);
+
+        for (int i = 0; i < size; i++) {
+            joint_pos(i) = ctrl_component_.joint_position_state_interface_[i].get().get_value();
+            joint_vel(i) = ctrl_component_.joint_velocity_state_interface_[i].get().get_value();
+        }
+
+        rbd_state_.segment(6, info_.actuatedDofNum) = joint_pos;
+        rbd_state_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel;
+    }
+
+    void StateEstimateBase::updateContact() {
+        const size_t size = ctrl_component_.foot_force_state_interface_.size();
+        for (int i = 0; i < size; i++) {
+            contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 1;
+        }
+    }
+
+    void StateEstimateBase::updateImu() {
+        quat_ = {
+            ctrl_component_.imu_state_interface_[0].get().get_value(),
+            ctrl_component_.imu_state_interface_[1].get().get_value(),
+            ctrl_component_.imu_state_interface_[2].get().get_value(),
+            ctrl_component_.imu_state_interface_[3].get().get_value()
+        };
+
+        angular_vel_local_ = {
+            ctrl_component_.imu_state_interface_[4].get().get_value(),
+            ctrl_component_.imu_state_interface_[5].get().get_value(),
+            ctrl_component_.imu_state_interface_[6].get().get_value()
+        };
+
+        linear_accel_local_ = {
+            ctrl_component_.imu_state_interface_[7].get().get_value(),
+            ctrl_component_.imu_state_interface_[8].get().get_value(),
+            ctrl_component_.imu_state_interface_[9].get().get_value()
+        };
+
+        // orientationCovariance_ = orientationCovariance;
+        // angularVelCovariance_ = angularVelCovariance;
+        // linearAccelCovariance_ = linearAccelCovariance;
+
+        const vector3_t zyx = quatToZyx(quat_) - zyx_offset_;
+        const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(
+            zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat_), angular_vel_local_));
+        updateAngular(zyx, angularVelGlobal);
+    }
+
+    void StateEstimateBase::updateAngular(const vector3_t &zyx, const vector_t &angularVel) {
+        rbd_state_.segment<3>(0) = zyx;
+        rbd_state_.segment<3>(info_.generalizedCoordinatesNum) = angularVel;
+    }
+
+    void StateEstimateBase::updateLinear(const vector_t &pos, const vector_t &linearVel) {
+        rbd_state_.segment<3>(3) = pos;
+        rbd_state_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel;
+    }
+
+    void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) const {
+        rclcpp::Time time = odom.header.stamp;
+        odom_pub_->publish(odom);
+
+        geometry_msgs::msg::PoseWithCovarianceStamped pose;
+        pose.header = odom.header;
+        pose.pose.pose = odom.pose.pose;
+        pose_pub_->publish(pose);
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp

@@ -0,0 +1,394 @@
+//
+// Created by qiayuan on 2022/7/16.
+//
+
+#include "ocs2_quadruped_controller/interface/LeggedInterface.h"
+
+#include <memory>
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+#include <ocs2_centroidal_model/FactoryFunctions.h>
+#include <ocs2_centroidal_model/ModelHelperFunctions.h>
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_core/misc/LoadStdVectorOfPair.h>
+#include <ocs2_core/soft_constraint/StateInputSoftConstraint.h>
+#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
+#include <ocs2_legged_robot/dynamics/LeggedRobotDynamicsAD.h>
+#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
+#include <pinocchio/fwd.hpp>  // forward declarations must be included first.
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+
+#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h"
+#include "ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h"
+#include "ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h"
+#include "ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h"
+#include "ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h"
+#include "ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h"
+#include "ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h"
+#include "ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h"
+
+// Boost
+#include <boost/filesystem/operations.hpp>
+#include <boost/filesystem/path.hpp>
+
+namespace ocs2::legged_robot {
+    LeggedInterface::LeggedInterface(const std::string &task_file,
+                                     const std::string &urdf_file,
+                                     const std::string &reference_file,
+                                     const bool use_hard_friction_cone_constraint)
+        : use_hard_friction_cone_constraint_(use_hard_friction_cone_constraint) {
+        // check that task file exists
+        if (const boost::filesystem::path task_file_path(task_file); exists(task_file_path)) {
+            std::cerr << "[LeggedInterface] Loading task file: " << task_file_path << std::endl;
+        } else {
+            throw std::invalid_argument("[LeggedInterface] Task file not found: " + task_file_path.string());
+        }
+
+        // check that urdf file exists
+        if (const boost::filesystem::path urdf_file_path(urdf_file); exists(urdf_file_path)) {
+            std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdf_file_path << std::endl;
+        } else {
+            throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdf_file_path.string());
+        }
+
+        // check that targetCommand file exists
+        if (const boost::filesystem::path reference_file_path(reference_file); exists(reference_file_path)) {
+            std::cerr << "[LeggedInterface] Loading target command settings from: " << reference_file_path << std::endl;
+        } else {
+            throw std::invalid_argument(
+                "[LeggedInterface] targetCommand file not found: " + reference_file_path.string());
+        }
+
+        bool verbose = false;
+        loadData::loadCppDataType(task_file, "legged_robot_interface.verbose", verbose);
+
+        // load setting from loading file
+        model_settings_ = loadModelSettings(task_file, "model_settings", verbose);
+
+        // Todo : load settings from ros param
+        model_settings_.jointNames = {
+            "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+            "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
+            "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
+            "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"
+        };
+        model_settings_.contactNames3DoF = {"FL_foot", "FR_foot", "RL_foot", "RR_foot"};
+
+        mpc_settings_ = mpc::loadSettings(task_file, "mpc", verbose);
+        ddp_settings_ = ddp::loadSettings(task_file, "ddp", verbose);
+        sqp_settings_ = sqp::loadSettings(task_file, "sqp", verbose);
+        ipm_settings_ = ipm::loadSettings(task_file, "ipm", verbose);
+        rollout_settings_ = rollout::loadSettings(task_file, "rollout", verbose);
+    }
+
+
+    void LeggedInterface::setupOptimalControlProblem(const std::string &task_file,
+                                                     const std::string &urdf_file,
+                                                     const std::string &reference_file,
+                                                     const bool verbose) {
+        setupModel(task_file, urdf_file, reference_file);
+
+        // Initial state
+        initial_state_.setZero(centroidal_model_info_.stateDim);
+        loadData::loadEigenMatrix(task_file, "initialState", initial_state_);
+
+        setupReferenceManager(task_file, urdf_file, reference_file, verbose);
+
+        // Optimal control problem
+        problem_ptr_ = std::make_unique<OptimalControlProblem>();
+
+        // Dynamics
+        std::unique_ptr<SystemDynamicsBase> dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(
+            *pinocchio_interface_ptr_, centroidal_model_info_, "dynamics",
+            model_settings_);
+        problem_ptr_->dynamicsPtr = std::move(dynamicsPtr);
+
+        // Cost terms
+        problem_ptr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(task_file, centroidal_model_info_, verbose));
+
+        // Constraint terms
+        // friction cone settings
+        scalar_t frictionCoefficient = 0.7;
+        RelaxedBarrierPenalty::Config barrierPenaltyConfig;
+        std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(task_file, verbose);
+
+        for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++) {
+            const std::string &footName = model_settings_.contactNames3DoF[i];
+            std::unique_ptr<EndEffectorKinematics<scalar_t> > eeKinematicsPtr =
+                    getEeKinematicsPtr({footName}, footName);
+
+            if (use_hard_friction_cone_constraint_) {
+                problem_ptr_->inequalityConstraintPtr->add(footName + "_frictionCone",
+                                                           getFrictionConeConstraint(i, frictionCoefficient));
+            } else {
+                problem_ptr_->softConstraintPtr->add(footName + "_frictionCone",
+                                                     getFrictionConeSoftConstraint(
+                                                         i, frictionCoefficient, barrierPenaltyConfig));
+            }
+            problem_ptr_->equalityConstraintPtr->add(footName + "_zeroForce", std::make_unique<ZeroForceConstraint>(
+                                                         *reference_manager_ptr_, i, centroidal_model_info_));
+            problem_ptr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
+                                                     getZeroVelocityConstraint(*eeKinematicsPtr, i));
+            problem_ptr_->equalityConstraintPtr->add(
+                footName + "_normalVelocity",
+                std::make_unique<NormalVelocityConstraintCppAd>(*reference_manager_ptr_, *eeKinematicsPtr, i));
+        }
+
+        // Self-collision avoidance constraint
+        problem_ptr_->stateSoftConstraintPtr->add("selfCollision",
+                                                  getSelfCollisionConstraint(
+                                                      *pinocchio_interface_ptr_, task_file, "selfCollision", verbose));
+
+        setupPreComputation(task_file, urdf_file, reference_file, verbose);
+
+        // Rollout
+        rollout_ptr_ = std::make_unique<TimeTriggeredRollout>(*problem_ptr_->dynamicsPtr, rollout_settings_);
+
+        // Initialization
+        constexpr bool extend_normalized_momentum = true;
+        initializer_ptr_ = std::make_unique<LeggedRobotInitializer>(centroidal_model_info_, *reference_manager_ptr_,
+                                                                    extend_normalized_momentum);
+    }
+
+
+    void LeggedInterface::setupModel(const std::string &task_file, const std::string &urdf_file,
+                                     const std::string &reference_file) {
+        // PinocchioInterface
+        pinocchio_interface_ptr_ =
+                std::make_unique<PinocchioInterface>(
+                    centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames));
+
+        // CentroidModelInfo
+        centroidal_model_info_ = centroidal_model::createCentroidalModelInfo(
+            *pinocchio_interface_ptr_, centroidal_model::loadCentroidalType(task_file),
+            centroidal_model::loadDefaultJointState(pinocchio_interface_ptr_->getModel().nq - 6, reference_file),
+            model_settings_.contactNames3DoF,
+            model_settings_.contactNames6DoF);
+    }
+
+
+    void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
+                                                const std::string &referenceFile,
+                                                const bool verbose) {
+        auto swingTrajectoryPlanner =
+                std::make_unique<SwingTrajectoryPlanner>(
+                    loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
+        reference_manager_ptr_ =
+                std::make_shared<SwitchedModelReferenceManager>(loadGaitSchedule(referenceFile, verbose),
+                                                                std::move(swingTrajectoryPlanner));
+    }
+
+
+    void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
+                                              const std::string &referenceFile,
+                                              bool verbose) {
+        problem_ptr_->preComputationPtr = std::make_unique<LeggedRobotPreComputation>(
+            *pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(),
+            model_settings_);
+    }
+
+
+    std::shared_ptr<GaitSchedule> LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const {
+        const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false);
+        const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false);
+
+        const auto defaultGait = [defaultModeSequenceTemplate] {
+            Gait gait{};
+            gait.duration = defaultModeSequenceTemplate.switchingTimes.back();
+            // Events: from time -> phase
+            std::for_each(defaultModeSequenceTemplate.switchingTimes.begin() + 1,
+                          defaultModeSequenceTemplate.switchingTimes.end() - 1,
+                          [&](double eventTime) { gait.eventPhases.push_back(eventTime / gait.duration); });
+            // Modes:
+            gait.modeSequence = defaultModeSequenceTemplate.modeSequence;
+            return gait;
+        }();
+
+        // display
+        if (verbose) {
+            std::cerr << "\n#### Modes Schedule: ";
+            std::cerr << "\n#### =============================================================================\n";
+            std::cerr << "Initial Modes Schedule: \n" << initModeSchedule;
+            std::cerr << "Default Modes Sequence Template: \n" << defaultModeSequenceTemplate;
+            std::cerr << "#### =============================================================================\n";
+        }
+
+        return std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate,
+                                              model_settings_.phaseTransitionStanceTime);
+    }
+
+
+    matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info) {
+        const size_t totalContactDim = 3 * info.numThreeDofContacts;
+
+        const auto &model = pinocchio_interface_ptr_->getModel();
+        auto &data = pinocchio_interface_ptr_->getData();
+        const auto q = centroidal_model::getGeneralizedCoordinates(initial_state_, centroidal_model_info_);
+        computeJointJacobians(model, data, q);
+        updateFramePlacements(model, data);
+
+        matrix_t base2feetJac(totalContactDim, info.actuatedDofNum);
+        for (size_t i = 0; i < info.numThreeDofContacts; i++) {
+            matrix_t jac = matrix_t::Zero(6, info.generalizedCoordinatesNum);
+            getFrameJacobian(model, data, model.getBodyId(model_settings_.contactNames3DoF[i]),
+                             pinocchio::LOCAL_WORLD_ALIGNED, jac);
+            base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum);
+        }
+
+        matrix_t rTaskspace(info.inputDim, info.inputDim);
+        loadData::loadEigenMatrix(taskFile, "R", rTaskspace);
+        matrix_t r = rTaskspace;
+        // Joint velocities
+        r.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) =
+                base2feetJac.transpose() * rTaskspace.block(totalContactDim, totalContactDim, info.actuatedDofNum,
+                                                            info.actuatedDofNum) *
+                base2feetJac;
+        return r;
+    }
+
+
+    std::unique_ptr<StateInputCost> LeggedInterface::getBaseTrackingCost(
+        const std::string &taskFile, const CentroidalModelInfo &info,
+        bool verbose) {
+        matrix_t Q(info.stateDim, info.stateDim);
+        loadData::loadEigenMatrix(taskFile, "Q", Q);
+        matrix_t R = initializeInputCostWeight(taskFile, info);
+
+        if (verbose) {
+            std::cerr << "\n #### Base Tracking Cost Coefficients: ";
+            std::cerr << "\n #### =============================================================================\n";
+            std::cerr << "Q:\n" << Q << "\n";
+            std::cerr << "R:\n" << R << "\n";
+            std::cerr << " #### =============================================================================\n";
+        }
+
+        return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info,
+                                                                    *reference_manager_ptr_);
+    }
+
+
+    std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedInterface::loadFrictionConeSettings(
+        const std::string &taskFile, bool verbose) {
+        boost::property_tree::ptree pt;
+        read_info(taskFile, pt);
+        const std::string prefix = "frictionConeSoftConstraint.";
+
+        scalar_t frictionCoefficient = 1.0;
+        RelaxedBarrierPenalty::Config barrierPenaltyConfig;
+        if (verbose) {
+            std::cerr << "\n #### Friction Cone Settings: ";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+        loadData::loadPtreeValue(pt, frictionCoefficient, prefix + "frictionCoefficient", verbose);
+        loadData::loadPtreeValue(pt, barrierPenaltyConfig.mu, prefix + "mu", verbose);
+        loadData::loadPtreeValue(pt, barrierPenaltyConfig.delta, prefix + "delta", verbose);
+        if (verbose) {
+            std::cerr << " #### =============================================================================\n";
+        }
+
+        return {frictionCoefficient, barrierPenaltyConfig};
+    }
+
+
+    std::unique_ptr<StateInputConstraint> LeggedInterface::getFrictionConeConstraint(
+        size_t contactPointIndex, scalar_t frictionCoefficient) {
+        FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
+        return std::make_unique<FrictionConeConstraint>(*reference_manager_ptr_, frictionConeConConfig,
+                                                        contactPointIndex,
+                                                        centroidal_model_info_);
+    }
+
+
+    std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint(
+        size_t contactPointIndex, scalar_t frictionCoefficient,
+        const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) {
+        return std::make_unique<StateInputSoftConstraint>(
+            getFrictionConeConstraint(contactPointIndex, frictionCoefficient),
+            std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
+    }
+
+
+    std::unique_ptr<EndEffectorKinematics<scalar_t> > LeggedInterface::getEeKinematicsPtr(
+        const std::vector<std::string> &foot_names,
+        const std::string &model_name) {
+        const auto infoCppAd = centroidal_model_info_.toCppAd();
+        const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);
+        auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state,
+                                                   PinocchioInterfaceCppAd &pinocchioInterfaceAd) {
+            const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
+            updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
+        };
+        std::unique_ptr<EndEffectorKinematics<scalar_t> > end_effector_kinematics = std::make_unique<
+            PinocchioEndEffectorKinematicsCppAd>(
+            *pinocchio_interface_ptr_, pinocchioMappingCppAd,
+            foot_names,
+            centroidal_model_info_.stateDim,
+            centroidal_model_info_.inputDim,
+            velocityUpdateCallback, model_name,
+            model_settings_.modelFolderCppAd,
+            model_settings_.recompileLibrariesCppAd,
+            model_settings_.verboseCppAd);
+
+        return end_effector_kinematics;
+    }
+
+
+    std::unique_ptr<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint(
+        const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
+        const size_t contact_point_index) {
+        auto eeZeroVelConConfig = [](scalar_t positionErrorGain) {
+            EndEffectorLinearConstraint::Config config;
+            config.b.setZero(3);
+            config.Av.setIdentity(3, 3);
+            if (!numerics::almost_eq(positionErrorGain, 0.0)) {
+                config.Ax.setZero(3, 3);
+                config.Ax(2, 2) = positionErrorGain;
+            }
+            return config;
+        };
+        return std::make_unique<ZeroVelocityConstraintCppAd>(
+            *reference_manager_ptr_, end_effector_kinematics, contact_point_index,
+            eeZeroVelConConfig(model_settings_.positionErrorGain));
+    }
+
+
+    std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
+                                                                           const std::string &taskFile,
+                                                                           const std::string &prefix,
+                                                                           bool verbose) {
+        std::vector<std::pair<size_t, size_t> > collisionObjectPairs;
+        std::vector<std::pair<std::string, std::string> > collisionLinkPairs;
+        scalar_t mu = 1e-2;
+        scalar_t delta = 1e-3;
+        scalar_t minimumDistance = 0.0;
+
+        boost::property_tree::ptree pt;
+        read_info(taskFile, pt);
+        if (verbose) {
+            std::cerr << "\n #### SelfCollision Settings: ";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+        loadData::loadPtreeValue(pt, mu, prefix + ".mu", verbose);
+        loadData::loadPtreeValue(pt, delta, prefix + ".delta", verbose);
+        loadData::loadPtreeValue(pt, minimumDistance, prefix + ".minimumDistance", verbose);
+        loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose);
+        loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose);
+
+        geometry_interface_ptr_ = std::make_unique<PinocchioGeometryInterface>(
+            pinocchioInterface, collisionLinkPairs, collisionObjectPairs);
+        if (verbose) {
+            std::cerr << " #### =============================================================================\n";
+            const size_t numCollisionPairs = geometry_interface_ptr_->getNumCollisionPairs();
+            std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n";
+        }
+
+        std::unique_ptr<StateConstraint> constraint = std::make_unique<LeggedSelfCollisionConstraint>(
+            CentroidalModelPinocchioMapping(centroidal_model_info_), *geometry_interface_ptr_, minimumDistance);
+
+        auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta});
+
+        return std::make_unique<StateSoftConstraint>(std::move(constraint), std::move(penalty));
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/interface/LeggedRobotPreComputation.cpp

@@ -0,0 +1,109 @@
+/******************************************************************************
+Copyright (c) 2020, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <pinocchio/fwd.hpp>
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+
+#include <ocs2_centroidal_model/ModelHelperFunctions.h>
+#include <ocs2_core/misc/Numerics.h>
+
+#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h"
+
+namespace ocs2::legged_robot {
+    LeggedRobotPreComputation::LeggedRobotPreComputation(PinocchioInterface pinocchioInterface,
+                                                         CentroidalModelInfo info,
+                                                         const SwingTrajectoryPlanner &swingTrajectoryPlanner,
+                                                         ModelSettings settings)
+        : pinocchioInterface_(std::move(pinocchioInterface)),
+          info_(std::move(info)),
+          swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner),
+          mappingPtr_(new CentroidalModelPinocchioMapping(info_)),
+          settings_(std::move(settings)) {
+        eeNormalVelConConfigs_.resize(info_.numThreeDofContacts);
+        mappingPtr_->setPinocchioInterface(pinocchioInterface_);
+    }
+
+
+    LeggedRobotPreComputation::LeggedRobotPreComputation(const LeggedRobotPreComputation &rhs)
+        : pinocchioInterface_(rhs.pinocchioInterface_),
+          info_(rhs.info_),
+          swingTrajectoryPlannerPtr_(rhs.swingTrajectoryPlannerPtr_),
+          mappingPtr_(rhs.mappingPtr_->clone()),
+          settings_(rhs.settings_) {
+        eeNormalVelConConfigs_.resize(rhs.eeNormalVelConConfigs_.size());
+        mappingPtr_->setPinocchioInterface(pinocchioInterface_);
+    }
+
+
+    void LeggedRobotPreComputation::request(RequestSet request, scalar_t t, const vector_t &x, const vector_t &u) {
+        if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint)) {
+            return;
+        }
+
+        // lambda to set config for normal velocity constraints
+        auto eeNormalVelConConfig = [&](size_t footIndex) {
+            EndEffectorLinearConstraint::Config config;
+            config.b = (vector_t(1) << -swingTrajectoryPlannerPtr_->getZvelocityConstraint(footIndex, t)).
+                    finished();
+            config.Av = (matrix_t(1, 3) << 0.0, 0.0, 1.0).finished();
+            if (!numerics::almost_eq(settings_.positionErrorGain, 0.0)) {
+                config.b(0) -= settings_.positionErrorGain * swingTrajectoryPlannerPtr_->getZpositionConstraint(
+                    footIndex, t);
+                config.Ax = (matrix_t(1, 3) << 0.0, 0.0, settings_.positionErrorGain).finished();
+            }
+            return config;
+        };
+
+        if (request.contains(Request::Constraint)) {
+            for (size_t i = 0; i < info_.numThreeDofContacts; i++) {
+                eeNormalVelConConfigs_[i] = eeNormalVelConConfig(i);
+            }
+        }
+
+        const auto &model = pinocchioInterface_.getModel();
+        auto &data = pinocchioInterface_.getData();
+        vector_t q = mappingPtr_->getPinocchioJointPosition(x);
+        if (request.contains(Request::Approximation)) {
+            forwardKinematics(model, data, q);
+            updateFramePlacements(model, data);
+            updateGlobalPlacements(model, data);
+            computeJointJacobians(model, data);
+
+            updateCentroidalDynamics(pinocchioInterface_, info_, q);
+            vector_t v = mappingPtr_->getPinocchioJointVelocity(x, u);
+            updateCentroidalDynamicsDerivatives(pinocchioInterface_, info_, q, v);
+        } else {
+            forwardKinematics(model, data, q);
+            updateFramePlacements(model, data);
+        }
+    }
+}

controllers/ocs2_quadruped_controller/src/interface/SwitchedModelReferenceManager.cpp

@@ -0,0 +1,64 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h"
+
+
+namespace ocs2::legged_robot {
+    SwitchedModelReferenceManager::SwitchedModelReferenceManager(std::shared_ptr<GaitSchedule> gaitSchedulePtr,
+                                                                 std::shared_ptr<SwingTrajectoryPlanner>
+                                                                 swingTrajectoryPtr)
+        : ReferenceManager(TargetTrajectories(), ModeSchedule()),
+          gaitSchedulePtr_(std::move(gaitSchedulePtr)),
+          swingTrajectoryPtr_(std::move(swingTrajectoryPtr)) {
+    }
+
+
+    void SwitchedModelReferenceManager::setModeSchedule(const ModeSchedule &modeSchedule) {
+        ReferenceManager::setModeSchedule(modeSchedule);
+        gaitSchedulePtr_->setModeSchedule(modeSchedule);
+    }
+
+
+    contact_flag_t SwitchedModelReferenceManager::getContactFlags(scalar_t time) const {
+        return modeNumber2StanceLeg(this->getModeSchedule().modeAtTime(time));
+    }
+
+
+    void SwitchedModelReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime,
+                                                         const vector_t &initState,
+                                                         TargetTrajectories &targetTrajectories,
+                                                         ModeSchedule &modeSchedule) {
+        const auto timeHorizon = finalTime - initTime;
+        modeSchedule = gaitSchedulePtr_->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon);
+
+        const scalar_t terrainHeight = 0.0;
+        swingTrajectoryPtr_->update(modeSchedule, terrainHeight);
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/EndEffectorLinearConstraint.cpp

@@ -0,0 +1,105 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h"
+
+
+namespace ocs2::legged_robot {
+    EndEffectorLinearConstraint::EndEffectorLinearConstraint(
+        const EndEffectorKinematics<scalar_t> &endEffectorKinematics,
+        size_t numConstraints, Config config)
+        : StateInputConstraint(ConstraintOrder::Linear),
+          endEffectorKinematicsPtr_(endEffectorKinematics.clone()),
+          numConstraints_(numConstraints),
+          config_(std::move(config)) {
+        if (endEffectorKinematicsPtr_->getIds().size() != 1) {
+            throw std::runtime_error(
+                "[EndEffectorLinearConstraint] this class only accepts a single end-effector!");
+        }
+    }
+
+
+    EndEffectorLinearConstraint::EndEffectorLinearConstraint(const EndEffectorLinearConstraint &rhs)
+        : StateInputConstraint(rhs),
+          endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()),
+          numConstraints_(rhs.numConstraints_),
+          config_(rhs.config_) {
+    }
+
+
+    void EndEffectorLinearConstraint::configure(Config &&config) {
+        assert(config.b.rows() == numConstraints_);
+        assert(config.Ax.size() > 0 || config.Av.size() > 0);
+        assert((config.Ax.size() > 0 && config.Ax.rows() == numConstraints_) || config.Ax.size() == 0);
+        assert((config.Ax.size() > 0 && config.Ax.cols() == 3) || config.Ax.size() == 0);
+        assert((config.Av.size() > 0 && config.Av.rows() == numConstraints_) || config.Av.size() == 0);
+        assert((config.Av.size() > 0 && config.Av.cols() == 3) || config.Av.size() == 0);
+        config_ = std::move(config);
+    }
+
+
+    vector_t EndEffectorLinearConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                                                   const PreComputation &preComp) const {
+        vector_t f = config_.b;
+        if (config_.Ax.size() > 0) {
+            f.noalias() += config_.Ax * endEffectorKinematicsPtr_->getPosition(state).front();
+        }
+        if (config_.Av.size() > 0) {
+            f.noalias() += config_.Av * endEffectorKinematicsPtr_->getVelocity(state, input).front();
+        }
+        return f;
+    }
+
+
+    VectorFunctionLinearApproximation EndEffectorLinearConstraint::getLinearApproximation(
+        scalar_t time, const vector_t &state,
+        const vector_t &input,
+        const PreComputation &preComp) const {
+        VectorFunctionLinearApproximation linearApproximation =
+                VectorFunctionLinearApproximation::Zero(getNumConstraints(time), state.size(), input.size());
+
+        linearApproximation.f = config_.b;
+
+        if (config_.Ax.size() > 0) {
+            const auto positionApprox = endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front();
+            linearApproximation.f.noalias() += config_.Ax * positionApprox.f;
+            linearApproximation.dfdx.noalias() += config_.Ax * positionApprox.dfdx;
+        }
+
+        if (config_.Av.size() > 0) {
+            const auto velocityApprox = endEffectorKinematicsPtr_->getVelocityLinearApproximation(state, input).
+                    front();
+            linearApproximation.f.noalias() += config_.Av * velocityApprox.f;
+            linearApproximation.dfdx.noalias() += config_.Av * velocityApprox.dfdx;
+            linearApproximation.dfdu.noalias() += config_.Av * velocityApprox.dfdu;
+        }
+
+        return linearApproximation;
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/FrictionConeConstraint.cpp

@@ -0,0 +1,188 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h"
+
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+
+
+namespace ocs2::legged_robot {
+    FrictionConeConstraint::FrictionConeConstraint(const SwitchedModelReferenceManager &referenceManager, Config config,
+                                                   size_t contactPointIndex, CentroidalModelInfo info)
+        : StateInputConstraint(ConstraintOrder::Quadratic),
+          referenceManagerPtr_(&referenceManager),
+          config_(std::move(config)),
+          contactPointIndex_(contactPointIndex),
+          info_(std::move(info)) {
+    }
+
+
+    void FrictionConeConstraint::setSurfaceNormalInWorld(const vector3_t &surfaceNormalInWorld) {
+        t_R_w.setIdentity();
+        throw std::runtime_error("[FrictionConeConstraint] setSurfaceNormalInWorld() is not implemented!");
+    }
+
+
+    bool FrictionConeConstraint::isActive(scalar_t time) const {
+        return referenceManagerPtr_->getContactFlags(time)[contactPointIndex_];
+    }
+
+
+    vector_t FrictionConeConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                                              const PreComputation &preComp) const {
+        const auto forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_);
+        const vector3_t localForce = t_R_w * forcesInWorldFrame;
+        return coneConstraint(localForce);
+    }
+
+
+    VectorFunctionLinearApproximation FrictionConeConstraint::getLinearApproximation(
+        scalar_t time, const vector_t &state,
+        const vector_t &input,
+        const PreComputation &preComp) const {
+        const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_);
+        const vector3_t localForce = t_R_w * forcesInWorldFrame;
+
+        const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame);
+        const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce);
+        const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives);
+
+        VectorFunctionLinearApproximation linearApproximation;
+        linearApproximation.f = coneConstraint(localForce);
+        linearApproximation.dfdx = matrix_t::Zero(1, state.size());
+        linearApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives);
+        return linearApproximation;
+    }
+
+
+    VectorFunctionQuadraticApproximation FrictionConeConstraint::getQuadraticApproximation(
+        scalar_t time, const vector_t &state,
+        const vector_t &input,
+        const PreComputation &preComp) const {
+        const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_);
+        const vector3_t localForce = t_R_w * forcesInWorldFrame;
+
+        const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame);
+        const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce);
+        const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives);
+
+        VectorFunctionQuadraticApproximation quadraticApproximation;
+        quadraticApproximation.f = coneConstraint(localForce);
+        quadraticApproximation.dfdx = matrix_t::Zero(1, state.size());
+        quadraticApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives);
+        quadraticApproximation.dfdxx.emplace_back(frictionConeSecondDerivativeState(state.size(), coneDerivatives));
+        quadraticApproximation.dfduu.emplace_back(frictionConeSecondDerivativeInput(input.size(), coneDerivatives));
+        quadraticApproximation.dfdux.emplace_back(matrix_t::Zero(input.size(), state.size()));
+        return quadraticApproximation;
+    }
+
+
+    FrictionConeConstraint::LocalForceDerivatives FrictionConeConstraint::computeLocalForceDerivatives(
+        const vector3_t &forcesInWorldFrame) const {
+        LocalForceDerivatives localForceDerivatives{};
+        localForceDerivatives.dF_du = t_R_w;
+        return localForceDerivatives;
+    }
+
+
+    FrictionConeConstraint::ConeLocalDerivatives FrictionConeConstraint::computeConeLocalDerivatives(
+        const vector3_t &localForces) const {
+        const auto F_x_square = localForces.x() * localForces.x();
+        const auto F_y_square = localForces.y() * localForces.y();
+        const auto F_tangent_square = F_x_square + F_y_square + config_.regularization;
+        const auto F_tangent_norm = sqrt(F_tangent_square);
+        const auto F_tangent_square_pow32 = F_tangent_norm * F_tangent_square; // = F_tangent_square ^ (3/2)
+
+        ConeLocalDerivatives coneDerivatives{};
+        coneDerivatives.dCone_dF(0) = -localForces.x() / F_tangent_norm;
+        coneDerivatives.dCone_dF(1) = -localForces.y() / F_tangent_norm;
+        coneDerivatives.dCone_dF(2) = config_.frictionCoefficient;
+
+        coneDerivatives.d2Cone_dF2(0, 0) = -(F_y_square + config_.regularization) / F_tangent_square_pow32;
+        coneDerivatives.d2Cone_dF2(0, 1) = localForces.x() * localForces.y() / F_tangent_square_pow32;
+        coneDerivatives.d2Cone_dF2(0, 2) = 0.0;
+        coneDerivatives.d2Cone_dF2(1, 0) = coneDerivatives.d2Cone_dF2(0, 1);
+        coneDerivatives.d2Cone_dF2(1, 1) = -(F_x_square + config_.regularization) / F_tangent_square_pow32;
+        coneDerivatives.d2Cone_dF2(1, 2) = 0.0;
+        coneDerivatives.d2Cone_dF2(2, 0) = 0.0;
+        coneDerivatives.d2Cone_dF2(2, 1) = 0.0;
+        coneDerivatives.d2Cone_dF2(2, 2) = 0.0;
+
+        return coneDerivatives;
+    }
+
+
+    vector_t FrictionConeConstraint::coneConstraint(const vector3_t &localForces) const {
+        const auto F_tangent_square = localForces.x() * localForces.x() + localForces.y() * localForces.y() + config_.
+                                      regularization;
+        const auto F_tangent_norm = sqrt(F_tangent_square);
+        const scalar_t coneConstraint = config_.frictionCoefficient * (localForces.z() + config_.gripperForce) -
+                                        F_tangent_norm;
+        return (vector_t(1) << coneConstraint).finished();
+    }
+
+
+    FrictionConeConstraint::ConeDerivatives FrictionConeConstraint::computeConeConstraintDerivatives(
+        const ConeLocalDerivatives &coneLocalDerivatives, const LocalForceDerivatives &localForceDerivatives) const {
+        ConeDerivatives coneDerivatives;
+        // First order derivatives
+        coneDerivatives.dCone_du.noalias() = coneLocalDerivatives.dCone_dF.transpose() * localForceDerivatives.dF_du;
+
+        // Second order derivatives
+        coneDerivatives.d2Cone_du2.noalias() =
+                localForceDerivatives.dF_du.transpose() * coneLocalDerivatives.d2Cone_dF2 * localForceDerivatives.dF_du;
+
+        return coneDerivatives;
+    }
+
+
+    matrix_t FrictionConeConstraint::frictionConeInputDerivative(size_t inputDim,
+                                                                 const ConeDerivatives &coneDerivatives) const {
+        matrix_t dhdu = matrix_t::Zero(1, inputDim);
+        dhdu.block<1, 3>(0, 3 * contactPointIndex_) = coneDerivatives.dCone_du;
+        return dhdu;
+    }
+
+
+    matrix_t FrictionConeConstraint::frictionConeSecondDerivativeInput(size_t inputDim,
+                                                                       const ConeDerivatives &coneDerivatives) const {
+        matrix_t ddhdudu = matrix_t::Zero(inputDim, inputDim);
+        ddhdudu.block<3, 3>(3 * contactPointIndex_, 3 * contactPointIndex_) = coneDerivatives.d2Cone_du2;
+        ddhdudu.diagonal().array() -= config_.hessianDiagonalShift;
+        return ddhdudu;
+    }
+
+
+    matrix_t FrictionConeConstraint::frictionConeSecondDerivativeState(size_t stateDim,
+                                                                       const ConeDerivatives &coneDerivatives) const {
+        matrix_t ddhdxdx = matrix_t::Zero(stateDim, stateDim);
+        ddhdxdx.diagonal().array() -= config_.hessianDiagonalShift;
+        return ddhdxdx;
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/NormalVelocityConstraintCppAd.cpp

@@ -0,0 +1,77 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h"
+#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h"
+
+
+namespace ocs2::legged_robot {
+    NormalVelocityConstraintCppAd::NormalVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager,
+                                                                 const EndEffectorKinematics<scalar_t> &
+                                                                 endEffectorKinematics,
+                                                                 size_t contactPointIndex)
+        : StateInputConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(&referenceManager),
+          eeLinearConstraintPtr_(new EndEffectorLinearConstraint(endEffectorKinematics, 1)),
+          contactPointIndex_(contactPointIndex) {
+    }
+
+
+    NormalVelocityConstraintCppAd::NormalVelocityConstraintCppAd(const NormalVelocityConstraintCppAd &rhs)
+        : StateInputConstraint(rhs),
+          referenceManagerPtr_(rhs.referenceManagerPtr_),
+          eeLinearConstraintPtr_(rhs.eeLinearConstraintPtr_->clone()),
+          contactPointIndex_(rhs.contactPointIndex_) {
+    }
+
+
+    bool NormalVelocityConstraintCppAd::isActive(scalar_t time) const {
+        return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_];
+    }
+
+
+    vector_t NormalVelocityConstraintCppAd::getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                                                     const PreComputation &preComp) const {
+        const auto &preCompLegged = cast<LeggedRobotPreComputation>(preComp);
+        eeLinearConstraintPtr_->configure(preCompLegged.getEeNormalVelocityConstraintConfigs()[contactPointIndex_]);
+
+        return eeLinearConstraintPtr_->getValue(time, state, input, preComp);
+    }
+
+
+    VectorFunctionLinearApproximation NormalVelocityConstraintCppAd::getLinearApproximation(
+        scalar_t time, const vector_t &state,
+        const vector_t &input,
+        const PreComputation &preComp) const {
+        const auto &preCompLegged = cast<LeggedRobotPreComputation>(preComp);
+        eeLinearConstraintPtr_->configure(preCompLegged.getEeNormalVelocityConstraintConfigs()[contactPointIndex_]);
+
+        return eeLinearConstraintPtr_->getLinearApproximation(time, state, input, preComp);
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/SwingTrajectoryPlanner.cpp

@@ -0,0 +1,261 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include "ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h"
+
+#include <iostream>
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_core/misc/Lookup.h>
+
+#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
+
+
+namespace ocs2::legged_robot {
+    SwingTrajectoryPlanner::SwingTrajectoryPlanner(Config config, size_t numFeet) : config_(std::move(config)),
+        numFeet_(numFeet) {
+    }
+
+
+    scalar_t SwingTrajectoryPlanner::getZvelocityConstraint(size_t leg, scalar_t time) const {
+        const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time);
+        return feetHeightTrajectories_[leg][index].velocity(time);
+    }
+
+
+    scalar_t SwingTrajectoryPlanner::getZpositionConstraint(size_t leg, scalar_t time) const {
+        const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time);
+        return feetHeightTrajectories_[leg][index].position(time);
+    }
+
+
+    void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, scalar_t terrainHeight) {
+        const scalar_array_t terrainHeightSequence(modeSchedule.modeSequence.size(), terrainHeight);
+        feet_array_t<scalar_array_t> liftOffHeightSequence;
+        liftOffHeightSequence.fill(terrainHeightSequence);
+        feet_array_t<scalar_array_t> touchDownHeightSequence;
+        touchDownHeightSequence.fill(terrainHeightSequence);
+        update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence);
+    }
+
+
+    void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule,
+                                        const feet_array_t<scalar_array_t> &liftOffHeightSequence,
+                                        const feet_array_t<scalar_array_t> &touchDownHeightSequence) {
+        scalar_array_t heightSequence(modeSchedule.modeSequence.size());
+        feet_array_t<scalar_array_t> maxHeightSequence;
+        for (size_t j = 0; j < numFeet_; j++) {
+            for (int p = 0; p < modeSchedule.modeSequence.size(); ++p) {
+                heightSequence[p] = std::max(liftOffHeightSequence[j][p], touchDownHeightSequence[j][p]);
+            }
+            maxHeightSequence[j] = heightSequence;
+        }
+        update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence, maxHeightSequence);
+    }
+
+
+    void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule,
+                                        const feet_array_t<scalar_array_t> &liftOffHeightSequence,
+                                        const feet_array_t<scalar_array_t> &touchDownHeightSequence,
+                                        const feet_array_t<scalar_array_t> &maxHeightSequence) {
+        const auto &modeSequence = modeSchedule.modeSequence;
+        const auto &eventTimes = modeSchedule.eventTimes;
+
+        const auto eesContactFlagStocks = extractContactFlags(modeSequence);
+
+        feet_array_t<std::vector<int> > startTimesIndices;
+        feet_array_t<std::vector<int> > finalTimesIndices;
+        for (size_t leg = 0; leg < numFeet_; leg++) {
+            std::tie(startTimesIndices[leg], finalTimesIndices[leg]) =
+                    updateFootSchedule(eesContactFlagStocks[leg]);
+        }
+
+        for (size_t j = 0; j < numFeet_; j++) {
+            feetHeightTrajectories_[j].clear();
+            feetHeightTrajectories_[j].reserve(modeSequence.size());
+            for (int p = 0; p < modeSequence.size(); ++p) {
+                if (!eesContactFlagStocks[j][p]) {
+                    // for a swing leg
+                    const int swingStartIndex = startTimesIndices[j][p];
+                    const int swingFinalIndex = finalTimesIndices[j][p];
+                    checkThatIndicesAreValid(j, p, swingStartIndex, swingFinalIndex, modeSequence);
+
+                    const scalar_t swingStartTime = eventTimes[swingStartIndex];
+                    const scalar_t swingFinalTime = eventTimes[swingFinalIndex];
+
+                    const scalar_t scaling = swingTrajectoryScaling(swingStartTime, swingFinalTime,
+                                                                    config_.swingTimeScale);
+
+                    const CubicSpline::Node liftOff{
+                        swingStartTime, liftOffHeightSequence[j][p], scaling * config_.liftOffVelocity
+                    };
+                    const CubicSpline::Node touchDown{
+                        swingFinalTime, touchDownHeightSequence[j][p], scaling * config_.touchDownVelocity
+                    };
+                    const scalar_t midHeight = maxHeightSequence[j][p] + scaling * config_.swingHeight;
+                    feetHeightTrajectories_[j].emplace_back(liftOff, midHeight, touchDown);
+                } else {
+                    // for a stance leg
+                    // Note: setting the time here arbitrarily to 0.0 -> 1.0 makes the assert in CubicSpline fail
+                    const CubicSpline::Node liftOff{0.0, liftOffHeightSequence[j][p], 0.0};
+                    const CubicSpline::Node touchDown{1.0, liftOffHeightSequence[j][p], 0.0};
+                    feetHeightTrajectories_[j].emplace_back(liftOff, liftOffHeightSequence[j][p], touchDown);
+                }
+            }
+            feetHeightTrajectoriesEvents_[j] = eventTimes;
+        }
+    }
+
+
+    std::pair<std::vector<int>, std::vector<int> > SwingTrajectoryPlanner::updateFootSchedule(
+        const std::vector<bool> &contactFlagStock) {
+        const size_t numPhases = contactFlagStock.size();
+
+        std::vector<int> startTimeIndexStock(numPhases, 0);
+        std::vector<int> finalTimeIndexStock(numPhases, 0);
+
+        // find the startTime and finalTime indices for swing feet
+        for (size_t i = 0; i < numPhases; i++) {
+            if (!contactFlagStock[i]) {
+                std::tie(startTimeIndexStock[i], finalTimeIndexStock[i]) = findIndex(i, contactFlagStock);
+            }
+        }
+        return {startTimeIndexStock, finalTimeIndexStock};
+    }
+
+
+    feet_array_t<std::vector<bool> > SwingTrajectoryPlanner::extractContactFlags(
+        const std::vector<size_t> &phaseIDsStock) const {
+        const size_t numPhases = phaseIDsStock.size();
+
+        feet_array_t<std::vector<bool> > contactFlagStock;
+        std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector<bool>(numPhases));
+
+        for (size_t i = 0; i < numPhases; i++) {
+            const auto contactFlag = modeNumber2StanceLeg(phaseIDsStock[i]);
+            for (size_t j = 0; j < numFeet_; j++) {
+                contactFlagStock[j][i] = contactFlag[j];
+            }
+        }
+        return contactFlagStock;
+    }
+
+
+    std::pair<int, int> SwingTrajectoryPlanner::findIndex(size_t index, const std::vector<bool> &contactFlagStock) {
+        const size_t numPhases = contactFlagStock.size();
+
+        // skip if it is a stance leg
+        if (contactFlagStock[index]) {
+            return {0, 0};
+        }
+
+        // find the starting time
+        int startTimesIndex = -1;
+        for (int ip = index - 1; ip >= 0; ip--) {
+            if (contactFlagStock[ip]) {
+                startTimesIndex = ip;
+                break;
+            }
+        }
+
+        // find the final time
+        int finalTimesIndex = numPhases - 1;
+        for (size_t ip = index + 1; ip < numPhases; ip++) {
+            if (contactFlagStock[ip]) {
+                finalTimesIndex = ip - 1;
+                break;
+            }
+        }
+
+        return {startTimesIndex, finalTimesIndex};
+    }
+
+
+    void SwingTrajectoryPlanner::checkThatIndicesAreValid(int leg, int index, int startIndex, int finalIndex,
+                                                          const std::vector<size_t> &phaseIDsStock) {
+        const size_t numSubsystems = phaseIDsStock.size();
+        if (startIndex < 0) {
+            std::cerr << "Subsystem: " << index << " out of " << numSubsystems - 1 << std::endl;
+            for (size_t i = 0; i < numSubsystems; i++) {
+                std::cerr << "[" << i << "]: " << phaseIDsStock[i] << ",  ";
+            }
+            std::cerr << std::endl;
+
+            throw std::runtime_error(
+                "The time of take-off for the first swing of the EE with ID " + std::to_string(leg) +
+                " is not defined.");
+        }
+        if (finalIndex >= numSubsystems - 1) {
+            std::cerr << "Subsystem: " << index << " out of " << numSubsystems - 1 << std::endl;
+            for (size_t i = 0; i < numSubsystems; i++) {
+                std::cerr << "[" << i << "]: " << phaseIDsStock[i] << ",  ";
+            }
+            std::cerr << std::endl;
+
+            throw std::runtime_error(
+                "The time of touch-down for the last swing of the EE with ID " + std::to_string(leg) +
+                " is not defined.");
+        }
+    }
+
+
+    scalar_t SwingTrajectoryPlanner::swingTrajectoryScaling(scalar_t startTime, scalar_t finalTime,
+                                                            scalar_t swingTimeScale) {
+        return std::min(1.0, (finalTime - startTime) / swingTimeScale);
+    }
+
+
+    SwingTrajectoryPlanner::Config loadSwingTrajectorySettings(const std::string &fileName,
+                                                               const std::string &fieldName, bool verbose) {
+        boost::property_tree::ptree pt;
+        read_info(fileName, pt);
+
+        if (verbose) {
+            std::cerr << "\n #### Swing Trajectory Config:";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+
+        SwingTrajectoryPlanner::Config config;
+        const std::string prefix = fieldName + ".";
+
+        loadData::loadPtreeValue(pt, config.liftOffVelocity, prefix + "liftOffVelocity", verbose);
+        loadData::loadPtreeValue(pt, config.touchDownVelocity, prefix + "touchDownVelocity", verbose);
+        loadData::loadPtreeValue(pt, config.swingHeight, prefix + "swingHeight", verbose);
+        loadData::loadPtreeValue(pt, config.swingTimeScale, prefix + "swingTimeScale", verbose);
+
+        if (verbose) {
+            std::cerr << " #### =============================================================================" <<
+                    std::endl;
+        }
+
+        return config;
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroForceConstraint.cpp

@@ -0,0 +1,67 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h"
+
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+
+
+namespace ocs2::legged_robot {
+    ZeroForceConstraint::ZeroForceConstraint(const SwitchedModelReferenceManager &referenceManager,
+                                             size_t contactPointIndex,
+                                             CentroidalModelInfo info)
+        : StateInputConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(&referenceManager),
+          contactPointIndex_(contactPointIndex),
+          info_(std::move(info)) {
+    }
+
+
+    bool ZeroForceConstraint::isActive(scalar_t time) const {
+        return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_];
+    }
+
+
+    vector_t ZeroForceConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                                           const PreComputation &preComp) const {
+        return centroidal_model::getContactForces(input, contactPointIndex_, info_);
+    }
+
+
+    VectorFunctionLinearApproximation ZeroForceConstraint::getLinearApproximation(
+        scalar_t time, const vector_t &state, const vector_t &input,
+        const PreComputation &preComp) const {
+        VectorFunctionLinearApproximation approx;
+        approx.f = getValue(time, state, input, preComp);
+        approx.dfdx = matrix_t::Zero(3, state.size());
+        approx.dfdu = matrix_t::Zero(3, input.size());
+        approx.dfdu.middleCols<3>(3 * contactPointIndex_).diagonal() = vector_t::Ones(3);
+        return approx;
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroVelocityConstraintCppAd.cpp

@@ -0,0 +1,70 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include "ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h"
+
+namespace ocs2::legged_robot {
+    ZeroVelocityConstraintCppAd::ZeroVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager,
+                                                             const EndEffectorKinematics<scalar_t> &
+                                                             endEffectorKinematics,
+                                                             size_t contactPointIndex,
+                                                             EndEffectorLinearConstraint::Config config)
+        : StateInputConstraint(ConstraintOrder::Linear),
+          referenceManagerPtr_(&referenceManager),
+          eeLinearConstraintPtr_(new EndEffectorLinearConstraint(endEffectorKinematics, 3, std::move(config))),
+          contactPointIndex_(contactPointIndex) {
+    }
+
+
+    ZeroVelocityConstraintCppAd::ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd &rhs)
+        : StateInputConstraint(rhs),
+          referenceManagerPtr_(rhs.referenceManagerPtr_),
+          eeLinearConstraintPtr_(rhs.eeLinearConstraintPtr_->clone()),
+          contactPointIndex_(rhs.contactPointIndex_) {
+    }
+
+
+    bool ZeroVelocityConstraintCppAd::isActive(scalar_t time) const {
+        return referenceManagerPtr_->getContactFlags(time)[contactPointIndex_];
+    }
+
+
+    vector_t ZeroVelocityConstraintCppAd::getValue(scalar_t time, const vector_t &state, const vector_t &input,
+                                                   const PreComputation &preComp) const {
+        return eeLinearConstraintPtr_->getValue(time, state, input, preComp);
+    }
+
+
+    VectorFunctionLinearApproximation ZeroVelocityConstraintCppAd::getLinearApproximation(
+        scalar_t time, const vector_t &state,
+        const vector_t &input,
+        const PreComputation &preComp) const {
+        return eeLinearConstraintPtr_->getLinearApproximation(time, state, input, preComp);
+    }
+}

controllers/ocs2_quadruped_controller/src/interface/initialization/LeggedRobotInitializer.cpp

@@ -0,0 +1,58 @@
+/******************************************************************************
+Copyright (c) 2021, Farbod Farshidian. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+ * Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+ * Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+******************************************************************************/
+
+#include <ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h>
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_legged_robot/common/utils.h>
+
+
+namespace ocs2::legged_robot {
+    LeggedRobotInitializer::LeggedRobotInitializer(CentroidalModelInfo info,
+                                                   const SwitchedModelReferenceManager &referenceManager,
+                                                   bool extendNormalizedMomentum)
+        : info_(std::move(info)), referenceManagerPtr_(&referenceManager),
+          extendNormalizedMomentum_(extendNormalizedMomentum) {
+    }
+
+
+    LeggedRobotInitializer *LeggedRobotInitializer::clone() const {
+        return new LeggedRobotInitializer(*this);
+    }
+
+
+    void LeggedRobotInitializer::compute(scalar_t time, const vector_t &state, scalar_t nextTime, vector_t &input,
+                                         vector_t &nextState) {
+        const auto contactFlags = referenceManagerPtr_->getContactFlags(time);
+        input = weightCompensatingInput(info_, contactFlags);
+        nextState = state;
+        if (!extendNormalizedMomentum_) {
+            centroidal_model::getNormalizedMomentum(nextState, info_).setZero();
+        }
+    }
+} // namespace ocs2::legged_robot

controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp

@@ -0,0 +1,22 @@
+//
+// Created by qiayuan on 22-12-23.
+//
+
+#include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h"
+#include "ocs2_quadruped_controller/wbc/HoQp.h"
+
+namespace ocs2::legged_robot {
+    vector_t HierarchicalWbc::update(const vector_t &stateDesired, const vector_t &inputDesired,
+                                     const vector_t &rbdStateMeasured, size_t mode,
+                                     scalar_t period) {
+        WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
+
+        Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +
+                     formulateNoContactMotionTask();
+        Task task1 = formulateBaseAccelTask(stateDesired, inputDesired, period) + formulateSwingLegTask();
+        Task task2 = formulateContactForceTask(inputDesired);
+        HoQp hoQp(task2, std::make_shared<HoQp>(task1, std::make_shared<HoQp>(task0)));
+
+        return hoQp.getSolutions();
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp

@@ -0,0 +1,162 @@
+//
+// Created by qiayuan on 2022/6/28.
+//
+// Ref: https://github.com/bernhardpg/quadruped_locomotion
+//
+
+#include "ocs2_quadruped_controller/wbc/HoQp.h"
+
+#include <qpOASES.hpp>
+#include <utility>
+
+namespace ocs2::legged_robot {
+    HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) {
+        initVars();
+        formulateProblem();
+        solveProblem();
+        // For next problem
+        buildZMatrix();
+        stackSlackSolutions();
+    }
+
+    void HoQp::initVars() {
+        // Task variables
+        numSlackVars_ = task_.d_.rows();
+        hasEqConstraints_ = task_.a_.rows() > 0;
+        hasIneqConstraints_ = numSlackVars_ > 0;
+
+        // Pre-Task variables
+        if (higherProblem_ != nullptr) {
+            stackedZPrev_ = higherProblem_->getStackedZMatrix();
+            stackedTasksPrev_ = higherProblem_->getStackedTasks();
+            stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions();
+            xPrev_ = higherProblem_->getSolutions();
+            numPrevSlackVars_ = higherProblem_->getSlackedNumVars();
+
+            numDecisionVars_ = stackedZPrev_.cols();
+        } else {
+            numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols());
+
+            stackedTasksPrev_ = Task(numDecisionVars_);
+            stackedZPrev_ = matrix_t::Identity(numDecisionVars_, numDecisionVars_);
+            stackedSlackSolutionsPrev_ = Eigen::VectorXd::Zero(0);
+            xPrev_ = Eigen::VectorXd::Zero(numDecisionVars_);
+            numPrevSlackVars_ = 0;
+        }
+
+        stackedTasks_ = task_ + stackedTasksPrev_;
+
+        // Init convenience matrices
+        eyeNvNv_ = matrix_t::Identity(numSlackVars_, numSlackVars_);
+        zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_);
+    }
+
+    void HoQp::formulateProblem() {
+        buildHMatrix();
+        buildCVector();
+        buildDMatrix();
+        buildFVector();
+    }
+
+    void HoQp::buildHMatrix() {
+        matrix_t zTaTaz(numDecisionVars_, numDecisionVars_);
+
+        if (hasEqConstraints_) {
+            // Make sure that all eigenvalues of A_t_A are non-negative, which could arise due to numerical issues
+            matrix_t aCurrZPrev = task_.a_ * stackedZPrev_;
+            zTaTaz = aCurrZPrev.transpose() * aCurrZPrev + 1e-12 * matrix_t::Identity(
+                         numDecisionVars_, numDecisionVars_);
+            // This way of splitting up the multiplication is about twice as fast as multiplying 4 matrices
+        } else {
+            zTaTaz.setZero();
+        }
+
+        h_ = (matrix_t(numDecisionVars_ + numSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off
+              << zTaTaz, zeroNvNx_.transpose(),
+              zeroNvNx_, eyeNvNv_) // clang-format on
+                .finished();
+    }
+
+    void HoQp::buildCVector() {
+        vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_);
+        vector_t zeroVec = vector_t::Zero(numSlackVars_);
+
+        vector_t temp(numDecisionVars_);
+        if (hasEqConstraints_) {
+            temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_);
+        } else {
+            temp.setZero();
+        }
+
+        c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished();
+    }
+
+    void HoQp::buildDMatrix() {
+        matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_);
+
+        matrix_t dCurrZ;
+        if (hasIneqConstraints_) {
+            dCurrZ = task_.d_ * stackedZPrev_;
+        } else {
+            dCurrZ = matrix_t::Zero(0, numDecisionVars_);
+        }
+
+        // NOTE: This is upside down compared to the paper,
+        // but more consistent with the rest of the algorithm
+        d_ = (matrix_t(2 * numSlackVars_ + numPrevSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off
+              << zeroNvNx_, -eyeNvNv_,
+              stackedTasksPrev_.d_ * stackedZPrev_, stackedZero,
+              dCurrZ, -eyeNvNv_) // clang-format on
+                .finished();
+    }
+
+    void HoQp::buildFVector() {
+        vector_t zeroVec = vector_t::Zero(numSlackVars_);
+
+        vector_t fMinusDXPrev;
+        if (hasIneqConstraints_) {
+            fMinusDXPrev = task_.f_ - task_.d_ * xPrev_;
+        } else {
+            fMinusDXPrev = vector_t::Zero(0);
+        }
+
+        f_ = (vector_t(2 * numSlackVars_ + numPrevSlackVars_) << zeroVec,
+              stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev)
+                .finished();
+    }
+
+    void HoQp::buildZMatrix() {
+        if (hasEqConstraints_) {
+            assert((task_.a_.cols() > 0));
+            stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel();
+        } else {
+            stackedZ_ = stackedZPrev_;
+        }
+    }
+
+    void HoQp::solveProblem() {
+        auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size());
+        qpOASES::Options options;
+        options.setToMPC();
+        options.printLevel = qpOASES::PL_LOW;
+        qpProblem.setOptions(options);
+        int nWsr = 20;
+
+        qpProblem.init(h_.data(), c_.data(), d_.data(), nullptr, nullptr, nullptr, f_.data(), nWsr);
+        vector_t qpSol(numDecisionVars_ + numSlackVars_);
+
+        qpProblem.getPrimalSolution(qpSol.data());
+
+        decisionVarsSolutions_ = qpSol.head(numDecisionVars_);
+        slackVarsSolutions_ = qpSol.tail(numSlackVars_);
+    }
+
+    void HoQp::stackSlackSolutions() {
+        if (higherProblem_ != nullptr) {
+            stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(),
+                                                         slackVarsSolutions_);
+        } else {
+            stackedSlackVars_ = slackVarsSolutions_;
+        }
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp

@@ -0,0 +1,282 @@
+//
+// Created by qiayuan on 2022/7/1.
+//
+
+#include "ocs2_quadruped_controller/wbc/WbcBase.h"
+
+#include <utility>
+#include <ocs2_centroidal_model/AccessHelperFunctions.h>
+#include <ocs2_centroidal_model/ModelHelperFunctions.h>
+#include <ocs2_core/misc/LoadData.h>
+#include <pinocchio/fwd.hpp>  // forward declarations must be included first.
+#include <pinocchio/algorithm/centroidal.hpp>
+#include <pinocchio/algorithm/crba.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/rnea.hpp>
+
+namespace ocs2::legged_robot {
+    WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
+                     const PinocchioEndEffectorKinematics &eeKinematics)
+        : pinocchio_interface_measured_(pinocchioInterface),
+          pinocchio_interface_desired_(pinocchioInterface),
+          info_(std::move(info)),
+          ee_kinematics_(eeKinematics.clone()),
+          mapping_(info_),
+          input_last_(vector_t::Zero(info_.inputDim)) {
+        num_decision_vars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
+        q_measured_ = vector_t(info_.generalizedCoordinatesNum);
+        v_measured_ = vector_t(info_.generalizedCoordinatesNum);
+    }
+
+    vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired,
+                             const vector_t &rbdStateMeasured, size_t mode,
+                             scalar_t /*period*/) {
+        contact_flag_ = modeNumber2StanceLeg(mode);
+        num_contacts_ = 0;
+        for (const bool flag: contact_flag_) {
+            if (flag) {
+                num_contacts_++;
+            }
+        }
+
+        updateMeasured(rbdStateMeasured);
+        updateDesired(stateDesired, inputDesired);
+
+        return {};
+    }
+
+    void WbcBase::updateMeasured(const vector_t &rbdStateMeasured) {
+        q_measured_.head<3>() = rbdStateMeasured.segment<3>(3);
+        q_measured_.segment<3>(3) = rbdStateMeasured.head<3>();
+        q_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum);
+        v_measured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3);
+        v_measured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity<scalar_t>(
+            q_measured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum));
+        v_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
+            info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum);
+
+        const auto &model = pinocchio_interface_measured_.getModel();
+        auto &data = pinocchio_interface_measured_.getData();
+
+        // For floating base EoM task
+        forwardKinematics(model, data, q_measured_, v_measured_);
+        computeJointJacobians(model, data);
+        updateFramePlacements(model, data);
+        crba(model, data, q_measured_);
+        data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
+        nonLinearEffects(model, data, q_measured_, v_measured_);
+        j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
+            jac.setZero(6, info_.generalizedCoordinatesNum);
+            getFrameJacobian(model, data, info_.endEffectorFrameIndices[i], pinocchio::LOCAL_WORLD_ALIGNED,
+                             jac);
+            j_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) = jac.template topRows<3>();
+        }
+
+        // For not contact motion task
+        computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_);
+        dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
+            jac.setZero(6, info_.generalizedCoordinatesNum);
+            getFrameJacobianTimeVariation(model, data, info_.endEffectorFrameIndices[i],
+                                          pinocchio::LOCAL_WORLD_ALIGNED, jac);
+            dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) = jac.template topRows<3>();
+        }
+    }
+
+    void WbcBase::updateDesired(const vector_t &stateDesired, const vector_t &inputDesired) {
+        const auto &model = pinocchio_interface_desired_.getModel();
+        auto &data = pinocchio_interface_desired_.getData();
+
+        mapping_.setPinocchioInterface(pinocchio_interface_desired_);
+        const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
+        forwardKinematics(model, data, qDesired);
+        computeJointJacobians(model, data, qDesired);
+        updateFramePlacements(model, data);
+        updateCentroidalDynamics(pinocchio_interface_desired_, info_, qDesired);
+        const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
+        forwardKinematics(model, data, qDesired, vDesired);
+    }
+
+    Task WbcBase::formulateFloatingBaseEomTask() {
+        const auto &data = pinocchio_interface_measured_.getData();
+
+        matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
+        s.block(0, 0, info_.actuatedDofNum, 6).setZero();
+        s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity();
+
+        matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, num_decision_vars_) << data.M, -j_.transpose(), -s.
+                      transpose()).finished();
+        vector_t b = -data.nle;
+
+        return {a, b, matrix_t(), vector_t()};
+    }
+
+    Task WbcBase::formulateTorqueLimitsTask() {
+        matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_);
+        d.setZero();
+        matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum);
+        d.block(0, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts, info_.actuatedDofNum,
+                info_.actuatedDofNum) = i;
+        d.block(info_.actuatedDofNum, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts,
+                info_.actuatedDofNum,
+                info_.actuatedDofNum) = -i;
+        vector_t f(2 * info_.actuatedDofNum);
+        for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) {
+            f.segment<3>(3 * l) = torque_limits_;
+        }
+
+        return {matrix_t(), vector_t(), d, f};
+    }
+
+    Task WbcBase::formulateNoContactMotionTask() {
+        matrix_t a(3 * num_contacts_, num_decision_vars_);
+        vector_t b(a.rows());
+        a.setZero();
+        b.setZero();
+        size_t j = 0;
+        for (size_t i = 0; i < info_.numThreeDofContacts; i++) {
+            if (contact_flag_[i]) {
+                a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
+                    3 * i, 0, 3, info_.generalizedCoordinatesNum);
+                b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
+                j++;
+            }
+        }
+
+        return {a, b, matrix_t(), vector_t()};
+    }
+
+    Task WbcBase::formulateFrictionConeTask() {
+        matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
+        a.setZero();
+        size_t j = 0;
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            if (!contact_flag_[i]) {
+                a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3);
+            }
+        }
+        vector_t b(a.rows());
+        b.setZero();
+
+        matrix_t frictionPyramic(5, 3); // clang-format off
+        frictionPyramic << 0, 0, -1,
+                1, 0, -friction_coeff_,
+                -1, 0, -friction_coeff_,
+                0, 1, -friction_coeff_,
+                0, -1, -friction_coeff_; // clang-format on
+
+        matrix_t d(5 * num_contacts_ + 3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
+        d.setZero();
+        j = 0;
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            if (contact_flag_[i]) {
+                d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic;
+            }
+        }
+        vector_t f = Eigen::VectorXd::Zero(d.rows());
+
+        return {a, b, d, f};
+    }
+
+    Task WbcBase::formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period) {
+        matrix_t a(6, num_decision_vars_);
+        a.setZero();
+        a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6);
+
+        vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - input_last_, info_) / period;
+        input_last_ = inputDesired;
+        mapping_.setPinocchioInterface(pinocchio_interface_desired_);
+
+        const auto &model = pinocchio_interface_desired_.getModel();
+        auto &data = pinocchio_interface_desired_.getData();
+        const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
+        const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired);
+
+        const auto &A = getCentroidalMomentumMatrix(pinocchio_interface_desired_);
+        const Matrix6 Ab = A.template leftCols<6>();
+        const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);
+        const auto Aj = A.rightCols(info_.actuatedDofNum);
+        const auto ADot = dccrba(model, data, qDesired, vDesired);
+        Vector6 centroidalMomentumRate = info_.robotMass * getNormalizedCentroidalMomentumRate(
+                                             pinocchio_interface_desired_, info_, inputDesired);
+        centroidalMomentumRate.noalias() -= ADot * vDesired;
+        centroidalMomentumRate.noalias() -= Aj * jointAccel;
+
+        Vector6 b = AbInv * centroidalMomentumRate;
+
+        return {a, b, matrix_t(), vector_t()};
+    }
+
+    Task WbcBase::formulateSwingLegTask() {
+        ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_);
+        std::vector<vector3_t> posMeasured = ee_kinematics_->getPosition(vector_t());
+        std::vector<vector3_t> velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t());
+        ee_kinematics_->setPinocchioInterface(pinocchio_interface_desired_);
+        std::vector<vector3_t> posDesired = ee_kinematics_->getPosition(vector_t());
+        std::vector<vector3_t> velDesired = ee_kinematics_->getVelocity(vector_t(), vector_t());
+
+        matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
+        vector_t b(a.rows());
+        a.setZero();
+        b.setZero();
+        size_t j = 0;
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            if (!contact_flag_[i]) {
+                vector3_t accel = swing_kp_ * (posDesired[i] - posMeasured[i]) + swing_kd_ * (
+                                      velDesired[i] - velMeasured[i]);
+                a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
+                    3 * i, 0, 3, info_.generalizedCoordinatesNum);
+                b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_;
+                j++;
+            }
+        }
+
+        return {a, b, matrix_t(), vector_t()};
+    }
+
+    Task WbcBase::formulateContactForceTask(const vector_t &inputDesired) const {
+        matrix_t a(3 * info_.numThreeDofContacts, num_decision_vars_);
+        vector_t b(a.rows());
+        a.setZero();
+
+        for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
+            a.block(3 * i, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3);
+        }
+        b = inputDesired.head(a.rows());
+
+        return {a, b, matrix_t(), vector_t()};
+    }
+
+    void WbcBase::loadTasksSetting(const std::string &taskFile, const bool verbose) {
+        // Load task file
+        torque_limits_ = vector_t(info_.actuatedDofNum / 4);
+        loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_);
+        if (verbose) {
+            std::cerr << "\n #### Torque Limits Task:";
+            std::cerr << "\n #### =============================================================================\n";
+            std::cerr << "\n #### Hip_joint Thigh_joint Calf_joint: " << torque_limits_.transpose() << "\n";
+            std::cerr << " #### =============================================================================\n";
+        }
+        boost::property_tree::ptree pt;
+        read_info(taskFile, pt);
+        std::string prefix = "frictionConeTask.";
+        if (verbose) {
+            std::cerr << "\n #### Friction Cone Task:";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+        loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose);
+        if (verbose) {
+            std::cerr << " #### =============================================================================\n";
+        }
+        prefix = "swingLegTask.";
+        if (verbose) {
+            std::cerr << "\n #### Swing Leg Task:";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+        loadData::loadPtreeValue(pt, swing_kp_, prefix + "kp", verbose);
+        loadData::loadPtreeValue(pt, swing_kd_, prefix + "kd", verbose);
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp

@@ -0,0 +1,81 @@
+//
+// Created by qiayuan on 22-12-23.
+//
+
+#include "ocs2_quadruped_controller/wbc/WeightedWbc.h"
+
+#include <qpOASES.hpp>
+
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <ocs2_core/misc/LoadData.h>
+
+namespace ocs2::legged_robot {
+    vector_t WeightedWbc::update(const vector_t &stateDesired, const vector_t &inputDesired,
+                                 const vector_t &rbdStateMeasured, size_t mode,
+                                 scalar_t period) {
+        WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
+
+        // Constraints
+        Task constraints = formulateConstraints();
+        size_t numConstraints = constraints.b_.size() + constraints.f_.size();
+
+        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A(numConstraints, getNumDecisionVars());
+        vector_t lbA(numConstraints), ubA(numConstraints); // clang-format off
+        A << constraints.a_,
+                constraints.d_;
+
+        lbA << constraints.b_,
+                -qpOASES::INFTY * vector_t::Ones(constraints.f_.size());
+        ubA << constraints.b_,
+                constraints.f_; // clang-format on
+
+        // Cost
+        Task weighedTask = formulateWeightedTasks(stateDesired, inputDesired, period);
+        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> H =
+                weighedTask.a_.transpose() * weighedTask.a_;
+        vector_t g = -weighedTask.a_.transpose() * weighedTask.b_;
+
+        // Solve
+        auto qpProblem = qpOASES::QProblem(getNumDecisionVars(), numConstraints);
+        qpOASES::Options options;
+        options.setToMPC();
+        options.printLevel = qpOASES::PL_LOW;
+        options.enableEqualities = qpOASES::BT_TRUE;
+        qpProblem.setOptions(options);
+        int nWsr = 20;
+
+        qpProblem.init(H.data(), g.data(), A.data(), nullptr, nullptr, lbA.data(), ubA.data(), nWsr);
+        vector_t qpSol(getNumDecisionVars());
+
+        qpProblem.getPrimalSolution(qpSol.data());
+        return qpSol;
+    }
+
+    Task WeightedWbc::formulateConstraints() {
+        return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +
+               formulateNoContactMotionTask();
+    }
+
+    Task WeightedWbc::formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired,
+                                             scalar_t period) {
+        return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) *
+               weightBaseAccel_ +
+               formulateContactForceTask(inputDesired) * weightContactForce_;
+    }
+
+    void WeightedWbc::loadTasksSetting(const std::string &taskFile, bool verbose) {
+        WbcBase::loadTasksSetting(taskFile, verbose);
+
+        boost::property_tree::ptree pt;
+        read_info(taskFile, pt);
+        const std::string prefix = "weight.";
+        if (verbose) {
+            std::cerr << "\n #### WBC weight:";
+            std::cerr << "\n #### =============================================================================\n";
+        }
+        loadData::loadPtreeValue(pt, weightSwingLeg_, prefix + "swingLeg", verbose);
+        loadData::loadPtreeValue(pt, weightBaseAccel_, prefix + "baseAccel", verbose);
+        loadData::loadPtreeValue(pt, weightContactForce_, prefix + "contactForce", verbose);
+    }
+} // namespace legged

controllers/unitree_guide_controller/README.md

@@ -10,6 +10,7 @@ Tested environment:
 * Ubuntu 22.04
     * ROS2 Humble
 
+[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)
 
 ## 1. Interfaces
 

controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h

@@ -38,8 +38,7 @@ struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
     imu_state_interface_;
 
-    control_input_msgs::msg::Inputs default_inputs_;
-    std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
+    control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
 
     QuadrupedRobot robot_model_;
@@ -48,8 +47,7 @@ struct CtrlComponent {
 
     WaveGenerator wave_generator_;
 
-    CtrlComponent() : control_inputs_(default_inputs_),
-                      robot_model_(*this),
+    CtrlComponent() : robot_model_(*this),
                       estimator_(*this) {
     }
 

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -36,11 +36,11 @@ void StateBalanceTest::enter() {
 }
 
 void StateBalanceTest::run() {
-    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
-    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
-    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
+    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.ly, _xMin, _xMax);
+    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.lx, _yMin, _yMax);
+    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.ry, _zMin, _zMax);
 
-    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
+    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.rx, _yawMin, _yawMax);
     Rd_ = rotz(yaw) * init_rotation_;
 
     for (int i = 0; i < 12; i++) {
@@ -56,7 +56,7 @@ void StateBalanceTest::exit() {
 }
 
 FSMStateName StateBalanceTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
             return FSMStateName::FIXEDDOWN;
         case 2:

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

@@ -15,7 +15,7 @@ void StateFixedDown::enter() {
     for (int i = 0; i < 12; i++) {
         start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
     }
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFixedDown::run() {
@@ -39,7 +39,7 @@ FSMStateName StateFixedDown::checkChange() {
     if (percent_ < 1.5) {
         return FSMStateName::FIXEDDOWN;
     }
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -15,7 +15,7 @@ void StateFixedStand::enter() {
     for (int i = 0; i < 12; i++) {
         start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
     }
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFixedStand::run() {
@@ -40,16 +40,18 @@ FSMStateName StateFixedStand::checkChange() {
     if (percent_ < 1.5) {
         return FSMStateName::FIXEDSTAND;
     }
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
             return FSMStateName::FIXEDDOWN;
         case 3:
             return FSMStateName::FREESTAND;
         case 4:
             return FSMStateName::TROTTING;
-        case 9:
+        case 5:
             return FSMStateName::SWINGTEST;
-        case 10:
+        case 6:
             return FSMStateName::BALANCETEST;
         default:
             return FSMStateName::FIXEDSTAND;

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -33,23 +33,23 @@ void StateFreeStand::enter() {
         foot_pos.p -= fr_init_pos_.p;
         foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
     }
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFreeStand::run() {
-    calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().ry, height_min_, height_max_));
+    calc_body_target(invNormalize(ctrl_comp_.control_inputs_.lx, row_min_, row_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.ly, pitch_min_, pitch_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.rx, yaw_min_, yaw_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.ry, height_min_, height_max_));
 }
 
 void StateFreeStand::exit() {
 }
 
 FSMStateName StateFreeStand::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:

controllers/unitree_guide_controller/src/FSM/StatePassive.cpp

@@ -27,7 +27,7 @@ void StatePassive::enter() {
     for (auto i: ctrl_comp_.joint_kd_command_interface_) {
         i.get().set_value(1);
     }
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StatePassive::run() {
@@ -37,7 +37,7 @@ void StatePassive::exit() {
 }
 
 FSMStateName StatePassive::checkChange() {
-    if (ctrl_comp_.control_inputs_.get().command == 2) {
+    if (ctrl_comp_.control_inputs_.command == 2) {
         return FSMStateName::FIXEDDOWN;
     }
     return FSMStateName::PASSIVE;

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -38,25 +38,25 @@ void StateSwingTest::enter() {
 }
 
 void StateSwingTest::run() {
-    if (ctrl_comp_.control_inputs_.get().ly > 0) {
-        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x(),
+    if (ctrl_comp_.control_inputs_.ly > 0) {
+        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x(),
                                       fr_init_pos_.p.x() + _xMax, 0, 1));
     } else {
-        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x() + _xMin,
+        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
                                       fr_init_pos_.p.x(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.get().lx > 0) {
-        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y(),
+    if (ctrl_comp_.control_inputs_.lx > 0) {
+        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y(),
                                       fr_init_pos_.p.y() + _yMax, 0, 1));
     } else {
-        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y() + _yMin,
+        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
                                       fr_init_pos_.p.y(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.get().ry > 0) {
-        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z(),
+    if (ctrl_comp_.control_inputs_.ry > 0) {
+        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z(),
                                       fr_init_pos_.p.z() + _zMax, 0, 1));
     } else {
-        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin,
+        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
                                       fr_init_pos_.p.z(), -1, 0));
     }
 
@@ -68,9 +68,9 @@ void StateSwingTest::exit() {
 }
 
 FSMStateName StateSwingTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -34,7 +34,7 @@ void StateTrotting::enter() {
     Rd = rotz(yaw_cmd_);
     w_cmd_global_.setZero();
 
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
     gait_generator_.restart();
 }
 
@@ -68,9 +68,9 @@ void StateTrotting::exit() {
 }
 
 FSMStateName StateTrotting::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:
@@ -80,12 +80,12 @@ FSMStateName StateTrotting::checkChange() {
 
 void StateTrotting::getUserCmd() {
     /* Movement */
-    v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, v_x_limit_(0), v_x_limit_(1));
-    v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, v_y_limit_(0), v_y_limit_(1));
+    v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.ly, v_x_limit_(0), v_x_limit_(1));
+    v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.lx, v_y_limit_(0), v_y_limit_(1));
     v_cmd_body_(2) = 0;
 
     /* Turning */
-    d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.get().rx, w_yaw_limit_(0), w_yaw_limit_(1));
+    d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.rx, w_yaw_limit_(0), w_yaw_limit_(1));
     d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_;
     d_yaw_cmd_past_ = d_yaw_cmd_;
 }

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -105,11 +105,11 @@ namespace unitree_guide_controller {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
             "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
                 // Handle message
-                ctrl_comp_.control_inputs_.get().command = msg->command;
-                ctrl_comp_.control_inputs_.get().lx = msg->lx;
-                ctrl_comp_.control_inputs_.get().ly = msg->ly;
-                ctrl_comp_.control_inputs_.get().rx = msg->rx;
-                ctrl_comp_.control_inputs_.get().ry = msg->ry;
+                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_comp_.control_inputs_.ry = msg->ry;
             });
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(

descriptions/aliengo_description/CMakeLists.txt

@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(aliengo_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+        DIRECTORY meshes xacro launch config urdf
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

descriptions/aliengo_description/README.md

@@ -0,0 +1,34 @@
+# Unitree AlienGo Description
+This repository contains the urdf model of Aliengo.
+
+![Aliengo](../../.images/aliengo.png)
+
+Tested environment:
+* Ubuntu 24.04
+    * ROS2 Jazzy
+
+## Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to aliengo_description
+```
+
+## Visualize the robot
+To visualize and check the configuration of the robot in rviz, simply launch:
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch aliengo_description visualize.launch.py
+```
+
+## Launch ROS2 Control
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description ocs2_control.launch.py
+  ```
+

descriptions/aliengo_description/config/ocs2/gait.info

@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}

descriptions/aliengo_description/config/ocs2/reference.info

@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.4
+
+defaultJointState
+{
+   (0,0)  -0.10   ; FL_hip_joint
+   (1,0)   0.62   ; FL_thigh_joint
+   (2,0)  -1.24   ; FL_calf_joint
+   (3,0)   0.10   ; FR_hip_joint
+   (4,0)   0.62   ; FR_thigh_joint
+   (5,0)  -1.24   ; FR_calf_joint
+   (6,0)  -0.10   ; RL_hip_joint
+   (7,0)   0.62   ; RL_thigh_joint
+   (8,0)  -1.24   ; RL_calf_joint
+   (9,0)   0.10   ; RR_hip_joint
+   (10,0)  0.62   ; RR_thigh_joint
+   (11,0) -1.24   ; RR_calf_joint
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}

descriptions/aliengo_description/config/ocs2/task.info

@@ -0,0 +1,319 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0
+  phaseTransitionStanceTime     0.1
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/aliengo
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.2
+  touchDownVelocity            -0.4
+  swingHeight                   0.1
+  swingTimeScale                0.15
+}
+
+; DDP settings
+ddp
+{
+  algorithm                       SLQ
+
+  nThreads                        3
+  threadPriority                  50
+
+  maxNumIterations                1
+  minRelCost                      1e-1
+  constraintTolerance             5e-3
+
+  displayInfo                     false
+  displayShortSummary             false
+  checkNumericalStability         false
+  debugPrintRollout               false
+  debugCaching                    false
+
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  maxNumStepsPerSecond            10000
+  timeStep                        0.015
+  backwardPassIntegratorType      ODE45
+
+  constraintPenaltyInitialValue   20.0
+  constraintPenaltyIncreaseRate   2.0
+
+  preComputeRiccatiTerms          true
+
+  useFeedbackPolicy               false
+
+  strategy                        LINE_SEARCH
+  lineSearch
+  {
+    minStepLength                 1e-2
+    maxStepLength                 1.0
+    hessianCorrectionStrategy     DIAGONAL_SHIFT
+    hessianCorrectionMultiple     1e-5
+  }
+}
+
+
+; Multiple_Shooting SQP settings
+sqp
+{
+  nThreads                              3
+  dt                                    0.015
+  sqpIteration                          1
+  deltaTol                              1e-4
+  g_max                                 1e-2
+  g_min                                 1e-6
+  inequalityConstraintMu                0.1
+  inequalityConstraintDelta             5.0
+  projectStateInputEqualityConstraints  true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+}
+
+; Multiple_Shooting IPM settings
+ipm
+{
+  nThreads                              3
+  dt                                    0.015
+  ipmIteration                          1
+  deltaTol                              1e-4
+  g_max                                 10.0
+  g_min                                 1e-6
+  computeLagrangeMultipliers            true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+
+  initialBarrierParameter               1e-4
+  targetBarrierParameter                1e-4
+  barrierLinearDecreaseFactor           0.2
+  barrierSuperlinearDecreasePower       1.5
+  barrierReductionCostTol               1e-3
+  barrierReductionConstraintTol         1e-3
+
+  fractionToBoundaryMargin              0.995
+  usePrimalStepSizeForDual              false
+
+  initialSlackLowerBound                1e-4
+  initialDualLowerBound                 1e-4
+  initialSlackMarginRate                1e-2
+  initialDualMarginRate                 1e-2
+}
+
+; Rollout settings
+rollout
+{
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  timeStep                        0.015
+  integratorType                  ODE45
+  maxNumStepsPerSecond            10000
+  checkNumericalStability         false
+}
+
+mpc
+{
+  timeHorizon                     1.0  ; [s]
+  solutionTimeWindow              -1   ; maximum [s]
+  coldStart                       false
+
+  debugPrint                      false
+
+  mpcDesiredFrequency             100  ; [Hz]
+  mrtDesiredFrequency             1000 ; [Hz] Useless
+}
+
+initialState
+{
+   ;; Normalized Centroidal Momentum: [linear, angular] ;;
+   (0,0)  0.0     ; vcom_x
+   (1,0)  0.0     ; vcom_y
+   (2,0)  0.0     ; vcom_z
+   (3,0)  0.0     ; L_x / robotMass
+   (4,0)  0.0     ; L_y / robotMass
+   (5,0)  0.0     ; L_z / robotMass
+
+   ;; Base Pose: [position, orientation] ;;
+   (6,0)  0.0     ; p_base_x
+   (7,0)  0.0     ; p_base_y
+   (8,0)  0.3     ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+   (12,0) -0.20   ; FL_hip_joint
+   (13,0)  0.62   ; FL_thigh_joint
+   (14,0) -1.24   ; FL_calf_joint
+   (15,0) -0.20   ; RL_hip_joint
+   (16,0)  0.62   ; RL_thigh_joint
+   (17,0) -1.24   ; RL_calf_joint
+   (18,0)  0.20   ; FR_hip_joint
+   (19,0)  0.62   ; FR_thigh_joint
+   (20,0) -1.24   ; FR_calf_joint
+   (21,0)  0.20   ; RR_hip_joint
+   (22,0)  0.62   ; RR_thigh_joint
+   (23,0) -1.24   ; RR_calf_joint
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   100.0    ; vcom_z
+  (3,3)   10.0     ; L_x / robotMass
+  (4,4)   30.0     ; L_y / robotMass
+  (5,5)   30.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   1000.0   ; p_base_x
+  (7,7)   1000.0   ; p_base_y
+  (8,8)   1500.0   ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 300.0    ; theta_base_y
+  (11,11) 300.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+  (12,12) 5.0     ; FL_hip_joint
+  (13,13) 5.0     ; FL_thigh_joint
+  (14,14) 2.5     ; FL_calf_joint
+  (15,15) 5.0     ; RL_hip_joint
+  (16,16) 5.0     ; RL_thigh_joint
+  (17,17) 2.5     ; RL_calf_joint
+  (18,18) 5.0     ; FR_hip_joint
+  (19,19) 5.0     ; FR_thigh_joint
+  (20,20) 2.5     ; FR_calf_joint
+  (21,21) 5.0     ; RR_hip_joint
+  (22,22) 5.0     ; RR_thigh_joint
+  (23,23) 2.5     ; RR_calf_joint
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, RL, RR] ;;
+  (0,0)   5.0       ; front_left_force
+  (1,1)   5.0       ; front_left_force
+  (2,2)   5.0       ; front_left_force
+  (3,3)   5.0       ; front_right_force
+  (4,4)   5.0       ; front_right_force
+  (5,5)   5.0       ; front_right_force
+  (6,6)   5.0       ; rear_left_force
+  (7,7)   5.0       ; rear_left_force
+  (8,8)   5.0       ; rear_left_force
+  (9,9)   5.0       ; rear_right_force
+  (10,10) 5.0       ; rear_right_force
+  (11,11) 5.0       ; rear_right_force
+
+  ;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.3
+
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "FL_calf, FR_calf"
+    [1] "RL_calf, RR_calf"
+    [2] "FL_calf, RL_calf"
+    [3] "FR_calf, RR_calf"
+    [4] "FL_foot, FR_foot"
+    [5] "RL_foot, RR_foot"
+    [6] "FL_foot, RL_foot"
+    [7] "FR_foot, RR_foot"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  35.278     ; HAA
+   (1,0)  35.278     ; HFE
+   (2,0)  44.4       ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

descriptions/aliengo_description/config/robot_control.yaml

@@ -0,0 +1,131 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    # Define the available controllers
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+    ocs2_quadruped_controller:
+      type: ocs2_quadruped_controller/Ocs2QuadrupedController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 500  # Hz
+    joints:
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - FL_hip_joint
+      - FL_thigh_joint
+      - FL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - FR_foot
+      - FL_foot
+      - RR_foot
+      - RL_foot
+
+    imu_name: "imu_sensor"
+    base_name: "base"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z
+
+ocs2_quadruped_controller:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    joints:
+      - FL_hip_joint
+      - FL_thigh_joint
+      - FL_calf_joint
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - FL_foot
+      - FR_foot
+      - RL_foot
+      - RR_foot
+
+
+    imu_name: "imu_sensor"
+    base_name: "base"
+
+    imu_interfaces:
+      - orientation.w
+      - orientation.x
+      - orientation.y
+      - orientation.z
+      - angular_velocity.x
+      - angular_velocity.y
+      - angular_velocity.z
+      - linear_acceleration.x
+      - linear_acceleration.y
+      - linear_acceleration.z
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR

descriptions/aliengo_description/config/visualize_urdf.rviz

@@ -0,0 +1,383 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 303
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        FL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RL_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RL_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_calf_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_foot:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_hip_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RR_thigh_rotor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_chin:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_laserscan_link_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_laserscan_link_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_optical_chin:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_rearDown:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_optical_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rearDown:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        trunk:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_face:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ultraSound_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 0.8687032461166382
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.017344314604997635
+        Y: -0.0033522010780870914
+        Z: -0.09058035165071487
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.49539798498153687
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.8403961062431335
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 630
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003130000005efc0100000002fb0000000800540069006d0065010000000000000313000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000313000001b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 787
+  X: 763
+  Y: 351

descriptions/aliengo_description/launch/ocs2_control.launch.py

@@ -0,0 +1,122 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from sympy.physics.vector.printing import params
+
+package_description = "aliengo_description"
+package_controller = "ocs2_quadruped_controller"
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers,
+                    {
+                        'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'),
+                        'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
+                                                  'task.info'),
+                        'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'reference.info'),
+                        'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'gait.info')
+                    }],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    ocs2_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[ocs2_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='aliengo',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])

descriptions/aliengo_description/launch/unitree_guide.launch.py

@@ -0,0 +1,112 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+package_description = "aliengo_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[unitree_guide_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='aliengo',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])

descriptions/aliengo_description/launch/visualize.launch.py

@@ -0,0 +1,66 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch_ros.actions import Node
+
+import xacro
+
+package_description = "aliengo_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return robot_description_config.toxml()
+
+
+def launch_setup(context, *args, **kwargs):
+    robot_description = process_xacro(context)
+    return [
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[
+                {
+                    'publish_frequency': 100.0,
+                    'use_tf_static': True,
+                    'robot_description': robot_description
+                }
+            ],
+        ),
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher',
+            output='screen',
+        )
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='aliengo',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])

descriptions/aliengo_description/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>aliengo_description</name>
+    <version>0.0.0</version>
+    <description>The aliengo_description package</description>
+
+    <maintainer email="aliengo@unitree.cc">unitree</maintainer>
+    <license>TODO</license>
+
+    <exec_depend>xacro</exec_depend>
+    <exec_depend>joint_state_publisher</exec_depend>
+    <exec_depend>robot_state_publisher</exec_depend>
+    <exec_depend>imu_sensor_broadcaster</exec_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+
+</package>

descriptions/aliengo_description/xacro/const.xacro

@@ -0,0 +1,160 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<!-- Constants for robot dimensions -->
+	<xacro:property name="PI" value="3.1415926535897931"/>
+	<xacro:property name="stick_mass" value="0.00001"/>
+
+	<!-- simplified collision value -->
+	<xacro:property name="trunk_width" value="0.150"/>
+	<xacro:property name="trunk_length" value="0.647"/>
+	<xacro:property name="trunk_height" value="0.112"/>
+	<xacro:property name="hip_radius" value="0.046"/>
+	<xacro:property name="hip_length" value="0.0418"/>
+	<xacro:property name="thigh_shoulder_radius" value="0.044"/>
+	<xacro:property name="thigh_shoulder_length" value="0.08"/>
+	<xacro:property name="thigh_width" value="0.0374"/>
+	<xacro:property name="thigh_height" value="0.043"/>
+	<xacro:property name="calf_width" value="0.0208"/>
+	<xacro:property name="calf_height" value="0.016"/>
+	<xacro:property name="foot_radius" value="0.0265"/>
+	<xacro:property name="stick_radius" value="0.01"/>
+	<xacro:property name="stick_length" value="0.2"/>
+
+	<!-- kinematic value -->
+	<xacro:property name="thigh_offset" value="0.0868"/>
+	<xacro:property name="thigh_length" value="0.25"/>
+	<xacro:property name="calf_length" value="0.25"/>
+
+	<!-- leg offset from trunk center value -->
+	<xacro:property name="leg_offset_x" value="0.2407"/>
+	<xacro:property name="leg_offset_y" value="0.051"/>
+	<xacro:property name="trunk_offset_z" value="0.01675"/>
+	<xacro:property name="hip_offset" value="0.083"/>
+
+	<!-- offset of link and rotor locations (left front) -->
+	<xacro:property name="hip_offset_x" value="0.2407"/>
+	<xacro:property name="hip_offset_y" value="0.051"/>
+	<xacro:property name="hip_offset_z" value="0.0"/>
+	<xacro:property name="hip_rotor_offset_x" value="0.139985"/>
+	<xacro:property name="hip_rotor_offset_y" value="0.051"/>
+	<xacro:property name="hip_rotor_offset_z" value="0.0"/>
+
+	<xacro:property name="thigh_offset_x" value="0"/>
+	<xacro:property name="thigh_offset_y" value="0.0868"/>
+	<xacro:property name="thigh_offset_z" value="0.0"/>
+	<xacro:property name="thigh_rotor_offset_x" value="0.0"/>
+	<xacro:property name="thigh_rotor_offset_y" value="0.0298"/>
+	<xacro:property name="thigh_rotor_offset_z" value="0.0"/>
+
+	<xacro:property name="calf_offset_x" value="0.0"/>
+	<xacro:property name="calf_offset_y" value="0.0"/>
+	<xacro:property name="calf_offset_z" value="-0.25"/>
+	<xacro:property name="calf_rotor_offset_x" value="0.0"/>
+	<xacro:property name="calf_rotor_offset_y" value="-0.0997"/>
+	<xacro:property name="calf_rotor_offset_z" value="0.0"/>
+
+
+	<!-- joint limits -->
+	<xacro:property name="damping" value="0"/>
+	<xacro:property name="friction" value="0"/>
+	<xacro:property name="hip_position_max" value="${70*PI/180.0}"/>
+	<xacro:property name="hip_position_min" value="${-70*PI/180.0}"/>
+	<xacro:property name="hip_velocity_max" value="20"/>
+	<xacro:property name="hip_torque_max" value="35.278"/>
+	<xacro:property name="thigh_position_max" value="${240*PI/180.0}"/>
+	<xacro:property name="thigh_position_min" value="${-120*PI/180.0}"/>
+	<xacro:property name="thigh_velocity_max" value="20"/>
+	<xacro:property name="thigh_torque_max" value="35.278"/>
+	<xacro:property name="calf_position_max" value="${-37*PI/180.0}"/>
+	<xacro:property name="calf_position_min" value="${-159*PI/180.0}"/>
+	<xacro:property name="calf_velocity_max" value="15.89"/>
+	<xacro:property name="calf_torque_max" value="44.4"/>
+
+	<!-- dynamics inertial value        total 23.0kg -->
+	<!-- trunk -->
+	<xacro:property name="trunk_mass" value="11.644"/>
+	<xacro:property name="trunk_com_x" value="0.008811"/>
+	<xacro:property name="trunk_com_y" value="0.003839"/>
+	<xacro:property name="trunk_com_z" value="0.000273"/>
+	<xacro:property name="trunk_ixx" value="0.051944892"/>
+	<xacro:property name="trunk_ixy" value="0.001703617"/>
+	<xacro:property name="trunk_ixz" value="0.000235941"/>
+	<xacro:property name="trunk_iyy" value="0.24693924"/>
+	<xacro:property name="trunk_iyz" value="0.000119783"/>
+	<xacro:property name="trunk_izz" value="0.270948307"/>
+
+	<!-- hip (left front) -->
+	<xacro:property name="hip_mass" value="1.993"/>
+	<xacro:property name="hip_com_x" value="-0.022191"/>
+	<xacro:property name="hip_com_y" value="0.015144"/>
+	<xacro:property name="hip_com_z" value="-0.000015"/>
+	<xacro:property name="hip_ixx" value="0.002446735"/>
+	<xacro:property name="hip_ixy" value="-0.00059805"/>
+	<xacro:property name="hip_ixz" value="0.000001945"/>
+	<xacro:property name="hip_iyy" value="0.003925876"/>
+	<xacro:property name="hip_iyz" value="0.000001284"/>
+	<xacro:property name="hip_izz" value="0.004148145"/>
+
+	<xacro:property name="hip_rotor_mass" value="0.146"/>
+	<xacro:property name="hip_rotor_com_x" value="0.0"/>
+	<xacro:property name="hip_rotor_com_y" value="0.0"/>
+	<xacro:property name="hip_rotor_com_z" value="0.0"/>
+	<xacro:property name="hip_rotor_ixx" value="0.000138702"/>
+	<xacro:property name="hip_rotor_ixy" value="0.0"/>
+	<xacro:property name="hip_rotor_ixz" value="0.0"/>
+	<xacro:property name="hip_rotor_iyy" value="0.000083352"/>
+	<xacro:property name="hip_rotor_iyz" value="0.0"/>
+	<xacro:property name="hip_rotor_izz" value="0.000083352"/>
+
+	<!-- thigh -->
+	<xacro:property name="thigh_mass" value="0.639"/>
+	<xacro:property name="thigh_com_x" value="-0.005607"/>
+	<xacro:property name="thigh_com_y" value="-0.003877"/>
+	<xacro:property name="thigh_com_z" value="-0.048199"/>
+	<xacro:property name="thigh_ixx" value="0.004173855"/>
+	<xacro:property name="thigh_ixy" value="0.000010284"/>
+	<xacro:property name="thigh_ixz" value="-0.000318874"/>
+	<xacro:property name="thigh_iyy" value="0.004343802"/>
+	<xacro:property name="thigh_iyz" value="0.000109233"/>
+	<xacro:property name="thigh_izz" value="0.000340136"/>
+
+	<xacro:property name="thigh_rotor_mass" value="0.146"/>
+	<xacro:property name="thigh_rotor_com_x" value="0.0"/>
+	<xacro:property name="thigh_rotor_com_y" value="0.0"/>
+	<xacro:property name="thigh_rotor_com_z" value="0.0"/>
+	<xacro:property name="thigh_rotor_ixx" value="0.000083352"/>
+	<xacro:property name="thigh_rotor_ixy" value="0.0"/>
+	<xacro:property name="thigh_rotor_ixz" value="0.0"/>
+	<xacro:property name="thigh_rotor_iyy" value="0.000138702"/>
+	<xacro:property name="thigh_rotor_iyz" value="0.0"/>
+	<xacro:property name="thigh_rotor_izz" value="0.000083352"/>
+
+	<!-- calf -->
+	<xacro:property name="calf_mass" value="0.207"/>
+	<xacro:property name="calf_com_x" value="0.002781"/>
+	<xacro:property name="calf_com_y" value="0.000063"/>
+	<xacro:property name="calf_com_z" value="-0.142518"/>
+	<xacro:property name="calf_ixx" value="0.002129279"/>
+	<xacro:property name="calf_ixy" value="0.000000039"/>
+	<xacro:property name="calf_ixz" value="0.000005757"/>
+	<xacro:property name="calf_iyy" value="0.002141463"/>
+	<xacro:property name="calf_iyz" value="-0.000000516"/>
+	<xacro:property name="calf_izz" value="0.000037583"/>
+
+	<xacro:property name="calf_rotor_mass" value="0.132"/>
+	<xacro:property name="calf_rotor_com_x" value="0.0"/>
+	<xacro:property name="calf_rotor_com_y" value="0.0"/>
+	<xacro:property name="calf_rotor_com_z" value="0.0"/>
+	<xacro:property name="calf_rotor_ixx" value="0.000145463"/>
+	<xacro:property name="calf_rotor_ixy" value="0.0"/>
+	<xacro:property name="calf_rotor_ixz" value="0.0"/>
+	<xacro:property name="calf_rotor_iyy" value="0.000133031"/>
+	<xacro:property name="calf_rotor_iyz" value="0.0"/>
+	<xacro:property name="calf_rotor_izz" value="0.000145463"/>
+
+	<!-- foot -->
+	<xacro:property name="foot_mass" value="0.06"/>
+
+</robot>

descriptions/aliengo_description/xacro/gazebo.xacro

@@ -0,0 +1,266 @@
+<?xml version="1.0"?>
+<robot>
+	<!-- ros_control plugin -->
+	<gazebo>
+		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+			<robotNamespace>/aliengo_gazebo</robotNamespace>
+			<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+		</plugin>
+	</gazebo>
+
+	<!-- Show the trajectory of trunk center. -->
+	<gazebo>
+		<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+			<frequency>10</frequency>
+			<plot>
+				<link>base</link>
+				<pose>0 0 0 0 0 0</pose>
+				<material>Gazebo/Yellow</material>
+			</plot>
+		</plugin>
+	</gazebo>
+
+	<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+	<!-- <gazebo>
+			<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+					<frequency>100</frequency>
+					<plot>
+							<link>FL_foot</link>
+							<pose>0 0 0 0 0 0</pose>
+							<material>Gazebo/Green</material>
+					</plot>
+			</plugin>
+	</gazebo> -->
+
+	<gazebo>
+		<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+			<bodyName>trunk</bodyName>
+			<topicName>/apply_force/trunk</topicName>
+		</plugin>
+	</gazebo>
+
+	<gazebo reference="imu_link">
+		<gravity>true</gravity>
+		<sensor name="imu_sensor" type="imu">
+			<always_on>true</always_on>
+			<update_rate>1000</update_rate>
+			<visualize>true</visualize>
+			<topic>__default_topic__</topic>
+			<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+				<topicName>trunk_imu</topicName>
+				<bodyName>imu_link</bodyName>
+				<updateRateHZ>1000.0</updateRateHZ>
+				<gaussianNoise>0.0</gaussianNoise>
+				<xyzOffset>0 0 0</xyzOffset>
+				<rpyOffset>0 0 0</rpyOffset>
+				<frameName>imu_link</frameName>
+			</plugin>
+			<pose>0 0 0 0 0 0</pose>
+		</sensor>
+	</gazebo>
+
+	<!-- Foot contacts. -->
+	<gazebo reference="FR_calf">
+		<sensor name="FR_foot_contact" type="contact">
+			<update_rate>100</update_rate>
+			<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+			<contact>
+				<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+			</contact>
+		</sensor>
+	</gazebo>
+	<gazebo reference="FL_calf">
+		<sensor name="FL_foot_contact" type="contact">
+			<update_rate>100</update_rate>
+			<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+			<contact>
+				<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+			</contact>
+		</sensor>
+	</gazebo>
+	<gazebo reference="RR_calf">
+		<sensor name="RR_foot_contact" type="contact">
+			<update_rate>100</update_rate>
+			<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+			<contact>
+				<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+			</contact>
+		</sensor>
+	</gazebo>
+	<gazebo reference="RL_calf">
+		<sensor name="RL_foot_contact" type="contact">
+			<update_rate>100</update_rate>
+			<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+			<contact>
+				<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+			</contact>
+		</sensor>
+	</gazebo>
+
+	<!-- Visualization of Foot contacts. -->
+	<gazebo reference="FR_foot">
+		<visual>
+			<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+				<topicName>FR_foot_contact</topicName>
+			</plugin>
+		</visual>
+	</gazebo>
+	<gazebo reference="FL_foot">
+		<visual>
+			<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+				<topicName>FL_foot_contact</topicName>
+			</plugin>
+		</visual>
+	</gazebo>
+	<gazebo reference="RR_foot">
+		<visual>
+			<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+				<topicName>RR_foot_contact</topicName>
+			</plugin>
+		</visual>
+	</gazebo>
+	<gazebo reference="RL_foot">
+		<visual>
+			<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+				<topicName>RL_foot_contact</topicName>
+			</plugin>
+		</visual>
+	</gazebo>
+
+	<gazebo reference="base">
+		<material>Gazebo/Green</material>
+		<turnGravityOff>false</turnGravityOff>
+	</gazebo>
+
+	<gazebo reference="trunk">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+	<gazebo reference="stick_link">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/White</material>
+	</gazebo>
+
+	<gazebo reference="imu_link">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/Red</material>
+	</gazebo>
+
+	<!-- FL leg -->
+	<gazebo reference="FL_hip">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/DarkGrey</material>
+	</gazebo>
+	<gazebo reference="FL_thigh">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FL_calf">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="FL_foot">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+	<!-- FR leg -->
+	<gazebo reference="FR_hip">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/DarkGrey</material>
+	</gazebo>
+	<gazebo reference="FR_thigh">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FR_calf">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="FR_foot">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+	<!-- RL leg -->
+	<gazebo reference="RL_hip">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/DarkGrey</material>
+	</gazebo>
+	<gazebo reference="RL_thigh">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="RL_calf">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="RL_foot">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+	<!-- RR leg -->
+	<gazebo reference="RR_hip">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<material>Gazebo/DarkGrey</material>
+	</gazebo>
+	<gazebo reference="RR_thigh">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="RR_calf">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="RR_foot">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<material>Gazebo/DarkGrey</material>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+</robot>

descriptions/aliengo_description/xacro/leg.xacro

@@ -0,0 +1,255 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find aliengo_description)/xacro/transmission.xacro"/>
+
+  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+
+    <joint name="${name}_hip_joint" type="revolute">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip"/>
+      <axis xyz="1 0 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <xacro:if value="${(mirror_dae == True)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
+               upper="${hip_position_max}"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
+               upper="${-hip_position_min}"/>
+      </xacro:if>
+    </joint>
+
+    <joint name="${name}_hip_rotor_joint" type="fixed">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip_rotor"/>
+    </joint>
+
+    <link name="${name}_hip">
+      <visual>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+          <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+          <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+          <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <geometry>
+          <mesh filename="file://$(find aliengo_description)/meshes/hip.dae" scale="1 1 1"/>
+        </geometry>
+        <material name="orange"/>
+      </visual>
+      <collision>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+        <geometry>
+          <cylinder length="${hip_length}" radius="${hip_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+        <mass value="${hip_mass}"/>
+        <inertia
+            ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+            iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+            izz="${hip_izz}"/>
+      </inertial>
+    </link>
+
+    <link name="${name}_hip_rotor">
+      <visual>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+        <mass value="${hip_rotor_mass}"/>
+        <inertia
+            ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+            iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+            izz="${hip_rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_thigh_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
+             upper="${thigh_position_max}"/>
+    </joint>
+
+    <joint name="${name}_thigh_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh_rotor"/>
+    </joint>
+
+    <link name="${name}_thigh">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <xacro:if value="${mirror_dae == True}">
+            <mesh filename="file://$(find aliengo_description)/meshes/thigh.dae" scale="1 1 1"/>
+          </xacro:if>
+          <xacro:if value="${mirror_dae == False}">
+            <mesh filename="file://$(find aliengo_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
+          </xacro:if>
+        </geometry>
+        <material name="orange"/>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
+        <geometry>
+          <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+        <mass value="${thigh_mass}"/>
+        <inertia
+            ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+            iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+            izz="${thigh_izz}"/>
+      </inertial>
+    </link>
+
+    <link name="${name}_thigh_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+        <mass value="${thigh_rotor_mass}"/>
+        <inertia
+            ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+            iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+            izz="${thigh_rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_calf_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
+             upper="${calf_position_max}"/>
+    </joint>
+
+    <joint name="${name}_calf_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf_rotor"/>
+    </joint>
+
+    <link name="${name}_calf">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find aliengo_description)/meshes/calf.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
+        <geometry>
+          <box size="${calf_length} ${calf_width} ${calf_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+        <mass value="${calf_mass}"/>
+        <inertia
+            ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+            iyy="${calf_iyy}" iyz="${calf_iyz}"
+            izz="${calf_izz}"/>
+      </inertial>
+    </link>
+
+    <link name="${name}_calf_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+        <mass value="${calf_rotor_mass}"/>
+        <inertia
+            ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+            iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+            izz="${calf_rotor_izz}"/>
+      </inertial>
+    </link>
+
+    <joint name="${name}_foot_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
+      <parent link="${name}_calf"/>
+      <child link="${name}_foot"/>
+    </joint>
+
+    <link name="${name}_foot">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius-0.01}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="${foot_mass}"/>
+        <inertia
+            ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+            iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+            izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
+      </inertial>
+    </link>
+
+    <xacro:leg_transmission name="${name}"/>
+  </xacro:macro>
+</robot>

descriptions/aliengo_description/xacro/materials.xacro

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot>
+
+	<material name="black">
+		<color rgba="0.0 0.0 0.0 1.0"/>
+	</material>
+
+	<material name="blue">
+		<color rgba="0.0 0.0 0.8 1.0"/>
+	</material>
+
+	<material name="green">
+		<color rgba="0.0 0.8 0.0 1.0"/>
+	</material>
+
+	<material name="grey">
+		<color rgba="0.2 0.2 0.2 1.0"/>
+	</material>
+
+	<material name="silver">
+		<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
+	</material>
+
+	<material name="orange">
+		<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+	</material>
+
+	<material name="brown">
+		<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+	</material>
+
+	<material name="red">
+		<color rgba="0.8 0.0 0.0 1.0"/>
+	</material>
+
+	<material name="white">
+		<color rgba="1.0 1.0 1.0 1.0"/>
+	</material>
+
+</robot>

descriptions/aliengo_description/xacro/robot.xacro

@@ -0,0 +1,99 @@
+<?xml version="1.0"?>
+
+<robot name="aliengo" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<xacro:arg name="DEBUG" default="false"/>
+
+	<xacro:include filename="$(find aliengo_description)/xacro/const.xacro"/>
+	<xacro:include filename="$(find aliengo_description)/xacro/materials.xacro"/>
+	<xacro:include filename="$(find aliengo_description)/xacro/leg.xacro"/>
+	<xacro:include filename="$(find aliengo_description)/xacro/ros2_control.xacro"/>
+	<!--    <xacro:include filename="$(find aliengo_description)/xacro/gazebo.xacro"/>-->
+
+	<!-- Rotor related joint and link is only for demonstrate location. -->
+	<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+
+	<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+	<xacro:if value="$(arg DEBUG)">
+		<link name="world"/>
+		<joint name="base_static_joint" type="fixed">
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<parent link="world"/>
+			<child link="base"/>
+		</joint>
+	</xacro:if>
+
+	<link name="base">
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<box size="0.001 0.001 0.001"/>
+			</geometry>
+		</visual>
+	</link>
+
+	<joint name="floating_base" type="fixed">
+		<origin rpy="0 0 0" xyz="0 0 0"/>
+		<parent link="base"/>
+		<child link="trunk"/>
+	</joint>
+
+	<link name="trunk">
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="file://$(find aliengo_description)/meshes/trunk.dae" scale="1 1 1"/>
+			</geometry>
+			<material name="orange"/>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
+			<mass value="${trunk_mass}"/>
+			<inertia
+					ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
+					iyy="${trunk_iyy}" iyz="${trunk_iyz}"
+					izz="${trunk_izz}"/>
+		</inertial>
+	</link>
+
+	<joint name="imu_joint" type="fixed">
+		<parent link="trunk"/>
+		<child link="imu_link"/>
+		<origin rpy="0 0 0" xyz="0 0 0"/>
+	</joint>
+
+	<link name="imu_link">
+		<inertial>
+			<mass value="0.001"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<box size="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="red"/>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<box size=".001 .001 .001"/>
+			</geometry>
+		</collision>
+	</link>
+
+	<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True"/>
+	<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True"/>
+	<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False"/>
+	<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False"/>
+
+	<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
+
+</robot>

descriptions/aliengo_description/xacro/ros2_control.xacro

@@ -0,0 +1,176 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="UnitreeSystem" type="system">
+
+    <hardware>
+      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    </hardware>
+
+    <joint name="FR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="RL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.w"/>
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+    </sensor>
+
+    <sensor name="foot_force">
+      <state_interface name="FR"/>
+      <state_interface name="FL"/>
+      <state_interface name="RR"/>
+      <state_interface name="RL"/>
+    </sensor>
+
+  </ros2_control>
+
+</robot>

descriptions/aliengo_description/xacro/stairs.xacro

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<xacro:property name="stair_length" value="0.640"/>
+	<xacro:property name="stair_width" value="0.310"/>
+	<xacro:property name="stair_height" value="0.170"/>
+
+	<xacro:macro name="stairs" params="stairs xpos ypos zpos">
+
+		<joint name="stair_joint_origin" type="fixed">
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<parent link="world"/>
+			<child link="stair_link_${stairs}"/>
+		</joint>
+
+		<link name="stair_link_${stairs}">
+			<visual>
+				<geometry>
+					<box size="${stair_length} ${stair_width} ${stair_height}"/>
+				</geometry>
+				<material name="grey"/>
+				<origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
+			</visual>
+			<collision>
+				<geometry>
+					<box size="${stair_length} ${stair_width} ${stair_height}"/>
+				</geometry>
+			</collision>
+			<inertial>
+				<mass value="0.80"/>
+				<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+			</inertial>
+		</link>
+
+		<xacro:if value="${stairs}">
+			<xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
+			<joint name="stair_joint_${stairs}" type="fixed">
+				<parent link="stair_link_${stairs}"/>
+				<child link="stair_link_${stairs-1}"/>
+			</joint>
+		</xacro:if>
+
+	</xacro:macro>
+
+</robot>

descriptions/aliengo_description/xacro/transmission.xacro

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<xacro:macro name="leg_transmission" params="name">
+
+		<transmission name="${name}_hip_tran">
+			<type>transmission_interface/SimpleTransmission</type>
+			<joint name="${name}_hip_joint">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+			</joint>
+			<actuator name="${name}_hip_motor">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+		</transmission>
+
+		<transmission name="${name}_thigh_tran">
+			<type>transmission_interface/SimpleTransmission</type>
+			<joint name="${name}_thigh_joint">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+			</joint>
+			<actuator name="${name}_thigh_motor">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+		</transmission>
+
+		<transmission name="${name}_calf_tran">
+			<type>transmission_interface/SimpleTransmission</type>
+			<joint name="${name}_calf_joint">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+			</joint>
+			<actuator name="${name}_calf_motor">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+		</transmission>
+
+	</xacro:macro>
+
+</robot>

descriptions/b2_description/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+
+project(b2_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+		DIRECTORY meshes xacro launch config urdf
+		DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

descriptions/b2_description/README.md

@@ -0,0 +1,31 @@
+# Unitree B2 Description
+This repository contains the urdf model of b2.
+
+![B2](../../.images/b2.png)
+
+## Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to b2_description
+```
+
+## Visualize the robot
+To visualize and check the configuration of the robot in rviz, simply launch:
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch b2_description visualize.launch.py
+```
+
+## Launch ROS2 Control
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch b2_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch b2_description ocs2_control.launch.py
+  ```
+
+

descriptions/b2_description/config/ocs2/gait.info

@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}