创建基本架构ros2_control

README.md

@@ -1,3 +1,4 @@
 # CM_DOG
 
-A simple ros2 program for legged robot . Robocon2025
+A simple ros2 program for legged robot . Robocon2025
+

src/commands/README.md

@@ -0,0 +1,3 @@
+# Control Command Inputs
+
+This folder contains the ros2 node for control input, like keyboard or gamepad.

src/commands/control_input_msgs/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.8)
+project(control_input_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/Inputs.msg"
+)
+
+ament_package()

src/commands/control_input_msgs/msg/Inputs.msg

@@ -0,0 +1,8 @@
+# control input message
+
+int32 command
+
+float32 lx
+float32 ly
+float32 rx
+float32 ry

src/commands/control_input_msgs/package.xml

@@ -0,0 +1,17 @@
+<?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>control_input_msgs</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/commands/joystick_input/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.8)
+project(joystick_input)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(control_input_msgs REQUIRED)
+
+add_executable(joystick_input src/JoystickInput.cpp)
+target_include_directories(joystick_input
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        joystick_input PUBLIC
+        rclcpp
+        sensor_msgs
+        control_input_msgs
+)
+
+install(TARGETS
+        joystick_input
+        DESTINATION lib/${PROJECT_NAME})
+
+install(
+        DIRECTORY launch
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+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()

src/commands/joystick_input/README.md

@@ -0,0 +1,34 @@
+# Joystick Input
+
+This node will listen to the joystick topic and publish a control_input_msgs/Input message.
+
+Tested environment:
+* Ubuntu 24.04
+  * ROS2 Jazzy
+  * Logitech F310 Gamepad
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to joystick_input
+```
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch joystick_input joystick.launch.py
+```
+
+## 1. Use Instructions for Unitree Guide
+
+### 1.1 Control Mode
+
+* Passive Mode: LB + B
+* Fixed Stand: LB + A
+    * Free Stand: LB + X
+    * Trot: LB + Y
+    * SwingTest: LT + B
+    * Balance: LT + A
+
+### 1.2 Control Input
+
+* WASD IJKL: Move robot
+* Space: Reset Speed Input

src/commands/joystick_input/include/joystick_input/JoystickInput.h

@@ -0,0 +1,27 @@
+//
+// Created by tlab-uav on 24-9-13.
+//
+
+#ifndef JOYSTICKINPUT_H
+#define JOYSTICKINPUT_H
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+
+
+class JoystickInput final : public rclcpp::Node {
+public:
+    JoystickInput();
+
+    ~JoystickInput() override = default;
+
+private:
+    void joy_callback(sensor_msgs::msg::Joy::SharedPtr msg);
+
+    control_input_msgs::msg::Inputs inputs_;
+    rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
+};
+
+
+#endif //JOYSTICKINPUT_H

src/commands/joystick_input/launch/joystick.launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='joy',
+            executable='joy_node',
+            name='joynode',
+            parameters=[{
+                'dev': '/dev/input/js0'
+            }]
+        ),
+        Node(
+            package='joystick_input',
+            executable='joystick_input',
+            name='joystick_input_node'
+        )
+    ])

src/commands/joystick_input/package.xml

@@ -0,0 +1,19 @@
+<?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>joystick_input</name>
+  <version>0.0.0</version>
+  <description>A package to convert joystick signal to control input</description>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>control_input_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/commands/joystick_input/src/JoystickInput.cpp

@@ -0,0 +1,50 @@
+//
+// Created by tlab-uav on 24-9-13.
+//
+
+#include "joystick_input/JoystickInput.h"
+
+using std::placeholders::_1;
+
+JoystickInput::JoystickInput() : Node("joysick_input_node") {
+    publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
+    subscription_ = create_subscription<
+        sensor_msgs::msg::Joy>("joy", 10, std::bind(&JoystickInput::joy_callback, this, _1));
+}
+
+void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
+    if (msg->buttons[1] && msg->buttons[4]) {
+        inputs_.command = 1; // LB + B
+    } else if (msg->buttons[0] && msg->buttons[4]) {
+        inputs_.command = 2; // LB + A
+    } else if (msg->buttons[2] && msg->buttons[4]) {
+        inputs_.command = 3; // LB + 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 = 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];
+        inputs_.ly = msg->axes[1];
+        inputs_.rx = -msg->axes[3];
+        inputs_.ry = msg->axes[4];
+    }
+    publisher_->publish(inputs_);
+}
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<JoystickInput>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/commands/keyboard_input/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(keyboard_input)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(control_input_msgs REQUIRED)
+
+add_executable(keyboard_input src/KeyboardInput.cpp)
+target_include_directories(keyboard_input
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        keyboard_input PUBLIC
+        rclcpp
+        control_input_msgs
+)
+
+install(TARGETS
+        keyboard_input
+        DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/commands/keyboard_input/README.md

@@ -0,0 +1,33 @@
+# Keyboard Input
+
+This node will read the keyboard input and publish a control_input_msgs/Input message.
+
+Tested environment:
+* Ubuntu 24.04
+  * ROS2 Jazzy
+* Ubuntu 22.04
+  * ROS2 Humble
+
+### Build Command
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to keyboard_input
+```
+
+### Launch Command
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 run keyboard_input keyboard_input
+```
+
+## 1. Use Instructions for Unitree Guide
+### 1.1 Control Mode
+* Passive Mode: Keyboard 1
+* Fixed Stand: Keyboard 2
+    * Free Stand: Keyboard 3
+    * Trot: Keyboard 4
+    * SwingTest: Keyboard 5
+    * Balance: Keyboard 6
+### 1.2 Control Input
+* WASD IJKL: Move robot
+* Space: Reset Speed Input

src/commands/keyboard_input/include/keyboard_input/KeyboardInput.h

@@ -0,0 +1,53 @@
+//
+// Created by biao on 24-9-11.
+//
+
+
+#ifndef KEYBOARDINPUT_H
+#define KEYBOARDINPUT_H
+#include <rclcpp/rclcpp.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+#include <termios.h>
+
+
+template <typename T1, typename T2>
+T1 max(const T1 a, const T2 b) {
+    return (a > b ? a : b);
+}
+
+template <typename T1, typename T2>
+T1 min(const T1 a, const T2 b) {
+    return (a < b ? a : b);
+}
+
+
+class KeyboardInput final : public rclcpp::Node {
+public:
+    KeyboardInput();
+
+    ~KeyboardInput() override {
+        tcsetattr(STDIN_FILENO, TCSANOW, &old_tio_);
+    }
+
+private:
+    void timer_callback();
+
+    void check_command(char key);
+    void check_value(char key);
+
+    static bool kbhit();
+
+    control_input_msgs::msg::Inputs inputs_;
+    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_{};
+};
+
+
+#endif //KEYBOARDINPUT_H

src/commands/keyboard_input/package.xml

@@ -0,0 +1,18 @@
+<?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>keyboard_input</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="biao876990970@hotmail.com">biao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>rclcpp</depend>
+    <depend>control_input_msgs</depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/commands/keyboard_input/src/KeyboardInput.cpp

@@ -0,0 +1,150 @@
+//
+// Created by biao on 24-9-11.
+//
+
+#include <keyboard_input/KeyboardInput.h>
+
+KeyboardInput::KeyboardInput() : Node("keyboard_input_node") {
+    publisher_ = create_publisher<control_input_msgs::msg::Inputs>("control_input", 10);
+    timer_ = create_wall_timer(std::chrono::microseconds(100), std::bind(&KeyboardInput::timer_callback, this));
+    inputs_ = control_input_msgs::msg::Inputs();
+
+    tcgetattr(STDIN_FILENO, &old_tio_);
+    new_tio_ = old_tio_;
+    new_tio_.c_lflag &= (~ICANON & ~ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &new_tio_);
+    RCLCPP_INFO(get_logger(), "Keyboard input node started.");
+    RCLCPP_INFO(get_logger(), "Press 1-0 to switch between different modes");
+    RCLCPP_INFO(get_logger(), "Use W/S/A/D and I/K/J/L to move the robot.");
+    RCLCPP_INFO(get_logger(), "Please input keys, press Ctrl+C to quit.");
+}
+
+void KeyboardInput::timer_callback() {
+    if (kbhit()) {
+        char key = getchar();
+        check_command(key);
+        if (inputs_.command == 0) check_value(key);
+        else {
+            inputs_.lx = 0;
+            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_);
+                }
+            }
+        }
+    }
+}
+
+void KeyboardInput::check_command(const char key) {
+    switch (key) {
+        case '1':
+            inputs_.command = 1; // L2_B
+            break;
+        case '2':
+            inputs_.command = 2; // L2_A
+            break;
+        case '3':
+            inputs_.command = 3; // L2_X
+            break;
+        case '4':
+            inputs_.command = 4; // L2_Y
+            break;
+        case '5':
+            inputs_.command = 5; // L1_A
+            break;
+        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;
+            inputs_.rx = 0;
+            inputs_.ry = 0;
+            inputs_.command = 0;
+            break;
+        default:
+            inputs_.command = 0;
+            break;
+    }
+}
+
+void KeyboardInput::check_value(char key) {
+    switch (key) {
+        case 'w':
+        case 'W':
+            inputs_.ly = min<float>(inputs_.ly + sensitivity_left_, 1.0);
+            break;
+        case 's':
+        case 'S':
+            inputs_.ly = max<float>(inputs_.ly - sensitivity_left_, -1.0);
+            break;
+        case 'd':
+        case 'D':
+            inputs_.lx = min<float>(inputs_.lx + sensitivity_left_, 1.0);
+            break;
+        case 'a':
+        case 'A':
+            inputs_.lx = max<float>(inputs_.lx - sensitivity_left_, -1.0);
+            break;
+
+        case 'i':
+        case 'I':
+            inputs_.ry = min<float>(inputs_.ry + sensitivity_right_, 1.0);
+            break;
+        case 'k':
+        case 'K':
+            inputs_.ry = max<float>(inputs_.ry - sensitivity_right_, -1.0);
+            break;
+        case 'l':
+        case 'L':
+            inputs_.rx = min<float>(inputs_.rx + sensitivity_right_, 1.0);
+            break;
+        case 'j':
+        case 'J':
+            inputs_.rx = max<float>(inputs_.rx - sensitivity_right_, -1.0);
+            break;
+        default:
+            break;
+    }
+}
+
+bool KeyboardInput::kbhit() {
+    timeval tv = {0L, 0L};
+    fd_set fds;
+    FD_ZERO(&fds);
+    FD_SET(STDIN_FILENO, &fds);
+    return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
+}
+
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<KeyboardInput>();
+    spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/robot_descriptions/README.md

@@ -0,0 +1,68 @@
+# Robot Descriptions
+
+This folder contains the URDF and SRDF files for the quadruped robot.
+
+* Unitree
+    * [Go1](unitree/go1_description/)
+    * [Go2](unitree/go2_description/)
+    * [A1](unitree/a1_description/)
+    * [Aliengo](unitree/aliengo_description/)
+    * [B2](unitree/b2_description/)
+* Xiaomi
+    * [Cyberdog](xiaomi/cyberdog_description/)
+* Deep Robotics
+    * [Lite 3](deep_robotics/lite3_description/)
+    * [X30](deep_robotics/x30_description/)
+* Anybotics
+    * [Anymal C](anybotics/anymal_c_description/)
+
+## 1. Steps to transfer urdf to Mujoco model
+
+* Install [Mujoco](https://github.com/google-deepmind/mujoco)
+* Transfer the mesh files to mujoco supported format, like stl.
+* Adjust the urdf tile to match the mesh file. Transfer the mesh file from .dae to .stl may change the scale size of the
+  mesh file.
+* use `xacro` to generate the urdf file.
+  ```
+  xacro robot.xacro > ../urdf/robot.urdf
+  ```
+* use mujoco to convert the urdf file to mujoco model.
+  ```
+  compile robot.urdf robot.xml
+  ```
+
+## 2. Dependencies for Gazebo Simulation
+
+Gazebo Simulation only tested on ROS2 Jazzy. It add support for ROS2 Humble because the package name is different.
+
+* Gazebo Harmonic
+  ```bash
+  sudo apt-get install ros-jazzy-ros-gz
+  ```
+* Ros2-Control for Gazebo
+  ```bash
+  sudo apt-get install ros-jazzy-gz-ros2-control
+  ``` 
+* Legged PD Controller
+    ```bash
+    cd ~/ros2_ws
+    colcon build --packages-up-to leg_pd_controller
+    ```
+
+## 2. Dependencies for Gazebo Classic Simulation
+
+Gazebo Classic (Gazebo11) Simulation only tested on ROS2 Humble.
+
+* Gazebo Classic
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros
+  ```
+* Ros2-Control for Gazebo
+  ```bash
+  sudo apt-get install ros-humble-gazebo-ros2-control
+  ``` 
+* Legged PD Controller
+    ```bash
+    cd ~/ros2_ws
+    colcon build --packages-up-to leg_pd_controller
+    ```

src/robot_descriptions/anybotics/anymal_c_description/CMakeLists.txt

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

src/robot_descriptions/anybotics/anymal_c_description/LICENSE

@@ -0,0 +1,29 @@
+Copyright 2020, ANYbotics AG.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+   notice, this list of conditions and the following disclaimer in
+   the documentation and/or other materials provided with the
+   distribution.
+
+3. 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.

src/robot_descriptions/anybotics/anymal_c_description/README.md

@@ -0,0 +1,39 @@
+# Anybotics Anymal_C Description
+This repository contains the urdf model of lite3.
+
+![anymal_c](../../../.images/anymal_c.png)
+
+Tested environment:
+* Ubuntu 24.04
+    * ROS2 Jazzy
+
+## Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to anymal_c_description --symlink-install
+```
+
+## 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 anymal_c_description visualize.launch.py
+```
+
+## Launch ROS2 Control
+### Mujoco Simulator
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```
+* Legged Gym Controller
+    ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=anymal_c_description
+  ```

src/robot_descriptions/anybotics/anymal_c_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.35
+     [2]     0.70
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+  }
+}
+
+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
+   }
+}

src/robot_descriptions/anybotics/anymal_c_description/config/ocs2/reference.info

@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.6;
+
+defaultJointState
+{
+   (0,0)  -0.25   ; LF_HAA
+   (1,0)   0.60   ; LF_HFE
+   (2,0)  -0.85   ; LF_KFE
+   (3,0)  -0.25   ; LH_HAA
+   (4,0)  -0.60   ; LH_HFE
+   (5,0)   0.85   ; LH_KFE
+   (6,0)   0.25   ; RF_HAA
+   (7,0)   0.60   ; RF_HFE
+   (8,0)  -0.85   ; RF_KFE
+   (9,0)   0.25   ; RH_HAA
+   (10,0) -0.60   ; RH_HFE
+   (11,0)  0.85   ; RH_KFE
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}

src/robot_descriptions/anybotics/anymal_c_description/config/ocs2/task.info

@@ -0,0 +1,318 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+  
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0 ; 20.0
+  phaseTransitionStanceTime     0.4
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/anymal_c
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.2
+  touchDownVelocity            -0.4
+  swingHeight                   0.1
+  touchdownAfterHorizon         0.2
+  swingTimeScale                0.15
+}
+
+; 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                     true
+  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                     true
+  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
+}
+
+; 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
+
+  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
+  }
+}
+
+; 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             500 ; [Hz]
+}
+
+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.575   ; 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: [LF, LH, RF, RH] ;;
+   (12,0) -0.25   ; LF_HAA
+   (13,0)  0.60   ; LF_HFE
+   (14,0) -0.85   ; LF_KFE
+   (15,0) -0.25   ; LH_HAA
+   (16,0) -0.60   ; LH_HFE
+   (17,0)  0.85   ; LH_KFE
+   (18,0)  0.25   ; RF_HAA
+   (19,0)  0.60   ; RF_HFE
+   (20,0) -0.85   ; RF_KFE
+   (21,0)  0.25   ; RH_HAA
+   (22,0) -0.60   ; RH_HFE
+   (23,0)  0.85   ; RH_KFE
+}
+
+; 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)   30.0     ; vcom_z
+  (3,3)   5.0      ; L_x / robotMass
+  (4,4)   10.0     ; L_y / robotMass
+  (5,5)   10.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   500.0    ; p_base_x
+  (7,7)   500.0    ; p_base_y
+  (8,8)   500.0    ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 200.0    ; theta_base_y
+  (11,11) 200.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [LF, LH, RF, RH] ;;
+  (12,12) 20.0     ; LF_HAA
+  (13,13) 20.0     ; LF_HFE
+  (14,14) 20.0     ; LF_KFE
+  (15,15) 20.0     ; LH_HAA
+  (16,16) 20.0     ; LH_HFE
+  (17,17) 20.0     ; LH_KFE
+  (18,18) 20.0     ; RF_HAA
+  (19,19) 20.0     ; RF_HFE
+  (20,20) 20.0     ; RF_KFE
+  (21,21) 20.0     ; RH_HAA
+  (22,22) 20.0     ; RH_HFE
+  (23,23) 20.0     ; RH_KFE
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [LF, RF, LH, RH] ;;
+  (0,0)   1.0       ; left_front_force
+  (1,1)   1.0       ; left_front_force
+  (2,2)   1.0       ; left_front_force
+  (3,3)   1.0       ; right_front_force
+  (4,4)   1.0       ; right_front_force
+  (5,5)   1.0       ; right_front_force
+  (6,6)   1.0       ; left_hind_force
+  (7,7)   1.0       ; left_hind_force
+  (8,8)   1.0       ; left_hind_force
+  (9,9)   1.0       ; right_hind_force
+  (10,10) 1.0       ; right_hind_force
+  (11,11) 1.0       ; right_hind_force
+
+  ;; foot velocity relative to base: [LF, LH, RF, RH] (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.5
+  
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "LF_shank_fixed, RF_shank_fixed"
+    [1] "LH_shank_fixed, RH_shank_fixed"
+    [2] "LF_shank_fixed, LH_shank_fixed"
+    [3] "RF_shank_fixed, RH_shank_fixed"
+    [4] "LF_FOOT, RF_FOOT"
+    [5] "LH_FOOT, RH_FOOT"
+    [6] "LF_FOOT, LH_FOOT"
+    [7] "RF_FOOT, RH_FOOT"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  80.0     ; HAA
+   (1,0)  80.0     ; HFE
+   (2,0)  80.0     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.015
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

src/robot_descriptions/anybotics/anymal_c_description/config/robot_control.yaml

@@ -0,0 +1,249 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 200  # 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
+
+    rl_quadruped_controller:
+      type: rl_quadruped_controller/LeggedGymController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    joints:
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+
+    down_pos:
+      - -0.0
+      - 1.41
+      - -2.58
+      - 0.0
+      - 1.41
+      - -2.58
+      - -0.0
+      - -1.41
+      - 2.58
+      - 0.0
+      - -1.41
+      - 2.58
+
+    stand_pos:
+      - 0.2
+      - 0.6
+      - -0.85
+      - -0.2
+      - 0.6
+      - -0.85
+      - 0.2
+      - -0.6
+      - 0.85
+      - -0.2
+      - -0.6
+      - 0.85
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_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: 100  # Hz
+    default_kd: 1.5
+    joints:
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_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:
+      - LF
+      - RF
+      - LH
+      - RH
+
+rl_quadruped_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    robot_pkg: "anymal_c_description"
+    model_folder: "legged_gym"
+    joints:
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+
+    down_pos:
+      - -0.0
+      - 1.41
+      - -2.58
+      - 0.0
+      - 1.41
+      - -2.58
+      - -0.0
+      - -1.41
+      - 2.58
+      - 0.0
+      - -1.41
+      - 2.58
+
+    stand_pos:
+      - 0.2
+      - 0.6
+      - -0.85
+      - -0.2
+      - 0.6
+      - -0.85
+      - 0.2
+      - -0.6
+      - 0.85
+      - -0.2
+      - -0.6
+      - 0.85
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - LF_FOOT
+      - RF_FOOT
+      - LH_FOOT
+      - RH_FOOT
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - LF
+      - RF
+      - LH
+      - RH
+
+    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

src/robot_descriptions/anybotics/anymal_c_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

src/robot_descriptions/anybotics/anymal_c_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 = "anymal_c_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='a1',
+        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]
+        )
+    ])

src/robot_descriptions/anybotics/anymal_c_description/launch/visualize.launch.py

@@ -0,0 +1,49 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro
+
+package_description = "anymal_c_description"
+
+def process_xacro():
+    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)
+    return robot_description_config.toxml()
+def generate_launch_description():
+    
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+    robot_description = process_xacro()
+
+    return LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        ),
+        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',
+        )
+    ])

src/robot_descriptions/anybotics/anymal_c_description/meshes/battery.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
+    </contributor>
+    <created>2020-01-07T14:47:32</created>
+    <modified>2020-01-07T14:47:32</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="battery-effect">
+      <profile_COMMON>
+        <newparam sid="battery-surface">
+          <surface type="2D">
+            <init_from>battery</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="battery-sampler">
+          <sampler2D>
+            <source>battery-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="battery-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="battery" name="battery">
+      <init_from>battery.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="battery-material" name="battery">
+      <instance_effect url="#battery-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array id="Cube-mesh-positions-array" count="24">-0.225881 -0.06249898 -0.03684729 -0.225881 -0.06249898 0.03725165 -0.225881 0.06249898 -0.03684729 -0.225881 0.06249898 0.03725165 0.232213 -0.06249898 -0.03684729 0.232213 -0.06249898 0.03725165 0.232213 0.06249898 -0.03684729 0.232213 0.06249898 0.03725165</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map">
+          <float_array id="Cube-mesh-map-array" count="72">0.8431081 0.2728654 1 0 1 0.2728654 0.6862162 1 0.8431079 0 0.8431081 1 0.8431081 0.5457308 1 0.2728654 1 0.5457308 0.6862159 0 0.5293241 1 0.5293239 0 0.264662 0 0.5293239 0.9999999 0.2646622 0.9999999 0.264662 0.9999999 0 0 0.2646618 0 0.8431081 0.2728654 0.8431081 0 1 0 0.6862162 1 0.6862161 0 0.8431079 0 0.8431081 0.5457308 0.8431081 0.2728654 1 0.2728654 0.6862159 0 0.6862161 1 0.5293241 1 0.264662 0 0.5293238 0 0.5293239 0.9999999 0.264662 0.9999999 1.73529e-7 0.9999999 0 0</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-map-array" count="36" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <triangles material="battery-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
+          <p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="AM3_Battery_LP" name="AM3 Battery LP" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 -0.03329167 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh" name="AM3 Battery LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="battery-material" target="#battery-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

src/robot_descriptions/anybotics/anymal_c_description/meshes/depth_camera.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.82.7 commit date:2020-02-12, commit time:16:20, hash:77d23b0bd76f</authoring_tool>
+    </contributor>
+    <created>2020-02-20T10:55:16</created>
+    <modified>2020-02-20T10:55:16</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="depth_camera-effect">
+      <profile_COMMON>
+        <newparam sid="depth_camera-surface">
+          <surface type="2D">
+            <init_from>depth_camera</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="depth_camera-sampler">
+          <sampler2D>
+            <source>depth_camera-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="depth_camera-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="depth_camera" name="depth_camera">
+      <init_from>depth_camera.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="depth_camera-material" name="depth_camera">
+      <instance_effect url="#depth_camera-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array id="Cube-mesh-positions-array" count="24">0 -0.04699999 -0.01968461 0 -0.04699999 0.01951932 0 0.04699999 -0.01968461 0 0.04699999 0.01951932 0.03039997 -0.04699999 -0.01968461 0.03039997 -0.04699999 0.01951932 0.03039997 0.04699999 -0.01968461 0.03039997 0.04699999 0.01951932</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array id="Cube-mesh-normals-array" count="18">-1 0 0 1 0 0 0 0 1 0 -1 0 0 1 0 0 0 -1</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map">
+          <float_array id="Cube-mesh-map-array" count="72">0.2816218 0.7056847 0.5632433 0 0.5632433 0.7056847 1.60564e-7 0.7056847 0.2816215 0 0.2816217 0.7056847 0.7816217 0.7056846 1 0 1 0.7056846 0.5632433 0.7056847 0.344865 1 0.344865 0.7056847 0.7816216 0.7056847 0.5632434 1 0.5632433 0.7056847 0.7816217 0 0.5632433 0.7056846 0.5632434 0 0.2816218 0.7056847 0.2816217 0 0.5632433 0 1.60564e-7 0.7056847 0 0 0.2816215 0 0.7816217 0.7056846 0.7816217 0 1 0 0.5632433 0.7056847 0.5632433 1 0.344865 1 0.7816216 0.7056847 0.7816218 1 0.5632434 1 0.7816217 0 0.7816216 0.7056846 0.5632433 0.7056846</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-map-array" count="36" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <triangles material="depth_camera-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map" offset="2" set="0"/>
+          <p>1 0 0 2 0 1 0 0 2 7 1 3 4 1 4 6 1 5 3 2 6 5 2 7 7 2 8 1 3 9 4 3 10 5 3 11 7 4 12 2 4 13 3 4 14 6 5 15 0 5 16 2 5 17 1 0 18 3 0 19 2 0 20 7 1 21 5 1 22 4 1 23 3 2 24 1 2 25 5 2 26 1 3 27 0 3 28 4 3 29 7 4 30 6 4 31 2 4 32 6 5 33 4 5 34 0 5 35</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Realsense_LP" name="Realsense LP" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh" name="Realsense LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="depth_camera-material" target="#depth_camera-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

src/robot_descriptions/anybotics/anymal_c_description/meshes/hatch.dae

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.81.16 commit date:2019-11-20, commit time:14:27, hash:26bd5ebd42e3</authoring_tool>
+    </contributor>
+    <created>2020-01-13T19:28:23</created>
+    <modified>2020-01-13T19:28:23</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="hatch-effect">
+      <profile_COMMON>
+        <newparam sid="hatch-surface">
+          <surface type="2D">
+            <init_from>hatch</init_from>
+          </surface>
+        </newparam>
+        <newparam sid="hatch-sampler">
+          <sampler2D>
+            <source>hatch-surface</source>
+          </sampler2D>
+        </newparam>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <texture texture="hatch-sampler" texcoord="UVMap"/>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images>
+    <image id="hatch" name="hatch">
+      <init_from>hatch.jpg</init_from>
+    </image>
+  </library_images>
+  <library_materials>
+    <material id="hatch-material" name="hatch">
+      <instance_effect url="#hatch-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch Detailed HD.002">
+      <mesh>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="96">-76.90127 -64.71254 0 -76.90127 -64.71254 5.500071 -76.90127 64.71254 0 -76.90127 64.71254 5.500071 76.90133 -64.71254 0 76.90133 -64.71254 5.500071 76.90133 64.71254 0 76.90133 64.71254 5.500071 -80.46286 11.54165 0 -91.00833 3.847216 0 -91.00833 -3.847217 0 -80.46286 -11.54165 0 -80.46286 -11.54165 5.500071 -91.00833 -3.847217 5.500071 -91.00833 3.847217 5.500071 -80.46286 11.54165 5.500071 80.46292 -11.54165 0 91.00839 -3.847217 0 91.00839 3.847217 0 80.46292 11.54165 0 80.46292 11.54165 5.500071 91.00839 3.847216 5.500071 91.00839 -3.847217 5.500071 80.46292 -11.54165 5.500071 -80.46286 38.12709 5.500071 80.46292 38.12709 0 -80.46286 38.12709 0 80.46292 38.12709 5.500071 -80.46286 -38.12709 0 80.46292 -38.12709 5.500071 -80.46286 -38.12709 5.500071 80.46292 -38.12709 0</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions-array" count="32" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="63">-0.9911455 0.1327809 0 0 1 0 0.9911455 -0.1327809 0 0 -1 0 0 0 1 0 0 1 1 0 0 0.5894234 0.8078244 0 0.5894234 -0.8078244 0 -1 0 0 -0.5894234 -0.8078244 0 -0.5894234 0.8078243 -2.12521e-7 0.9911454 0.1327813 0 -0.9911454 -0.1327813 0 -0.9911454 0.1327813 0 0.9911454 -0.1327813 0 0 0 1 0.5894235 0.8078243 2.12521e-7 -0.5894234 0.8078244 0 0.9911455 0.1327809 0 -0.9911455 -0.1327809 0</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals-array" count="21" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="AM3_Application_Hatch_Detailed_HD_002-mesh-map">
+          <float_array id="AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="276">0.8547119 0.7845032 0.8910338 0.6826047 0.8910338 0.7845032 0.8547118 0.6826048 0.8910338 0 0.8910338 0.6826047 0.963678 0.5650011 0.927356 0.7110616 0.927356 0.5650011 0.891034 0.7845032 0.927356 0.1018984 0.927356 0.7845032 0.1755681 0.9420632 0 0.07750421 0.1755678 0.05793678 0.679144 0.9420632 0.5035759 0.05793678 0.6791438 0.05793678 0.503576 0.9420632 0.4527625 0 0.5035759 0.05793678 0.4527627 1 0.4019491 0 0.4527625 0 0.4019494 1 0.3511358 0.05793678 0.4019491 0 0.963678 0.1460605 0.927356 0.2921209 0.927356 0.1460605 0.963678 0.2921209 0.927356 0.3343942 0.927356 0.2921209 0.963678 0.3343942 0.927356 0.3766674 0.927356 0.3343942 0.963678 0.3766674 0.927356 0.4189407 0.927356 0.3766674 0.927356 0.7971531 0.963678 0.7110617 0.963678 0.7971532 0.963678 0.07171952 1 0 1 0.07171952 0.8547119 0.9672312 0.8910339 0.9423143 0.8910339 0.9672312 0.8547119 0.9423143 0.8910339 0.8705947 0.8910339 0.9423143 0.963678 0 0.927356 0.1460605 0.9273561 0 0.8547118 0.9224958 0.6791438 0.05793678 0.8547117 0.07750409 0.8547119 0.8705947 0.8910338 0.7845032 0.8910339 0.8705947 0.891034 0.1018984 0.927356 0 0.927356 0.1018984 0.351136 0.9420632 0.1755678 0.05793678 0.3511358 0.05793678 0.963678 0.4189407 0.927356 0.5650011 0.927356 0.4189407 0.8547119 0.7845032 0.8547118 0.6826048 0.8910338 0.6826047 0.8547118 0.6826048 0.8547118 0 0.8910338 0 0.963678 0.5650011 0.963678 0.7110616 0.927356 0.7110616 0.891034 0.7845032 0.891034 0.1018984 0.927356 0.1018984 0.1755681 0.9420632 0 0.9224959 0 0.07750421 0.679144 0.9420632 0.503576 0.9420632 0.5035759 0.05793678 0.503576 0.9420632 0.4527627 1 0.4527625 0 0.4527627 1 0.4019494 1 0.4019491 0 0.4019494 1 0.351136 0.9420632 0.3511358 0.05793678 0.963678 0.1460605 0.963678 0.2921209 0.927356 0.2921209 0.963678 0.2921209 0.963678 0.3343942 0.927356 0.3343942 0.963678 0.3343942 0.963678 0.3766674 0.927356 0.3766674 0.963678 0.3766674 0.963678 0.4189407 0.927356 0.4189407 0.927356 0.7971531 0.927356 0.7110616 0.963678 0.7110617 0.963678 0.07171952 0.963678 0 1 0 0.8547119 0.9672312 0.8547119 0.9423143 0.8910339 0.9423143 0.8547119 0.9423143 0.8547119 0.8705947 0.8910339 0.8705947 0.963678 0 0.963678 0.1460605 0.927356 0.1460605 0.8547118 0.9224958 0.679144 0.9420632 0.6791438 0.05793678 0.8547119 0.8705947 0.8547119 0.7845032 0.8910338 0.7845032 0.891034 0.1018984 0.891034 0 0.927356 0 0.351136 0.9420632 0.1755681 0.9420632 0.1755678 0.05793678 0.963678 0.4189407 0.963678 0.5650011 0.927356 0.5650011</float_array>
+          <technique_common>
+            <accessor source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map-array" count="138" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="AM3_Application_Hatch_Detailed_HD_002-mesh-vertices">
+          <input semantic="POSITION" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-positions"/>
+        </vertices>
+        <triangles material="hatch-material" count="46">
+          <input semantic="VERTEX" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#AM3_Application_Hatch_Detailed_HD_002-mesh-map" offset="2" set="0"/>
+          <p>24 0 0 2 0 1 26 0 2 3 1 3 6 1 4 2 1 5 29 2 6 4 2 7 31 2 8 5 3 9 0 3 10 4 3 11 30 4 12 5 4 13 29 4 14 24 4 15 20 4 16 27 4 17 15 4 18 21 4 19 20 4 20 14 5 21 22 5 22 21 5 23 13 4 24 23 4 25 22 4 26 27 6 27 19 6 28 25 6 29 20 7 30 18 7 31 19 7 32 21 6 33 17 6 34 18 6 35 22 8 36 16 8 37 17 8 38 30 9 39 11 9 40 28 9 41 12 10 42 10 10 43 11 10 44 13 9 45 9 9 46 10 9 47 14 11 48 8 11 49 9 11 50 7 12 51 25 12 52 6 12 53 3 4 54 27 4 55 7 4 56 15 9 57 26 9 58 8 9 59 1 13 60 28 13 61 0 13 62 12 4 63 29 4 64 23 4 65 23 6 66 31 6 67 16 6 68 24 14 69 3 14 70 2 14 71 3 1 72 7 1 73 6 1 74 29 15 75 5 15 76 4 15 77 5 3 78 1 3 79 0 3 80 30 4 81 1 4 82 5 4 83 24 4 84 15 4 85 20 4 86 15 16 87 14 16 88 21 16 89 14 4 90 13 4 91 22 4 92 13 4 93 12 4 94 23 4 95 27 6 96 20 6 97 19 6 98 20 17 99 21 17 100 18 17 101 21 6 102 22 6 103 17 6 104 22 8 105 23 8 106 16 8 107 30 9 108 12 9 109 11 9 110 12 10 111 13 10 112 10 10 113 13 9 114 14 9 115 9 9 116 14 18 117 15 18 118 8 18 119 7 19 120 27 19 121 25 19 122 3 4 123 24 4 124 27 4 125 15 9 126 24 9 127 26 9 128 1 20 129 30 20 130 28 20 131 12 4 132 30 4 133 29 4 134 23 6 135 29 6 136 31 6 137</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="AM3_Application_Hatch_LP" name="AM3 Application Hatch LP" type="NODE">
+        <matrix sid="transform">0.001006675 0 0 0 0 9.99987e-4 0 0 0 0 9.99987e-4 0 0 0 0 1</matrix>
+        <instance_geometry url="#AM3_Application_Hatch_Detailed_HD_002-mesh" name="AM3 Application Hatch LP">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="hatch-material" target="#hatch-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

src/robot_descriptions/anybotics/anymal_c_description/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>anymal_c_description</name>
+  <version>0.0.0</version>
+  <description>The anymal_c_description package</description>
+
+  <license>BSD-3</license>
+
+  <author email="lisler@anybotics.com">Linus Isler</author>
+  <maintainer email="lisler@anybotics.com">Linus Isler</maintainer>
+  <maintainer email="rdiethelm@anybotics.com">Remo Diethelm</maintainer>
+
+  <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>

src/robot_descriptions/anybotics/anymal_c_description/xacro/gazebo.xacro

@@ -0,0 +1,195 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+
+    <joint name="FR_HipX">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_HipY">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FR_Knee">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_HipX">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_HipY">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="FL_Knee">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HR_HipX">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HR_HipY">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HR_Knee">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HL_HipX">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HL_HipY">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <joint name="HL_Knee">
+      <command_interface name="effort" initial_value="0.0"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <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>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters>$(find anymal_c_description)/config/gazebo.yaml</parameters>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
+    </plugin>
+  </gazebo>
+
+  <gazebo reference="trunk">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <visual>
+      <material>
+        <ambient>.05 .05 .05 1.0</ambient>
+        <diffuse>.05 .05 .05 1.0</diffuse>
+        <specular>.05 .05 .05 1.0</specular>
+      </material>
+    </visual>
+  </gazebo>
+
+  <gazebo reference="imu_link">
+    <sensor name="imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>500</update_rate>
+      <visualize>true</visualize>
+      <topic>imu</topic>
+      <imu>
+        <angular_velocity>
+          <x>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>2e-4</stddev>-->
+<!--              <bias_mean>0.0000075</bias_mean>-->
+<!--              <bias_stddev>0.0000008</bias_stddev>-->
+<!--            </noise>-->
+          </x>
+          <y>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>2e-4</stddev>-->
+<!--              <bias_mean>0.0000075</bias_mean>-->
+<!--              <bias_stddev>0.0000008</bias_stddev>-->
+<!--            </noise>-->
+          </y>
+          <z>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>2e-4</stddev>-->
+<!--              <bias_mean>0.0000075</bias_mean>-->
+<!--              <bias_stddev>0.0000008</bias_stddev>-->
+<!--            </noise>-->
+          </z>
+        </angular_velocity>
+        <linear_acceleration>
+          <x>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>1.7e-2</stddev>-->
+<!--              <bias_mean>0.1</bias_mean>-->
+<!--              <bias_stddev>0.001</bias_stddev>-->
+<!--            </noise>-->
+          </x>
+          <y>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>1.7e-2</stddev>-->
+<!--              <bias_mean>0.1</bias_mean>-->
+<!--              <bias_stddev>0.001</bias_stddev>-->
+<!--            </noise>-->
+          </y>
+          <z>
+<!--            <noise type="gaussian">-->
+<!--              <mean>0.0</mean>-->
+<!--              <stddev>1.7e-2</stddev>-->
+<!--              <bias_mean>0.1</bias_mean>-->
+<!--              <bias_stddev>0.001</bias_stddev>-->
+<!--            </noise>-->
+          </z>
+        </linear_acceleration>
+      </imu>
+    </sensor>
+  </gazebo>
+  <gazebo reference="imu_joint">
+    <disableFixedJointLumping>true</disableFixedJointLumping>
+  </gazebo>
+
+</robot>

src/robot_descriptions/anybotics/anymal_c_description/xacro/imu_link.xacro

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <joint name="imu_joint" type="fixed">
+    <parent link="TORSO"/>
+    <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>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

src/robot_descriptions/anybotics/anymal_c_description/xacro/materials.xacro

@@ -0,0 +1,36 @@
+<?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="deep-grey">
+        <color rgba="0.1 0.1 0.1 1.0"/>
+    </material>
+
+    <material name="grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+    </material>
+
+    <material name="light-grey">
+        <color rgba="0.35 0.35 0.35 1.0"/>
+    </material>
+
+    <material name="silver">
+        <color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
+    </material>
+
+    <material name="white">
+        <color rgba="1.0 1.0 1.0 1.0"/>
+    </material>
+
+</robot>

src/robot_descriptions/anybotics/anymal_c_description/xacro/robot.xacro

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<robot name="x30" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find anymal_c_description)/xacro/body.xacro"/>
+  <xacro:include filename="$(find anymal_c_description)/xacro/materials.xacro"/>
+
+  <xacro:arg name="GAZEBO" default="false"/>
+  <xacro:if value="$(arg GAZEBO)">
+    <xacro:include filename="$(find anymal_c_description)/xacro/gazebo.xacro"/>
+  </xacro:if>
+  <xacro:unless value="$(arg GAZEBO)">
+    <xacro:include filename="$(find anymal_c_description)/xacro/ros2_control.xacro"/>
+  </xacro:unless>
+
+</robot>

src/robot_descriptions/anybotics/anymal_c_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="RF_HAA">
+      <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="RF_HFE">
+      <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="RF_KFE">
+      <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="LF_HAA">
+      <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="LF_HFE">
+      <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="LF_KFE">
+      <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="RH_HAA">
+      <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="RH_HFE">
+      <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="RH_KFE">
+      <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="LH_HAA">
+      <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="LH_HFE">
+      <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="LH_KFE">
+      <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="RF"/>
+      <state_interface name="LF"/>
+      <state_interface name="RH"/>
+      <state_interface name="LH"/>
+    </sensor>
+
+  </ros2_control>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/CMakeLists.txt

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

src/robot_descriptions/deep_robotics/lite3_description/README.md

@@ -0,0 +1,74 @@
+# DeepRobotics Lite3 Description
+
+This repository contains the urdf model of lite3.
+
+![lite3](../../../.images/lite3.png)
+
+Tested environment:
+
+* Ubuntu 24.04
+    * ROS2 Jazzy
+
+## Build
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to lite3_description --symlink-install
+```
+
+## 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 lite3_description visualize.launch.py
+```
+
+## Launch ROS2 Control
+
+### Mujoco Simulator
+
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=lite3_description
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch ocs2_quadruped_controller mujoco.launch.py pkg_description:=lite3_description
+  ```
+* RL Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch rl_quadruped_controller mujoco.launch.py pkg_description:=lite3_description
+  ```
+
+### Gazebo Classic 11 (ROS2 Humble)
+
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=lite3_description
+  ```
+
+### Gazebo Harmonic (ROS2 Jazzy)
+
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43
+  ```
+  
+### Gazebo Playground (ROS2 Jazzy)
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch gz_quadruped_playground gazebo.launch.py pkg_description:=lite3_description controller:=ocs2 world:=warehouse
+   ```

src/robot_descriptions/deep_robotics/lite3_description/config/gazebo.yaml

@@ -0,0 +1,165 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # 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
+
+    rl_quadruped_controller:
+      type: rl_quadruped_controller/LeggedGymController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+    update_rate: 200
+
+ocs2_quadruped_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    robot_pkg: "lite3_description"
+    default_kd: 2.0
+    joints:
+      - FL_HipX
+      - FL_HipY
+      - FL_Knee
+      - FR_HipX
+      - FR_HipY
+      - FR_Knee
+      - HL_HipX
+      - HL_HipY
+      - HL_Knee
+      - HR_HipX
+      - HR_HipY
+      - HR_Knee
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet:
+      - FL_FOOT
+      - FR_FOOT
+      - HL_FOOT
+      - HR_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
+
+#    estimator_type: "odom"
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL_foot_force
+      - HL_foot_force
+      - FR_foot_force
+      - HR_foot_force
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+#    stand_kp: 30.0
+    stand_kd: 1.0
+    joints:
+      - FR_HipX
+      - FR_HipY
+      - FR_Knee
+      - FL_HipX
+      - FL_HipY
+      - FL_Knee
+      - HR_HipX
+      - HR_HipY
+      - HR_Knee
+      - HL_HipX
+      - HL_HipY
+      - HL_Knee
+
+    down_pos:
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+
+    stand_pos:
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - FR_FOOT
+      - FL_FOOT
+      - HR_FOOT
+      - HL_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

src/robot_descriptions/deep_robotics/lite3_description/config/legged_gym/config.yaml

@@ -0,0 +1,53 @@
+model_name: "policy.pt"
+framework: "isaacgym"
+rows: 4
+cols: 3
+decimation: 4
+num_observations: 48
+observations: ["lin_vel", "ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
+#observations_history: [6, 5, 4, 3, 2, 1, 0]
+clip_obs: 100.0
+clip_actions_lower: [-100, -100, -100,
+                     -100, -100, -100,
+                     -100, -100, -100,
+                     -100, -100, -100]
+clip_actions_upper: [100, 100, 100,
+                     100, 100, 100,
+                     100, 100, 100,
+                     100, 100, 100]
+rl_kp: [20, 20, 20,
+        20, 20, 20,
+        20, 20, 20,
+        20, 20, 20]
+rl_kd: [0.5, 0.5, 0.5,
+        0.5, 0.5, 0.5,
+        0.5, 0.5, 0.5,
+        0.5, 0.5, 0.5]
+fixed_kp: [60, 60, 60,
+           60, 60, 60,
+           60, 60, 60,
+           60, 60, 60]
+fixed_kd: [5, 5, 5,
+           5, 5, 5,
+           5, 5, 5,
+           5, 5, 5]
+hip_scale_reduction: 1.0
+hip_scale_reduction_indices: [0, 3, 6, 9]
+num_of_dofs: 12
+action_scale: 0.25
+
+lin_vel_scale: 2.0
+ang_vel_scale: 0.25
+dof_pos_scale: 1.0
+dof_vel_scale: 0.05
+
+commands_scale: [2.0, 2.0, 0.25]
+
+torque_limits: [33.5, 33.5, 33.5,
+                33.5, 33.5, 33.5,
+                33.5, 33.5, 33.5,
+                33.5, 33.5, 33.5]
+default_dof_pos: [ 0.1000, 0.8000, -1.5000,
+                   -0.1000, 0.8000, -1.5000,
+                   0.1000, 1.0000, -1.5000,
+                   -0.1000, 1.0000, -1.5000]

src/robot_descriptions/deep_robotics/lite3_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
+   }
+}

src/robot_descriptions/deep_robotics/lite3_description/config/ocs2/reference.info

@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.3;
+
+defaultJointState
+{
+   (0,0)   0.1    ; FL_HipX
+   (1,0)   -0.985 ; FL_HipY
+   (2,0)   2.084  ; FL_Knee
+   (3,0)   -0.1   ; FR_HipX
+   (4,0)   -0.985 ; FR_HipY
+   (5,0)   2.084  ; FR_Knee
+   (6,0)   0.1    ; HL_HipX
+   (7,0)   -0.985 ; HL_HipY
+   (8,0)   2.084  ; HL_Knee
+   (9,0)   -0.1   ; HR_HipX
+   (10,0)  -0.985 ; HR_HipY
+   (11,0)  2.084  ; HR_Knee
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}

src/robot_descriptions/deep_robotics/lite3_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              ocs2_cpp_ad/lite3
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.05
+  touchDownVelocity            -0.1
+  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, HL, FR, HR] ;;
+   (12,0)   0.1    ; FL_HipX
+   (13,0)   -0.985 ; FL_HipY
+   (14,0)   2.084  ; FL_Knee
+   (15,0)   0.1    ; HL_HipX
+   (16,0)   -0.985 ; HL_HipY
+   (17,0)   2.084  ; HL_Knee
+   (18,0)   -0.1   ; FR_HipX
+   (19,0)   -0.985 ; FR_HipY
+   (20,0)   2.084  ; FR_Knee
+   (21,0)   -0.1   ; HR_HipX
+   (22,0)   -0.985 ; HR_HipY
+   (23,0)   2.084  ; HR_Knee
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   10.0     ; vcom_x
+  (1,1)   10.0     ; vcom_y
+  (2,2)   80.0    ; vcom_z
+  (3,3)   8.0     ; L_x / robotMass
+  (4,4)   20.0     ; L_y / robotMass
+  (5,5)   20.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, HL, FR, HR] ;;
+  (12,12) 5.0     ; FL_HipX
+  (13,13) 5.0     ; FL_HipY
+  (14,14) 2.5     ; FL_Knee
+  (15,15) 5.0     ; HL_HipX
+  (16,16) 5.0     ; HL_HipY
+  (17,17) 2.5     ; HL_Knee
+  (18,18) 5.0     ; FR_HipX
+  (19,19) 5.0     ; FR_HipY
+  (20,20) 2.5     ; HR_Knee
+  (21,21) 5.0     ; HR_HipX
+  (22,22) 5.0     ; HR_HipY
+  (23,23) 2.5     ; HR_Knee
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, HL, HR] ;;
+  (0,0)   1.0       ; front_left_force
+  (1,1)   1.0       ; front_left_force
+  (2,2)   1.0       ; front_left_force
+  (3,3)   1.0       ; front_right_force
+  (4,4)   1.0       ; front_right_force
+  (5,5)   1.0       ; front_right_force
+  (6,6)   1.0       ; rear_left_force
+  (7,7)   1.0       ; rear_left_force
+  (8,8)   1.0       ; rear_left_force
+  (9,9)   1.0       ; rear_right_force
+  (10,10) 1.0       ; rear_right_force
+  (11,11) 1.0       ; rear_right_force
+
+  ;; foot velocity relative to base: [FL, HL, FR, HR] (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_SHANK, FR_SHANK"
+    [1] "HL_SHANK, HR_SHANK"
+    [2] "FL_SHANK, HL_SHANK"
+    [3] "FR_SHANK, HR_SHANK"
+    [4] "FL_FOOT, FR_FOOT"
+    [5] "HL_FOOT, HR_FOOT"
+    [6] "FL_FOOT, HL_FOOT"
+    [7] "FR_FOOT, HR_FOOT"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  24.0     ; HAA
+   (1,0)  24.0     ; HFE
+   (2,0)  36.0     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   300
+    kd                   20
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.01
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

src/robot_descriptions/deep_robotics/lite3_description/config/robot_control.yaml

@@ -0,0 +1,249 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # 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
+
+    rl_quadruped_controller:
+      type: rl_quadruped_controller/LeggedGymController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+    
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    joints:
+      - FR_HipX
+      - FR_HipY
+      - FR_Knee
+      - FL_HipX
+      - FL_HipY
+      - FL_Knee
+      - HR_HipX
+      - HR_HipY
+      - HR_Knee
+      - HL_HipX
+      - HL_HipY
+      - HL_Knee
+
+    down_pos:
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+
+    stand_pos:
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+      - 0.0
+      - -0.732
+      - 1.361
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - FR_FOOT
+      - FL_FOOT
+      - HR_FOOT
+      - HL_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: 100  # Hz
+    default_kd: 1.2
+    joints:
+      - FL_HipX
+      - FL_HipY
+      - FL_Knee
+      - FR_HipX
+      - FR_HipY
+      - FR_Knee
+      - HL_HipX
+      - HL_HipY
+      - HL_Knee
+      - HR_HipX
+      - HR_HipY
+      - HR_Knee
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet:
+      - FL_FOOT
+      - FR_FOOT
+      - HL_FOOT
+      - HR_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
+      - HL
+      - FR
+      - HR
+
+rl_quadruped_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    robot_pkg: "lite3_description"
+    model_folder: "legged_gym"
+    joints:
+      - FL_HipX
+      - FL_HipY
+      - FL_Knee
+      - FR_HipX
+      - FR_HipY
+      - FR_Knee
+      - HL_HipX
+      - HL_HipY
+      - HL_Knee
+      - HR_HipX
+      - HR_HipY
+      - HR_Knee
+
+    down_pos:
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+      - 0.0
+      - -1.22
+      - 2.61
+
+    stand_pos:
+      - 0.0
+      - -1.0
+      - 1.8
+      - 0.0
+      - -1.0
+      - 1.8
+      - 0.0
+      - -1.0
+      - 1.8
+      - 0.0
+      - -1.0
+      - 1.8
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - FL_FOOT
+      - FR_FOOT
+      - HL_FOOT
+      - HR_FOOT
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - FR
+      - HL
+      - HR
+
+    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

src/robot_descriptions/deep_robotics/lite3_description/config/visualize_urdf.rviz

@@ -0,0 +1,254 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 295
+  - 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_FOOT:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_HIP:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_SHANK:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FL_THIGH:
+          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_SHANK:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        FR_THIGH:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HL_FOOT:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HL_HIP:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HL_SHANK:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HL_THIGH:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HR_FOOT:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HR_HIP:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HR_SHANK:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HR_THIGH:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        INERTIA:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        Link Tree Style: Links in Alphabetic Order
+        TORSO:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          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: TORSO
+    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: 1.1684969663619995
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.011728689074516296
+        Y: 0.011509218253195286
+        Z: -0.10348724573850632
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.42539793252944946
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.6953961849212646
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 691
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000001f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001f1000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bb0000005efc0100000002fb0000000800540069006d00650100000000000003bb0000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000025f000001f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 955
+  X: 773
+  Y: 168

src/robot_descriptions/deep_robotics/lite3_description/launch/visualize.launch.py

@@ -0,0 +1,49 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+import xacro
+
+package_description = "lite3_description"
+
+def process_xacro():
+    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)
+    return robot_description_config.toxml()
+def generate_launch_description():
+    
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+    robot_description = process_xacro()
+
+    return LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        ),
+        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',
+        )
+    ])

src/robot_descriptions/deep_robotics/lite3_description/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lite3_description</name>
+  <version>0.0.0</version>
+  <description>The lite3_description package</description>
+
+  <maintainer email="1973671061@qq.com">ZhenyuXu</maintainer>
+  <license>MIT</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>

src/robot_descriptions/deep_robotics/lite3_description/urdf/robot.urdf

@@ -0,0 +1,839 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from robot.xacro                    | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="lite3">
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <robotNamespace>/lite3_gazebo</robotNamespace>
+      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+    </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>
+  <gazebo reference="imu_link">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <!-- Foot contacts. -->
+  <gazebo reference="FR_SHANK">
+    <sensor name="FR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libRobotFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FR_SHANK_fixed_joint_lump__FR_FOOT_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="FL_SHANK">
+    <sensor name="FL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libRobotFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>FL_SHANK_fixed_joint_lump__FL_FOOT_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="HR_SHANK">
+    <sensor name="RR_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libRobotFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>HR_SHANK_fixed_joint_lump__HR_FOOT_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="HL_SHANK">
+    <sensor name="RL_foot_contact" type="contact">
+      <update_rate>100</update_rate>
+      <plugin filename="libRobotFootContactPlugin.so" name="contactPlugin"/>
+      <contact>
+        <collision>HL_SHANK_fixed_joint_lump__HL_FOOT_collision_1</collision>
+      </contact>
+    </sensor>
+  </gazebo>
+  <gazebo reference="TORSO">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FL_HIP">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <gazebo reference="FL_THIGH">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FL_SHANK">
+    <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>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FR_HIP">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <gazebo reference="FR_THIGH">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="FR_SHANK">
+    <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>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="HL_HIP">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <gazebo reference="HL_THIGH">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="HL_SHANK">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="HL_FOOT">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="HR_HIP">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+  </gazebo>
+  <gazebo reference="HR_THIGH">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <gazebo reference="HR_SHANK">
+    <mu1>0.2</mu1>
+    <mu2>0.2</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+  <gazebo reference="HR_FOOT">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+  </gazebo>
+  <joint name="imu_joint" type="fixed">
+    <parent link="TORSO"/>
+    <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>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+  <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="TORSO"/>
+  </joint>
+  <link name="TORSO">
+    <visual>
+      <origin rpy="0 0 3.1416" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/Lite3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.35 0.184 0.08"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="INERTIA">
+    <inertial>
+      <origin xyz="0.004098 -0.000663 -0.002069"/>
+      <mass value="4.130"/>
+      <inertia ixx="0.016982120" ixy="2.1294E-05" ixz="6.0763E-05" iyy="0.030466501" iyz="1.7968E-05" izz="0.042609956"/>
+    </inertial>
+  </link>
+  <joint name="Torso2Inertia" type="fixed">
+    <parent link="TORSO"/>
+    <child link="INERTIA"/>
+  </joint>
+  <link name="FL_HIP">
+    <inertial>
+      <origin xyz="-0.0047 -0.0091 -0.0018"/>
+      <mass value="0.428"/>
+      <inertia ixx="0.00014538" ixy="8.1579E-07" ixz="-1.264E-05" iyy="0.00024024" iyz="1.3443E-06" izz="0.00013038"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.05 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/FL_HIP.dae"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="FL_HipX" type="revolute">
+    <origin xyz="0.1745 0.062 0"/>
+    <parent link="TORSO"/>
+    <child link="FL_HIP"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="24" lower="-0.42" upper="0.42" velocity="26"/>
+  </joint>
+  <link name="FL_THIGH">
+    <inertial>
+      <origin xyz="-0.00523 -0.0216 -0.0273"/>
+      <mass value="0.61"/>
+      <inertia ixx="0.001" ixy="-2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="3.75E-07" izz="2.68E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/l_thigh.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08"/>
+      <geometry>
+        <box size="0.02 0.02 0.16"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FL_HipY" type="revolute">
+    <origin xyz="0 0.0985 0"/>
+    <parent link="FL_HIP"/>
+    <child link="FL_THIGH"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="24" lower="-2.67" upper="0.314" velocity="26"/>
+  </joint>
+  <link name="FL_SHANK">
+    <inertial>
+      <origin xyz="0.00585 -8.732E-07 -0.12"/>
+      <mass value="0.115"/>
+      <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/SHANK.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.09"/>
+      <geometry>
+        <box size="0.02 0.02 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FL_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="FL_THIGH"/>
+    <child link="FL_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="36" lower="0.6" upper="2.72" velocity="17"/>
+  </joint>
+  <link name="FL_FOOT">
+    <inertial>
+      <mass value="1E-12"/>
+      <inertia ixx="1E-12" ixy="0" ixz="0" iyy="1E-12" iyz="0" izz="1E-12"/>
+    </inertial>
+    <collision>
+      <geometry>
+        <sphere radius="0.013"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FL_Ankle" type="fixed">
+    <origin xyz="0 0 -0.21"/>
+    <parent link="FL_SHANK"/>
+    <child link="FL_FOOT"/>
+  </joint>
+  <link name="FR_HIP">
+    <inertial>
+      <origin xyz="-0.0047 0.0091 -0.0018"/>
+      <mass value="0.428"/>
+      <inertia ixx="0.00014538" ixy="-8.1551E-07" ixz="-1.2639E-05" iyy="0.00024024" iyz="-1.3441E-06" izz="0.00013038"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.05 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/FR_HIP.dae"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="FR_HipX" type="revolute">
+    <origin xyz="0.1745 -0.062 0"/>
+    <parent link="TORSO"/>
+    <child link="FR_HIP"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="24" lower="-0.42" upper="0.42" velocity="26"/>
+  </joint>
+  <link name="FR_THIGH">
+    <inertial>
+      <origin xyz="-0.00523 0.0216 -0.0273"/>
+      <mass value="0.61"/>
+      <inertia ixx="0.001" ixy="2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="-3.75E-07" izz="2.68E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/r_thigh.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08"/>
+      <geometry>
+        <box size="0.02 0.02 0.16"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FR_HipY" type="revolute">
+    <origin xyz="0 -0.0985 0"/>
+    <parent link="FR_HIP"/>
+    <child link="FR_THIGH"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="24" lower="-2.67" upper="0.314" velocity="26"/>
+  </joint>
+  <link name="FR_SHANK">
+    <inertial>
+      <origin xyz="0.00585 -8.732E-07 -0.12"/>
+      <mass value="0.115"/>
+      <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/SHANK.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.09"/>
+      <geometry>
+        <box size="0.02 0.02 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FR_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="FR_THIGH"/>
+    <child link="FR_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="36" lower="0.6" upper="2.72" velocity="17"/>
+  </joint>
+  <link name="FR_FOOT">
+    <inertial>
+      <mass value="1E-12"/>
+      <inertia ixx="1E-12" ixy="0" ixz="0" iyy="1E-12" iyz="0" izz="1E-12"/>
+    </inertial>
+    <collision>
+      <geometry>
+        <sphere radius="0.013"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="FR_Ankle" type="fixed">
+    <origin xyz="0 0 -0.21"/>
+    <parent link="FR_SHANK"/>
+    <child link="FR_FOOT"/>
+  </joint>
+  <link name="HL_HIP">
+    <inertial>
+      <origin xyz="0.0047 -0.0091 -0.0018"/>
+      <mass value="0.428"/>
+      <inertia ixx="0.00014538" ixy="-8.1585E-07" ixz="1.2639E-05" iyy="0.00024024" iyz="1.3444E-06" izz="0.00013038"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/HL_HIP.dae"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="HL_HipX" type="revolute">
+    <origin xyz="-0.1745 0.062 0"/>
+    <parent link="TORSO"/>
+    <child link="HL_HIP"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="24" lower="-0.42" upper="0.42" velocity="26"/>
+  </joint>
+  <link name="HL_THIGH">
+    <inertial>
+      <origin xyz="-0.00523 -0.0216 -0.0273"/>
+      <mass value="0.61"/>
+      <inertia ixx="0.001" ixy="-2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="3.75E-07" izz="2.68E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/l_thigh.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08"/>
+      <geometry>
+        <box size="0.02 0.02 0.16"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HL_HipY" type="revolute">
+    <origin xyz="0 0.0985 0"/>
+    <parent link="HL_HIP"/>
+    <child link="HL_THIGH"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="24" lower="-2.67" upper="0.314" velocity="26"/>
+  </joint>
+  <link name="HL_SHANK">
+    <inertial>
+      <origin xyz="0.00585 -8.732E-07 -0.12"/>
+      <mass value="0.115"/>
+      <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/SHANK.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.09"/>
+      <geometry>
+        <box size="0.02 0.02 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HL_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="HL_THIGH"/>
+    <child link="HL_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="36" lower="0.6" upper="2.72" velocity="17"/>
+  </joint>
+  <link name="HL_FOOT">
+    <inertial>
+      <mass value="1E-12"/>
+      <inertia ixx="1E-12" ixy="0" ixz="0" iyy="1E-12" iyz="0" izz="1E-12"/>
+    </inertial>
+    <collision>
+      <geometry>
+        <sphere radius="0.013"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HL_Ankle" type="fixed">
+    <origin xyz="0 0 -0.21"/>
+    <parent link="HL_SHANK"/>
+    <child link="HL_FOOT"/>
+  </joint>
+  <link name="HR_HIP">
+    <inertial>
+      <origin xyz="0.0047 0.0091 -0.0018"/>
+      <mass value="0.428"/>
+      <inertia ixx="0.00014538" ixy="8.1545E-07" ixz="1.2639E-05" iyy="0.00024024" iyz="-1.344E-06" izz="0.00013038"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.05 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/HR_HIP.dae"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="HR_HipX" type="revolute">
+    <origin xyz="-0.1745 -0.062 0"/>
+    <parent link="TORSO"/>
+    <child link="HR_HIP"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="24" lower="-0.42" upper="0.42" velocity="26"/>
+  </joint>
+  <link name="HR_THIGH">
+    <inertial>
+      <origin xyz="-0.00523 0.0216 -0.0273"/>
+      <mass value="0.61"/>
+      <inertia ixx="0.001" ixy="2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="-3.75E-07" izz="2.68E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/r_thigh.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.08"/>
+      <geometry>
+        <box size="0.02 0.02 0.16"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HR_HipY" type="revolute">
+    <origin xyz="0 -0.0985 0"/>
+    <parent link="HR_HIP"/>
+    <child link="HR_THIGH"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="24" lower="-2.67" upper="0.314" velocity="26"/>
+  </joint>
+  <link name="HR_SHANK">
+    <inertial>
+      <origin xyz="0.00585 -8.732E-07 -0.12"/>
+      <mass value="0.115"/>
+      <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/lite3_description/share/lite3_description/meshes/SHANK.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.09"/>
+      <geometry>
+        <box size="0.02 0.02 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HR_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="HR_THIGH"/>
+    <child link="HR_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="36" lower="0.6" upper="2.72" velocity="17"/>
+  </joint>
+  <link name="HR_FOOT">
+    <inertial>
+      <mass value="1E-12"/>
+      <inertia ixx="1E-12" ixy="0" ixz="0" iyy="1E-12" iyz="0" izz="1E-12"/>
+    </inertial>
+    <collision>
+      <geometry>
+        <sphere radius="0.013"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="HR_Ankle" type="fixed">
+    <origin xyz="0 0 -0.21"/>
+    <parent link="HR_SHANK"/>
+    <child link="HR_FOOT"/>
+  </joint>
+  <!-- FL transmissions -->
+  <transmission name="FL_HipX_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_HipX">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_HipX_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FL_HipY_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_HipY">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_HipY_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FL_Knee_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FL_Knee">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FL_Knee_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- FR transmissions -->
+  <transmission name="FR_HipX_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_HipX">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_HipX_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FR_HipY_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_HipY">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_HipY_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="FR_Knee_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="FR_Knee">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="FR_Knee_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- HL transmissions -->
+  <transmission name="HL_HipX_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HL_HipX">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HL_HipX_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="HL_HipY_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HL_HipY">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HL_HipY_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="HL_Knee_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HL_Knee">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HL_Knee_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!-- HR transmissions -->
+  <transmission name="HR_HipX_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HR_HipX">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HR_HipX_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="HR_HipY_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HR_HipY">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HR_HipY_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="HR_Knee_tran">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="HR_Knee">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="HR_Knee_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <ros2_control name="UnitreeSystem" type="system">
+    <hardware>
+      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    </hardware>
+    <joint name="FR_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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="HR_HipX">
+      <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="HR_HipY">
+      <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="HR_Knee">
+      <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="HL_HipX">
+      <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="HL_HipY">
+      <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="HL_Knee">
+      <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="HR"/>
+      <state_interface name="HL"/>
+    </sensor>
+  </ros2_control>
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/const.xacro

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:property name="damping" value="0.0"/>
+  <xacro:property name="friction" value="0.1"/>
+
+  <xacro:property name="thigh_offset" value="0.0955"/>
+
+  <xacro:property name="leg_offset_x" value="0.1745"/>
+  <xacro:property name="leg_offset_y" value="0.062"/>
+
+  <!-- hip (left front) -->
+  <xacro:property name="hip_mass" value="0.428"/>
+  <xacro:property name="hip_com_x" value="-0.0047"/>
+  <xacro:property name="hip_com_y" value="-0.0091"/>
+  <xacro:property name="hip_com_z" value="-0.0018"/>
+  <xacro:property name="hip_ixx" value="0.00014538"/>
+  <xacro:property name="hip_ixy" value="8.1579E-07"/>
+  <xacro:property name="hip_ixz" value="-1.264E-05"/>
+  <xacro:property name="hip_iyy" value="0.00024024"/>
+  <xacro:property name="hip_iyz" value="1.3443E-06"/>
+  <xacro:property name="hip_izz" value="0.00013038"/>
+
+  <!-- thigh -->
+  <xacro:property name="thigh_mass" value="0.61"/>
+  <xacro:property name="thigh_com_x" value="-0.00523"/>
+  <xacro:property name="thigh_com_y" value="-0.0216"/>
+  <xacro:property name="thigh_com_z" value="-0.0273"/>
+  <xacro:property name="thigh_ixx" value="0.001"/>
+  <xacro:property name="thigh_ixy" value="-2.5E-06"/>
+  <xacro:property name="thigh_ixz" value="-1.12E-04"/>
+  <xacro:property name="thigh_iyy" value="0.00116"/>
+  <xacro:property name="thigh_iyz" value="3.75E-07"/>
+  <xacro:property name="thigh_izz" value="2.68E-04"/>
+
+  <!-- foot -->
+  <xacro:property name="foot_mass" value="0.06"/>
+  <xacro:property name="foot_radius" value="0.013"/>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/gazebo.xacro

@@ -0,0 +1,289 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>
+
+  <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
+    </hardware>
+
+    <joint name="FR_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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="HR_HipX">
+      <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="HR_HipY">
+      <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="HR_Knee">
+      <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="HL_HipX">
+      <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="HL_HipY">
+      <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="HL_Knee">
+      <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.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="orientation.w"/>
+      <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_foot_force"/>
+      <state_interface name="FL_foot_force"/>
+      <state_interface name="HR_foot_force"/>
+      <state_interface name="HL_foot_force"/>
+    </sensor>
+
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
+      <parameters>$(find lite3_description)/config/gazebo.yaml</parameters>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
+    </plugin>
+    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
+    <!--    <plugin filename="gz-sim-odometry-publisher-system"-->
+    <!--            name="gz::sim::systems::OdometryPublisher">-->
+    <!--      <odom_frame>odom</odom_frame>-->
+    <!--      <robot_base_frame>base</robot_base_frame>-->
+    <!--      <odom_publish_frequency>1000</odom_publish_frequency>-->
+    <!--      <odom_topic>odom</odom_topic>-->
+    <!--      <dimensions>3</dimensions>-->
+    <!--      <odom_covariance_topic>odom_with_covariance</odom_covariance_topic>-->
+    <!--      <tf_topic>tf</tf_topic>-->
+    <!--    </plugin>-->
+  </gazebo>
+
+  <gazebo reference="trunk">
+    <mu1>0.6</mu1>
+    <mu2>0.6</mu2>
+    <self_collide>1</self_collide>
+  </gazebo>
+
+  <gazebo reference="imu_link">
+    <sensor name="imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>500</update_rate>
+      <visualize>true</visualize>
+      <topic>imu</topic>
+      <imu>
+        <angular_velocity>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </z>
+        </angular_velocity>
+        <linear_acceleration>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </z>
+        </linear_acceleration>
+      </imu>
+    </sensor>
+  </gazebo>
+
+  <gazebo reference="imu_joint">
+    <disableFixedJointLumping>true</disableFixedJointLumping>
+  </gazebo>
+
+  <xacro:foot_force_sensor name="FL" suffix="Ankle"/>
+  <xacro:foot_force_sensor name="HL" suffix="Ankle"/>
+  <xacro:foot_force_sensor name="FR" suffix="Ankle"/>
+  <xacro:foot_force_sensor name="HR" suffix="Ankle"/>
+
+  <xacro:arg name="EXTERNAL_SENSORS" default="false"/>
+  <xacro:if value="$(arg EXTERNAL_SENSORS)">
+    <xacro:include filename="$(find gz_quadruped_playground)/models/D435/model.xacro"/>
+    <xacro:include filename="$(find gz_quadruped_playground)/models/Lidar3DV1/model.xacro"/>
+
+    <xacro:d435 camID="1" name="d435">
+      <origin rpy="0 0.1 0" xyz="0.23 0 0.06"/>
+    </xacro:d435>
+
+    <xacro:Lidar3D vertical="16" name="lslidar">
+      <origin rpy="0 0 0" xyz="0.163 0 0.058"/>
+    </xacro:Lidar3D>
+
+  </xacro:if>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/gazebo_classic.xacro

@@ -0,0 +1,238 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+	<ros2_control name="GazeboSystem" type="system">
+		<hardware>
+			<plugin>gazebo_ros2_control/GazeboSystem</plugin>
+		</hardware>
+
+		<joint name="FR_HipX">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="FR_HipY">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="FR_Knee">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="FL_HipX">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="FL_HipY">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="FL_Knee">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HR_HipX">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HR_HipY">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HR_Knee">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HL_HipX">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HL_HipY">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<joint name="HL_Knee">
+			<command_interface name="effort" initial_value="0.0"/>
+			<state_interface name="position"/>
+			<state_interface name="velocity"/>
+			<state_interface name="effort"/>
+		</joint>
+
+		<sensor name="imu_sensor">
+			<state_interface name="orientation.x"/>
+			<state_interface name="orientation.y"/>
+			<state_interface name="orientation.z"/>
+			<state_interface name="orientation.w"/>
+			<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>
+	</ros2_control>
+
+	<gazebo>
+		<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+			<parameters>$(find lite3_description)/config/gazebo.yaml</parameters>
+		</plugin>
+	</gazebo>
+
+	<gazebo reference="trunk">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+	</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>
+
+	<gazebo reference="TORSO">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FL_HIP">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+	</gazebo>
+	<gazebo reference="FL_THIGH">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FL_SHANK">
+		<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>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FR_HIP">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+	</gazebo>
+	<gazebo reference="FR_THIGH">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="FR_SHANK">
+		<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>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="HL_HIP">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+	</gazebo>
+	<gazebo reference="HL_THIGH">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="HL_SHANK">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="HL_FOOT">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="HR_HIP">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+	</gazebo>
+	<gazebo reference="HR_THIGH">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+	<gazebo reference="HR_SHANK">
+		<mu1>0.2</mu1>
+		<mu2>0.2</mu2>
+		<self_collide>1</self_collide>
+	</gazebo>
+	<gazebo reference="HR_FOOT">
+		<mu1>0.6</mu1>
+		<mu2>0.6</mu2>
+		<self_collide>1</self_collide>
+		<kp value="1000000.0"/>
+		<kd value="1.0"/>
+	</gazebo>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/leg.xacro

@@ -0,0 +1,164 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find lite3_description)/xacro/transmission.xacro"/>
+
+  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+
+    <joint name="${name}_HipX" 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="TORSO"/>
+      <child link="${name}_HIP"/>
+      <axis xyz="-1 0 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit lower="-0.42" upper="0.42" effort="24" velocity="26"/>
+    </joint>
+
+    <link name="${name}_HIP">
+      <inertial>
+        <inertia ixx="0.00014538" ixy="8.1579E-07" ixz="-1.264E-05" iyy="0.00024024" iyz="1.3443E-06" izz="0.00013038"/>
+        <origin 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>
+      <visual>
+        <origin rpy="0 0 0" xyz="${-0.05*front_hind} 0.0 0.0"/>
+        <geometry>
+          <mesh filename="file://$(find lite3_description)/meshes/${name}_HIP.dae"/>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="${name}_HipY" type="revolute">
+      <origin 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 lower="-2.67" upper="0.314" effort="24" velocity="26"/>
+    </joint>
+
+    <link name="${name}_THIGH">
+      <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}"/>
+        <inertia ixx="0.001" ixy="-2.5E-06" ixz="-1.12E-04" iyy="0.00116" iyz="3.75E-07" izz="2.68E-04"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+        <geometry>
+          <xacro:if value="${mirror_dae == True}">
+            <mesh filename="file://$(find lite3_description)/meshes/l_thigh.dae"/>
+          </xacro:if>
+          <xacro:if value="${mirror_dae == False}">
+            <mesh filename="file://$(find lite3_description)/meshes/r_thigh.dae"/>
+          </xacro:if>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.08"/>
+        <geometry>
+          <box size="0.02 0.02 0.16"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_Knee" type="revolute">
+      <origin xyz="0 0 -0.20"/>
+      <parent link="${name}_THIGH"/>
+      <child link="${name}_SHANK"/>
+      <axis xyz="0 -1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit lower="0.6" upper="2.72" effort="36" velocity="17"/>
+    </joint>
+
+    <link name="${name}_SHANK">
+      <inertial>
+        <origin xyz="0.00585 -8.732E-07 -0.12"/>
+        <mass value="0.115"/>
+        <inertia ixx="6.68E-04" ixy="-1.24E-08" ixz="6.91E-06" iyy="6.86E-04" iyz="5.65E-09" izz="3.155E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="3.14 -1.42 0" xyz="0.0 0.0 0.0"/>
+        <geometry>
+          <mesh filename="file://$(find lite3_description)/meshes/SHANK.dae"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 -0.09"/>
+        <geometry>
+          <box size="0.02 0.02 0.18"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_Ankle" type="fixed">
+      <origin xyz="0 0 -0.21"/>
+      <parent link="${name}_SHANK"/>
+      <child link="${name}_FOOT"/>
+    </joint>
+
+    <link name="${name}_FOOT">
+      <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>
+      <collision>
+        <geometry>
+          <sphere radius="${foot_radius}"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <gazebo reference="${name}_HIP">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+    </gazebo>
+
+    <gazebo reference="${name}_THIGH">
+      <mu1>0.2</mu1>
+      <mu2>0.2</mu2>
+      <self_collide>0</self_collide>
+      <kp value="1000000.0"/>
+      <kd value="100.0"/>
+    </gazebo>
+
+    <gazebo reference="${name}_SHANK">
+      <mu1>0.6</mu1>
+      <mu2>0.6</mu2>
+      <self_collide>1</self_collide>
+    </gazebo>
+
+    <gazebo reference="${name}_FOOT">
+      <mu1>0.6</mu1>
+      <mu2>0.6</mu2>
+      <self_collide>1</self_collide>
+      <kp value="1000000.0"/>
+      <kd value="100.0"/>
+    </gazebo>
+
+    <xacro:leg_transmission name="${name}"/>
+
+  </xacro:macro>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/robot.xacro

@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+
+<robot name="lite3" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find lite3_description)/xacro/const.xacro"/>
+  <xacro:include filename="$(find lite3_description)/xacro/leg.xacro"/>
+
+  <xacro:arg name="GAZEBO" default="false"/>
+  <xacro:arg name="CLASSIC" default="false"/>
+
+
+  <xacro:if value="$(arg GAZEBO)">
+    <xacro:if value="$(arg CLASSIC)">
+      <xacro:include filename="$(find lite3_description)/xacro/gazebo_classic.xacro"/>
+    </xacro:if>
+    <xacro:unless value="$(arg CLASSIC)">
+      <xacro:include filename="$(find lite3_description)/xacro/gazebo.xacro"/>
+    </xacro:unless>
+  </xacro:if>
+  <xacro:unless value="$(arg GAZEBO)">
+    <xacro:include filename="$(find lite3_description)/xacro/ros2_control.xacro"/>
+  </xacro:unless>
+
+  <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="TORSO"/>
+  </joint>
+
+  <link name="TORSO">
+    <visual>
+      <origin rpy="0 0 3.1416" xyz="0.0 0.0 0.0"/>
+      <geometry>
+        <mesh filename="file://$(find lite3_description)/meshes/Lite3.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.35 0.184 0.08"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0.004098 -0.000663 -0.002069"/>
+      <mass value="4.130"/>
+      <inertia ixx="0.016982120" ixy="2.1294E-05" ixz="6.0763E-05" iyy="0.030466501" iyz="1.7968E-05"
+               izz="0.042609956"/>
+    </inertial>
+  </link>
+
+  <joint name="imu_joint" type="fixed">
+    <parent link="TORSO"/>
+    <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>
+    </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="HR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False"/>
+  <xacro:leg name="HL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False"/>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_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_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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_HipX">
+      <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_HipY">
+      <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_Knee">
+      <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="HR_HipX">
+      <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="HR_HipY">
+      <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="HR_Knee">
+      <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="HL_HipX">
+      <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="HL_HipY">
+      <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="HL_Knee">
+      <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="HR"/>
+      <state_interface name="HL"/>
+    </sensor>
+
+  </ros2_control>
+
+</robot>

src/robot_descriptions/deep_robotics/lite3_description/xacro/transmission.xacro

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="leg_transmission" params="name">
+    <transmission name="${name}_HipX_tran">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="${name}_HipX">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="${name}_HipX_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+    <transmission name="{name}_HipY_tran">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="{name}_HipY">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="{name}_HipY_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+    <transmission name="{name}_Knee_tran">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="{name}_Knee">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="{name}_Knee_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+  </xacro:macro>
+
+</robot>

src/robot_descriptions/deep_robotics/x30_description/CMakeLists.txt

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