Merge pull request #4 from legubiao/legged_gym

Legged gym

README.md

@@ -13,7 +13,8 @@ Todo List:
 - [x] [Leg PD Controller](controllers/leg_pd_controller)
 - [x] [Contact Sensor Simulation](https://github.com/legubiao/unitree_mujoco)
 - [x] [OCS2 Quadruped Control](controllers/ocs2_quadruped_controller)
-- [ ] Learning-based Controller
+- [x] [Learning-based Controller](controllers/rl_quadruped_controller/)
+- [ ] Fully understand the RL Workflow
 
 Video for Unitree Guide Controller:
 [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)
@@ -53,4 +54,6 @@ For more details, please refer to the [unitree guide controller](controllers/uni
 ### Miscellaneous
 [1] Unitree Robotics. *unitree\_guide: An open source project for controlling the quadruped robot of Unitree Robotics, and it is also the software project accompanying 《四足机器人控制算法--建模、控制与实践》 published by Unitree Robotics*. [Online]. Available: [https://github.com/unitreerobotics/unitree_guide](https://github.com/unitreerobotics/unitree_guide)
 
-[2] Qiayuan Liao. *legged\_control: An open-source NMPC, WBC, state estimation, and sim2real framework for legged robots*. [Online]. Available: [https://github.com/qiayuanl/legged_control](https://github.com/qiayuanl/legged_control)
+[2] Qiayuan Liao. *legged\_control: An open-source NMPC, WBC, state estimation, and sim2real framework for legged robots*. [Online]. Available: [https://github.com/qiayuanl/legged_control](https://github.com/qiayuanl/legged_control)
+
+[3] Ziqi Fan. *rl\_sar: Simulation Verification and Physical Deployment of Robot Reinforcement Learning Algorithm.* 2024. Available: [https://github.com/fan-ziqi/rl_sar](https://github.com/fan-ziqi/rl_sar) 

controllers/ocs2_quadruped_controller/README.md

@@ -3,8 +3,32 @@
 This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control)
 and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
 
+Tested environment:
+* Ubuntu 24.04
+  * ROS2 Jazzy
+
 [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)
 
+## 1. Interfaces
+
+Required hardware interfaces:
+
+* command:
+  * joint position
+  * joint velocity
+  * joint effort
+  * KP
+  * KD
+* state:
+  * joint effort
+  * joint position
+  * joint velocity
+  * imu sensor
+    * linear acceleration
+    * angular velocity
+    * orientation
+  * feet force sensor
+
 ## 2. Build
 
 ### 2.1 Build Dependencies

controllers/ocs2_quadruped_controller/package.xml

@@ -3,8 +3,8 @@
 <package format="3">
   <name>ocs2_quadruped_controller</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
+  <description>A ROS2-Control quadruped controller based on OCS2 library</description>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

controllers/rl_quadruped_controller/CMakeLists.txt

@@ -0,0 +1,91 @@
+cmake_minimum_required(VERSION 3.18 FATAL_ERROR)
+set(CMAKE_CXX_STANDARD 17)
+project(rl_quadruped_controller)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+find_package(ament_cmake REQUIRED)
+
+find_package(Torch REQUIRED)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
+
+find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+find_package(Python3 COMPONENTS NumPy)
+
+find_package(yaml-cpp REQUIRED)
+
+set(dependencies
+        pluginlib
+        rcpputils
+        controller_interface
+        realtime_tools
+        control_input_msgs
+)
+
+# find dependencies
+foreach (Dependency IN ITEMS ${dependencies})
+    find_package(${Dependency} REQUIRED)
+endforeach ()
+
+add_library(${PROJECT_NAME} SHARED
+        src/LeggedGymController.cpp
+
+        src/common/ObservationBuffer.cpp
+
+        src/FSM/StatePassive.cpp
+        src/FSM/StateFixedStand.cpp
+        src/FSM/StateFixedDown.cpp
+        src/FSM/StateRL.cpp
+)
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+        PRIVATE
+        ${YAML_CPP_INCLUDE_DIR}
+        src)
+target_link_libraries(${PROJECT_NAME} PUBLIC
+        "${TORCH_LIBRARIES}"
+        yaml-cpp
+)
+ament_target_dependencies(
+        ${PROJECT_NAME} PUBLIC
+        ${dependencies}
+)
+
+pluginlib_export_plugin_description_file(controller_interface rl_quadruped_controller.xml)
+
+install(
+        DIRECTORY include/
+        DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+        TARGETS ${PROJECT_NAME}
+        EXPORT export_${PROJECT_NAME}
+        ARCHIVE DESTINATION lib/${PROJECT_NAME}
+        LIBRARY DESTINATION lib/${PROJECT_NAME}
+        RUNTIME DESTINATION bin
+)
+
+ament_export_dependencies(${dependencies})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+if (BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # comment the line when a copyright and license is added to all source files
+    set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # comment the line when this package is in a git repo and when
+    # a copyright and license is added to all source files
+    set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+endif ()
+
+ament_package()

controllers/rl_quadruped_controller/README.md

@@ -0,0 +1,34 @@
+# RL Quadruped Controller
+
+This repository contains the reinforcement learning based controllers for the quadruped robot.
+
+Tested environment:
+* Ubuntu 24.04
+    * ROS2 Jazzy
+
+
+## 2. Build
+### 2.1 Installing libtorch
+```bash
+cd ~/CLionProjects/
+wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
+unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip -d ./
+```
+```bash
+cd ~/CLionProjects/
+rm -rf libtorch-shared-with-deps-latest.zip
+echo 'export Torch_DIR=~/CLionProjects/libtorch' >> ~/.bashrc
+```
+
+### 2.2 Build Controller
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to rl_quadruped_controller
+```
+
+## 3. Launch
+* Unitree A1 Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch a1_description rl_control.launch.py
+  ```

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/FSMState.h

@@ -0,0 +1,40 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#ifndef FSMSTATE_H
+#define FSMSTATE_H
+
+#include <string>
+#include <utility>
+#include <rclcpp/time.hpp>
+
+#include "rl_quadruped_controller/common/enumClass.h"
+#include "rl_quadruped_controller/control/CtrlComponent.h"
+
+class FSMState {
+public:
+    virtual ~FSMState() = default;
+
+    FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlComponent &ctrl_comp)
+        : state_name(state_name),
+          state_name_string(std::move(state_name_string)),
+          ctrl_comp_(ctrl_comp) {
+    }
+
+    virtual void enter() = 0;
+
+    virtual void run() = 0;
+
+    virtual void exit() = 0;
+
+    virtual FSMStateName checkChange() { return FSMStateName::INVALID; }
+
+    FSMStateName state_name;
+    std::string state_name_string;
+
+protected:
+    CtrlComponent &ctrl_comp_;
+};
+
+#endif //FSMSTATE_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedDown.h

@@ -0,0 +1,37 @@
+//
+// Created by tlab-uav on 24-9-11.
+//
+
+#ifndef STATEFIXEDDOWN_H
+#define STATEFIXEDDOWN_H
+
+#include "FSMState.h"
+
+class StateFixedDown final : public FSMState {
+public:
+    explicit StateFixedDown(CtrlComponent &ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+private:
+    double target_pos_[12] = {
+        0.0473455, 1.22187, -2.44375, -0.0473455,
+        1.22187, -2.44375, 0.0473455, 1.22187,
+        -2.44375, -0.0473455, 1.22187, -2.44375
+    };
+
+    double start_pos_[12] = {};
+    rclcpp::Time start_time_;
+
+    double duration_ = 600; // steps
+    double percent_ = 0; //%
+    double phase = 0.0;
+};
+
+
+#endif //STATEFIXEDDOWN_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateFixedStand.h

@@ -0,0 +1,39 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#ifndef STATEFIXEDSTAND_H
+#define STATEFIXEDSTAND_H
+
+#include "FSMState.h"
+
+class StateFixedStand final : public FSMState {
+public:
+    explicit StateFixedStand(CtrlComponent &ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+
+private:
+    double target_pos_[12] = {
+        0.00571868, 0.608813, -1.21763,
+        -0.00571868, 0.608813, -1.21763,
+        0.00571868, 0.608813, -1.21763,
+        -0.00571868, 0.608813, -1.21763
+    };
+
+    double start_pos_[12] = {};
+    rclcpp::Time start_time_;
+
+    double duration_ = 600; // steps
+    double percent_ = 0; //%
+    double phase = 0.0;
+};
+
+
+#endif //STATEFIXEDSTAND_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StatePassive.h

@@ -0,0 +1,24 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#ifndef STATEPASSIVE_H
+#define STATEPASSIVE_H
+#include "FSMState.h"
+
+
+class StatePassive final : public FSMState {
+public:
+    explicit StatePassive(CtrlComponent &ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+};
+
+
+#endif //STATEPASSIVE_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/FSM/StateRL.h

@@ -0,0 +1,160 @@
+//
+// Created by biao on 24-10-6.
+//
+
+#ifndef STATERL_H
+#define STATERL_H
+
+#include <common/ObservationBuffer.h>
+#include <torch/script.h>
+
+#include "FSMState.h"
+
+template <typename Functor>
+void executeAndSleep(Functor f, const double frequency) {
+    using clock = std::chrono::high_resolution_clock;
+    const auto start = clock::now();
+
+    // Execute wrapped function
+    f();
+
+    // Compute desired duration rounded to clock decimation
+    const std::chrono::duration<double> desiredDuration(1.0 / frequency);
+    const auto dt = std::chrono::duration_cast<clock::duration>(desiredDuration);
+
+    // Sleep
+    const auto sleepTill = start + dt;
+    std::this_thread::sleep_until(sleepTill);
+}
+
+template<typename T>
+struct RobotCommand {
+    struct MotorCommand {
+        std::vector<T> q = std::vector<T>(32, 0.0);
+        std::vector<T> dq = std::vector<T>(32, 0.0);
+        std::vector<T> tau = std::vector<T>(32, 0.0);
+        std::vector<T> kp = std::vector<T>(32, 0.0);
+        std::vector<T> kd = std::vector<T>(32, 0.0);
+    } motor_command;
+};
+
+template<typename T>
+struct RobotState {
+    struct IMU {
+        std::vector<T> quaternion = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z
+        std::vector<T> gyroscope = {0.0, 0.0, 0.0};
+        std::vector<T> accelerometer = {0.0, 0.0, 0.0};
+    } imu;
+
+    struct MotorState {
+        std::vector<T> q = std::vector<T>(32, 0.0);
+        std::vector<T> dq = std::vector<T>(32, 0.0);
+        std::vector<T> ddq = std::vector<T>(32, 0.0);
+        std::vector<T> tauEst = std::vector<T>(32, 0.0);
+        std::vector<T> cur = std::vector<T>(32, 0.0);
+    } motor_state;
+};
+
+struct Control
+{
+    double x = 0.0;
+    double y = 0.0;
+    double yaw = 0.0;
+};
+
+struct ModelParams {
+    std::string model_name;
+    std::string framework;
+    bool use_history;
+    int decimation;
+    int num_observations;
+    std::vector<std::string> observations;
+    double damping;
+    double stiffness;
+    double action_scale;
+    double hip_scale_reduction;
+    std::vector<int> hip_scale_reduction_indices;
+    int num_of_dofs;
+    double lin_vel_scale;
+    double ang_vel_scale;
+    double dof_pos_scale;
+    double dof_vel_scale;
+    double clip_obs;
+    torch::Tensor clip_actions_upper;
+    torch::Tensor clip_actions_lower;
+    torch::Tensor torque_limits;
+    torch::Tensor rl_kd;
+    torch::Tensor rl_kp;
+    torch::Tensor fixed_kp;
+    torch::Tensor fixed_kd;
+    torch::Tensor commands_scale;
+    torch::Tensor default_dof_pos;
+};
+
+struct Observations
+{
+    torch::Tensor lin_vel;
+    torch::Tensor ang_vel;
+    torch::Tensor gravity_vec;
+    torch::Tensor commands;
+    torch::Tensor base_quat;
+    torch::Tensor dof_pos;
+    torch::Tensor dof_vel;
+    torch::Tensor actions;
+};
+
+class StateRL final : public FSMState {
+public:
+    explicit StateRL(CtrlComponent &ctrl_component, const std::string &config_path);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+
+private:
+    torch::Tensor computeObservation();
+
+    void loadYaml(const std::string &config_path);
+
+    static torch::Tensor quatRotateInverse(const torch::Tensor &q, const torch::Tensor& v, const std::string& framework);
+
+    /**
+    * @brief Forward the RL model to get the action
+    */
+    torch::Tensor forward();
+
+    void getState();
+
+    void runModel();
+
+    void setCommand() const;
+
+    // Parameters
+    ModelParams params_;
+    Observations obs_;
+    Control control_;
+
+    RobotState<double> robot_state_;
+    RobotCommand<double> robot_command_;
+
+    // history buffer
+    std::shared_ptr<ObservationBuffer> history_obs_buf_;
+    torch::Tensor history_obs_;
+
+    // rl module
+    torch::jit::script::Module model_;
+    std::thread rl_thread_;
+    bool running_ = false;
+    bool updated_ = false;
+
+    // output buffer
+    torch::Tensor output_torques;
+    torch::Tensor output_dof_pos_;
+};
+
+
+#endif //STATERL_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/common/enumClass.h

@@ -0,0 +1,22 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#ifndef ENUMCLASS_H
+#define ENUMCLASS_H
+
+enum class FSMStateName {
+    // EXIT,
+    INVALID,
+    PASSIVE,
+    FIXEDDOWN,
+    FIXEDSTAND,
+    RL,
+};
+
+enum class FSMMode {
+    NORMAL,
+    CHANGE
+};
+
+#endif //ENUMCLASS_H

controllers/rl_quadruped_controller/include/rl_quadruped_controller/control/CtrlComponent.h

@@ -0,0 +1,61 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#ifndef INTERFACE_H
+#define INTERFACE_H
+
+#include <vector>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/loaned_state_interface.hpp>
+#include <control_input_msgs/msg/inputs.hpp>
+
+struct CtrlComponent {
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_torque_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kp_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kd_command_interface_;
+
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_effort_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_position_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_velocity_state_interface_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    imu_state_interface_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    foot_force_state_interface_;
+
+    control_input_msgs::msg::Inputs control_inputs_;
+    int frequency_{};
+
+    CtrlComponent() {
+    }
+
+    void clear() {
+        joint_torque_command_interface_.clear();
+        joint_position_command_interface_.clear();
+        joint_velocity_command_interface_.clear();
+        joint_kd_command_interface_.clear();
+        joint_kp_command_interface_.clear();
+
+        joint_effort_state_interface_.clear();
+        joint_position_state_interface_.clear();
+        joint_velocity_state_interface_.clear();
+
+        imu_state_interface_.clear();
+        foot_force_state_interface_.clear();
+    }
+};
+
+#endif //INTERFACE_H

controllers/rl_quadruped_controller/package.xml

@@ -0,0 +1,23 @@
+<?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>rl_quadruped_controller</name>
+  <version>0.0.0</version>
+  <description>A Quadruped robot controller relied on .pt RL network</description>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>control_input_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

controllers/rl_quadruped_controller/rl_quadruped_controller.xml

@@ -0,0 +1,9 @@
+<library path="rl_quadruped_controller">
+    <class name="rl_quadruped_controller/LeggedGymController"
+           type="rl_quadruped_controller::LeggedGymController"
+           base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Quadruped Controller used RL-model trained in Legged Gym.
+        </description>
+    </class>
+</library>

controllers/rl_quadruped_controller/src/FSM/StateFixedDown.cpp

@@ -0,0 +1,52 @@
+//
+// Created by tlab-uav on 24-9-11.
+//
+
+#include "rl_quadruped_controller/FSM/StateFixedDown.h"
+
+#include <cmath>
+
+StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp): FSMState(
+    FSMStateName::FIXEDDOWN, "fixed down", ctrlComp) {
+    duration_ = ctrl_comp_.frequency_ * 1.2;
+}
+
+void StateFixedDown::enter() {
+    for (int i = 0; i < 12; i++) {
+        start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
+    }
+    ctrl_comp_.control_inputs_.command = 0;
+}
+
+void StateFixedDown::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_position_command_interface_[i].get().set_value(
+            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5);
+    }
+}
+
+void StateFixedDown::exit() {
+    percent_ = 0;
+}
+
+FSMStateName StateFixedDown::checkChange() {
+    if (percent_ < 1.5) {
+        return FSMStateName::FIXEDDOWN;
+    }
+    switch (ctrl_comp_.control_inputs_.command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDSTAND;
+        case 3:
+            return FSMStateName::RL;
+        default:
+            return FSMStateName::FIXEDDOWN;
+    }
+}

controllers/rl_quadruped_controller/src/FSM/StateFixedStand.cpp

@@ -0,0 +1,53 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#include "rl_quadruped_controller/FSM/StateFixedStand.h"
+
+#include <cmath>
+
+StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp): FSMState(
+    FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp) {
+    duration_ = ctrl_comp_.frequency_ * 1.2;
+}
+
+void StateFixedStand::enter() {
+    for (int i = 0; i < 12; i++) {
+        start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
+    }
+    ctrl_comp_.control_inputs_.command = 0;
+}
+
+void StateFixedStand::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_position_command_interface_[i].get().set_value(
+            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(
+            phase * 60.0 + (1 - phase) * 20.0);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
+    }
+}
+
+void StateFixedStand::exit() {
+    percent_ = 0;
+}
+
+FSMStateName StateFixedStand::checkChange() {
+    if (percent_ < 1.5) {
+        return FSMStateName::FIXEDSTAND;
+    }
+    switch (ctrl_comp_.control_inputs_.command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDDOWN;
+        case 3:
+            return FSMStateName::RL;
+        default:
+            return FSMStateName::FIXEDSTAND;
+    }
+}

controllers/rl_quadruped_controller/src/FSM/StatePassive.cpp

@@ -0,0 +1,43 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#include "rl_quadruped_controller/FSM/StatePassive.h"
+
+#include <iostream>
+
+StatePassive::StatePassive(CtrlComponent &ctrlComp) : FSMState(
+    FSMStateName::PASSIVE, "passive", ctrlComp) {
+}
+
+void StatePassive::enter() {
+    for (auto i: ctrl_comp_.joint_torque_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrl_comp_.joint_position_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrl_comp_.joint_velocity_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrl_comp_.joint_kp_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrl_comp_.joint_kd_command_interface_) {
+        i.get().set_value(1);
+    }
+    ctrl_comp_.control_inputs_.command = 0;
+}
+
+void StatePassive::run() {
+}
+
+void StatePassive::exit() {
+}
+
+FSMStateName StatePassive::checkChange() {
+    if (ctrl_comp_.control_inputs_.command == 2) {
+        return FSMStateName::FIXEDDOWN;
+    }
+    return FSMStateName::PASSIVE;
+}

controllers/rl_quadruped_controller/src/FSM/StateRL.cpp

@@ -0,0 +1,308 @@
+//
+// Created by biao on 24-10-6.
+//
+
+#include "rl_quadruped_controller/FSM/StateRL.h"
+
+#include <rclcpp/logging.hpp>
+#include <yaml-cpp/yaml.h>
+
+template<typename T>
+std::vector<T> ReadVectorFromYaml(const YAML::Node &node) {
+    std::vector<T> values;
+    for (const auto &val: node) {
+        values.push_back(val.as<T>());
+    }
+    return values;
+}
+
+template<typename T>
+std::vector<T> ReadVectorFromYaml(const YAML::Node &node, const std::string &framework, const int &rows,
+                                  const int &cols) {
+    std::vector<T> values;
+    for (const auto &val: node) {
+        values.push_back(val.as<T>());
+    }
+
+    if (framework == "isaacsim") {
+        std::vector<T> transposed_values(cols * rows);
+        for (int r = 0; r < rows; ++r) {
+            for (int c = 0; c < cols; ++c) {
+                transposed_values[c * rows + r] = values[r * cols + c];
+            }
+        }
+        return transposed_values;
+    }
+    if (framework == "isaacgym") {
+        return values;
+    }
+    throw std::invalid_argument("Unsupported framework: " + framework);
+}
+
+StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path) : FSMState(
+    FSMStateName::RL, "rl", ctrl_component) {
+    // read params from yaml
+    loadYaml(config_path);
+
+    // history
+    if (params_.use_history) {
+        history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, 6);
+    }
+
+    model_ = torch::jit::load(config_path + "/" + params_.model_name);
+    std::cout << "Model loaded: " << config_path + "/" + params_.model_name << std::endl;
+
+    rl_thread_ = std::thread([&] {
+        while (true) {
+            try {
+                executeAndSleep(
+                    [&] {
+                        if (running_) {
+                            runModel();
+                        }
+                    },
+                    ctrl_comp_.frequency_ / params_.decimation);
+            } catch (const std::exception &e) {
+                running_ = false;
+                RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what());
+            }
+        }
+    });
+}
+
+void StateRL::enter() {
+    // Init observations
+    obs_.lin_vel = torch::tensor({{0.0, 0.0, 0.0}});
+    obs_.ang_vel = torch::tensor({{0.0, 0.0, 0.0}});
+    obs_.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}});
+    obs_.commands = torch::tensor({{0.0, 0.0, 0.0}});
+    obs_.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}});
+    obs_.dof_pos = params_.default_dof_pos;
+    obs_.dof_vel = torch::zeros({1, params_.num_of_dofs});
+    obs_.actions = torch::zeros({1, params_.num_of_dofs});
+
+    // Init output
+    output_torques = torch::zeros({1, params_.num_of_dofs});
+    output_dof_pos_ = params_.default_dof_pos;
+
+    // Init control
+    control_.x = 0.0;
+    control_.y = 0.0;
+    control_.yaw = 0.0;
+
+    running_ = true;
+}
+
+void StateRL::run() {
+    getState();
+    setCommand();
+}
+
+void StateRL::exit() {
+    running_ = false;
+}
+
+FSMStateName StateRL::checkChange() {
+    switch (ctrl_comp_.control_inputs_.command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDDOWN;
+        default:
+            return FSMStateName::RL;
+    }
+}
+
+torch::Tensor StateRL::computeObservation() {
+    std::vector<torch::Tensor> obs_list;
+
+    for (const std::string &observation: params_.observations) {
+        if (observation == "lin_vel") {
+            obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale);
+        } else if (observation == "ang_vel") {
+            // obs_list.push_back(obs.ang_vel * params_.ang_vel_scale); // TODO is QuatRotateInverse necessery?
+            obs_list.push_back(
+                quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale);
+        } else if (observation == "gravity_vec") {
+            obs_list.push_back(quatRotateInverse(obs_.base_quat, obs_.gravity_vec, params_.framework));
+        } else if (observation == "commands") {
+            obs_list.push_back(obs_.commands * params_.commands_scale);
+        } else if (observation == "dof_pos") {
+            obs_list.push_back((obs_.dof_pos - params_.default_dof_pos) * params_.dof_pos_scale);
+        } else if (observation == "dof_vel") {
+            obs_list.push_back(obs_.dof_vel * params_.dof_vel_scale);
+        } else if (observation == "actions") {
+            obs_list.push_back(obs_.actions);
+        }
+    }
+
+    const torch::Tensor obs = cat(obs_list, 1);
+    torch::Tensor clamped_obs = clamp(obs, -params_.clip_obs, params_.clip_obs);
+    return clamped_obs;
+}
+
+void StateRL::loadYaml(const std::string &config_path) {
+    YAML::Node config;
+    try {
+        config = YAML::LoadFile(config_path + "/config.yaml");
+    } catch ([[maybe_unused]] YAML::BadFile &e) {
+        RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "The file '%s' does not exist", config_path.c_str());
+        return;
+    }
+
+    params_.model_name = config["model_name"].as<std::string>();
+
+    params_.model_name = config["model_name"].as<std::string>();
+    params_.framework = config["framework"].as<std::string>();
+    const int rows = config["rows"].as<int>();
+    const int cols = config["cols"].as<int>();
+    params_.use_history = config["use_history"].as<bool>();
+    params_.decimation = config["decimation"].as<int>();
+    params_.num_observations = config["num_observations"].as<int>();
+    params_.observations = ReadVectorFromYaml<std::string>(config["observations"]);
+    params_.clip_obs = config["clip_obs"].as<double>();
+    if (config["clip_actions_lower"].IsNull() && config["clip_actions_upper"].IsNull()) {
+        params_.clip_actions_upper = torch::tensor({}).view({1, -1});
+        params_.clip_actions_lower = torch::tensor({}).view({1, -1});
+    } else {
+        params_.clip_actions_upper = torch::tensor(
+            ReadVectorFromYaml<double>(config["clip_actions_upper"], params_.framework, rows, cols)).view({1, -1});
+        params_.clip_actions_lower = torch::tensor(
+            ReadVectorFromYaml<double>(config["clip_actions_lower"], params_.framework, rows, cols)).view({1, -1});
+    }
+    params_.action_scale = config["action_scale"].as<double>();
+    params_.hip_scale_reduction = config["hip_scale_reduction"].as<double>();
+    params_.hip_scale_reduction_indices = ReadVectorFromYaml<int>(config["hip_scale_reduction_indices"]);
+    params_.num_of_dofs = config["num_of_dofs"].as<int>();
+    params_.lin_vel_scale = config["lin_vel_scale"].as<double>();
+    params_.ang_vel_scale = config["ang_vel_scale"].as<double>();
+    params_.dof_pos_scale = config["dof_pos_scale"].as<double>();
+    params_.dof_vel_scale = config["dof_vel_scale"].as<double>();
+    // params_.commands_scale = torch::tensor(ReadVectorFromYaml<double>(config["commands_scale"])).view({1, -1});
+    params_.commands_scale = torch::tensor({params_.lin_vel_scale, params_.lin_vel_scale, params_.ang_vel_scale});
+    params_.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"], params_.framework, rows, cols)).view({
+        1, -1
+    });
+    params_.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], params_.framework, rows, cols)).view({
+        1, -1
+    });
+    params_.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"], params_.framework, rows, cols)).
+            view({1, -1});
+    params_.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"], params_.framework, rows, cols)).
+            view({1, -1});
+    params_.torque_limits = torch::tensor(
+        ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1});
+    params_.default_dof_pos = torch::tensor(
+        ReadVectorFromYaml<double>(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1});
+}
+
+torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Tensor &v, const std::string &framework) {
+    torch::Tensor q_w;
+    torch::Tensor q_vec;
+    if (framework == "isaacsim") {
+        q_w = q.index({torch::indexing::Slice(), 0});
+        q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(1, 4)});
+    } else if (framework == "isaacgym") {
+        q_w = q.index({torch::indexing::Slice(), 3});
+        q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
+    }
+    const c10::IntArrayRef shape = q.sizes();
+
+    const torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1);
+    const torch::Tensor b = cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
+    const torch::Tensor c = q_vec * bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
+    return a - b + c;
+}
+
+torch::Tensor StateRL::forward() {
+    torch::autograd::GradMode::set_enabled(false);
+    torch::Tensor clamped_obs = computeObservation();
+    torch::Tensor actions;
+
+    if (params_.use_history) {
+        history_obs_buf_->insert(clamped_obs);
+        history_obs_ = history_obs_buf_->getObsVec({0, 1, 2, 3, 4, 5});
+        actions = model_.forward({history_obs_}).toTensor();
+    } else {
+        actions = model_.forward({clamped_obs}).toTensor();
+    }
+
+    if (params_.clip_actions_upper.numel() != 0 && params_.clip_actions_lower.numel() != 0) {
+        return clamp(actions, params_.clip_actions_lower, params_.clip_actions_upper);
+    }
+    return actions;
+}
+
+void StateRL::getState() {
+    if (params_.framework == "isaacgym") {
+        robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[0].get().get_value();
+        robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[1].get().get_value();
+        robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[2].get().get_value();
+        robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[3].get().get_value();
+    } else if (params_.framework == "isaacsim") {
+        robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[0].get().get_value();
+        robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[1].get().get_value();
+        robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[2].get().get_value();
+        robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[3].get().get_value();
+    }
+
+    robot_state_.imu.gyroscope[0] = ctrl_comp_.imu_state_interface_[4].get().get_value();
+    robot_state_.imu.gyroscope[1] = ctrl_comp_.imu_state_interface_[5].get().get_value();
+    robot_state_.imu.gyroscope[2] = ctrl_comp_.imu_state_interface_[6].get().get_value();
+
+    robot_state_.imu.accelerometer[0] = ctrl_comp_.imu_state_interface_[7].get().get_value();
+    robot_state_.imu.accelerometer[1] = ctrl_comp_.imu_state_interface_[8].get().get_value();
+    robot_state_.imu.accelerometer[2] = ctrl_comp_.imu_state_interface_[9].get().get_value();
+
+    for (int i = 0; i < 12; i++) {
+        robot_state_.motor_state.q[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
+        robot_state_.motor_state.dq[i] = ctrl_comp_.joint_velocity_state_interface_[i].get().get_value();
+        robot_state_.motor_state.tauEst[i] = ctrl_comp_.joint_effort_state_interface_[i].get().get_value();
+    }
+
+    control_.x = ctrl_comp_.control_inputs_.ly;
+    control_.y = -ctrl_comp_.control_inputs_.lx;
+    control_.yaw = -ctrl_comp_.control_inputs_.rx;
+
+    updated_ = true;
+}
+
+void StateRL::runModel() {
+    obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
+    obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}});
+    obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0);
+    obs_.dof_pos = torch::tensor(robot_state_.motor_state.q).narrow(0, 0, params_.num_of_dofs).unsqueeze(0);
+    obs_.dof_vel = torch::tensor(robot_state_.motor_state.dq).narrow(0, 0, params_.num_of_dofs).unsqueeze(0);
+
+    const torch::Tensor clamped_actions = forward();
+
+    for (const int i: params_.hip_scale_reduction_indices) {
+        clamped_actions[0][i] *= params_.hip_scale_reduction;
+    }
+
+    obs_.actions = clamped_actions;
+
+    const torch::Tensor actions_scaled = clamped_actions * params_.action_scale;
+    // torch::Tensor output_torques = params_.rl_kp * (actions_scaled + params_.default_dof_pos - obs_.dof_pos) - params_.rl_kd * obs_.dof_vel;
+    // output_torques = clamp(output_torques, -(params_.torque_limits), params_.torque_limits);
+
+    output_dof_pos_ = actions_scaled + params_.default_dof_pos;
+
+    for (int i = 0; i < params_.num_of_dofs; ++i) {
+        robot_command_.motor_command.q[i] = output_dof_pos_[0][i].item<double>();
+        robot_command_.motor_command.dq[i] = 0;
+        robot_command_.motor_command.kp[i] = params_.rl_kp[0][i].item<double>();
+        robot_command_.motor_command.kd[i] = params_.rl_kd[0][i].item<double>();
+        robot_command_.motor_command.tau[i] = 0;
+    }
+}
+
+void StateRL::setCommand() const {
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_position_command_interface_[i].get().set_value(robot_command_.motor_command.q[i]);
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(robot_command_.motor_command.dq[i]);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(robot_command_.motor_command.kp[i]);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(robot_command_.motor_command.kd[i]);
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(robot_command_.motor_command.tau[i]);
+    }
+}

controllers/rl_quadruped_controller/src/LeggedGymController.cpp

@@ -0,0 +1,194 @@
+//
+// Created by tlab-uav on 24-10-4.
+//
+
+#include "LeggedGymController.h"
+
+namespace rl_quadruped_controller {
+    using config_type = controller_interface::interface_configuration_type;
+
+    controller_interface::InterfaceConfiguration LeggedGymController::command_interface_configuration() const {
+        controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+        conf.names.reserve(joint_names_.size() * command_interface_types_.size());
+        for (const auto &joint_name: joint_names_) {
+            for (const auto &interface_type: command_interface_types_) {
+                if (!command_prefix_.empty()) {
+                    conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
+                } else {
+                    conf.names.push_back(joint_name + "/" += interface_type);
+                }
+            }
+        }
+
+        return conf;
+    }
+
+    controller_interface::InterfaceConfiguration LeggedGymController::state_interface_configuration() const {
+        controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+        conf.names.reserve(joint_names_.size() * state_interface_types_.size());
+        for (const auto &joint_name: joint_names_) {
+            for (const auto &interface_type: state_interface_types_) {
+                conf.names.push_back(joint_name + "/" += interface_type);
+            }
+        }
+
+        for (const auto &interface_type: imu_interface_types_) {
+            conf.names.push_back(imu_name_ + "/" += interface_type);
+        }
+
+        for (const auto &interface_type: foot_force_interface_types_) {
+            conf.names.push_back(foot_force_name_ + "/" += interface_type);
+        }
+
+        return conf;
+    }
+
+    controller_interface::return_type LeggedGymController::
+    update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        if (mode_ == FSMMode::NORMAL) {
+            current_state_->run();
+            next_state_name_ = current_state_->checkChange();
+            if (next_state_name_ != current_state_->state_name) {
+                mode_ = FSMMode::CHANGE;
+                next_state_ = getNextState(next_state_name_);
+                RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s",
+                            current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str());
+            }
+        } else if (mode_ == FSMMode::CHANGE) {
+            current_state_->exit();
+            current_state_ = next_state_;
+
+            current_state_->enter();
+            mode_ = FSMMode::NORMAL;
+        }
+
+        return controller_interface::return_type::OK;
+    }
+
+    controller_interface::CallbackReturn LeggedGymController::on_init() {
+        try {
+            joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
+            feet_names_ = auto_declare<std::vector<std::string> >("feet_names", feet_names_);
+            command_interface_types_ =
+                    auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
+            state_interface_types_ =
+                    auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
+
+            command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
+
+            // imu sensor
+            imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
+            imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+
+            // rl config folder
+            rl_config_folder_ = auto_declare<std::string>("config_folder", rl_config_folder_);
+
+            get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+            RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
+        } catch (const std::exception &e) {
+            fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+            return controller_interface::CallbackReturn::ERROR;
+        }
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn LeggedGymController::on_configure(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
+            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
+                // Handle message
+                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_comp_.control_inputs_.ry = msg->ry;
+            });
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn LeggedGymController::on_activate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        // clear out vectors in case of restart
+        ctrl_comp_.clear();
+
+        // assign command interfaces
+        for (auto &interface: command_interfaces_) {
+            std::string interface_name = interface.get_interface_name();
+            if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
+                command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
+            } else {
+                command_interface_map_[interface_name]->push_back(interface);
+            }
+        }
+
+        // assign state interfaces
+        for (auto &interface: state_interfaces_) {
+            if (interface.get_prefix_name() == imu_name_) {
+                ctrl_comp_.imu_state_interface_.emplace_back(interface);
+            } else if (interface.get_prefix_name() == foot_force_name_) {
+                ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
+            } else {
+                state_interface_map_[interface.get_interface_name()]->push_back(interface);
+            }
+        }
+
+        // Create FSM List
+        state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
+        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
+        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
+        state_list_.rl = std::make_shared<StateRL>(ctrl_comp_, rl_config_folder_);
+
+        // Initialize FSM
+        current_state_ = state_list_.passive;
+        current_state_->enter();
+        next_state_ = current_state_;
+        next_state_name_ = current_state_->state_name;
+        mode_ = FSMMode::NORMAL;
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn LeggedGymController::on_deactivate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        release_interfaces();
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn
+    LeggedGymController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
+        return ControllerInterface::on_cleanup(previous_state);
+    }
+
+    controller_interface::CallbackReturn
+    LeggedGymController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
+        return ControllerInterface::on_shutdown(previous_state);
+    }
+
+    controller_interface::CallbackReturn LeggedGymController::on_error(const rclcpp_lifecycle::State &previous_state) {
+        return ControllerInterface::on_error(previous_state);
+    }
+
+    std::shared_ptr<FSMState> LeggedGymController::getNextState(const FSMStateName stateName) const {
+        switch (stateName) {
+            case FSMStateName::INVALID:
+                return state_list_.invalid;
+            case FSMStateName::PASSIVE:
+                return state_list_.passive;
+            case FSMStateName::FIXEDDOWN:
+                return state_list_.fixedDown;
+            case FSMStateName::FIXEDSTAND:
+                return state_list_.fixedStand;
+            case FSMStateName::RL:
+                return state_list_.rl;
+            default:
+                return state_list_.invalid;
+        }
+    }
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(rl_quadruped_controller::LeggedGymController, controller_interface::ControllerInterface);

controllers/rl_quadruped_controller/src/LeggedGymController.h

@@ -0,0 +1,114 @@
+//
+// Created by tlab-uav on 24-10-4.
+//
+
+#ifndef LEGGEDGYMCONTROLLER_H
+#define LEGGEDGYMCONTROLLER_H
+#include <controller_interface/controller_interface.hpp>
+#include <rl_quadruped_controller/FSM/StateRL.h>
+
+#include "rl_quadruped_controller/FSM/StateFixedStand.h"
+#include "rl_quadruped_controller/FSM/StateFixedDown.h"
+#include "rl_quadruped_controller/FSM/StatePassive.h"
+#include "rl_quadruped_controller/control/CtrlComponent.h"
+
+namespace rl_quadruped_controller {
+    struct FSMStateList {
+        std::shared_ptr<FSMState> invalid;
+        std::shared_ptr<StatePassive> passive;
+        std::shared_ptr<StateFixedDown> fixedDown;
+        std::shared_ptr<StateFixedStand> fixedStand;
+        std::shared_ptr<StateRL> rl;
+    };
+
+    class LeggedGymController final : public controller_interface::ControllerInterface {
+    public:
+        CONTROLLER_INTERFACE_PUBLIC
+        LeggedGymController() = default;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::return_type update(
+            const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_init() override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_configure(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_activate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_deactivate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_cleanup(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_shutdown(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        CONTROLLER_INTERFACE_PUBLIC
+        controller_interface::CallbackReturn on_error(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+    private:
+        std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
+
+        CtrlComponent ctrl_comp_;
+        std::vector<std::string> joint_names_;
+        std::vector<std::string> feet_names_;
+        std::vector<std::string> command_interface_types_;
+        std::vector<std::string> state_interface_types_;
+
+        std::string command_prefix_;
+
+        // IMU Sensor
+        std::string imu_name_;
+        std::vector<std::string> imu_interface_types_;
+        // Foot Force Sensor
+        std::string foot_force_name_;
+        std::vector<std::string> foot_force_interface_types_;
+
+        std::string rl_config_folder_;
+
+        std::unordered_map<
+            std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
+        command_interface_map_ = {
+            {"effort", &ctrl_comp_.joint_torque_command_interface_},
+            {"position", &ctrl_comp_.joint_position_command_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
+            {"kp", &ctrl_comp_.joint_kp_command_interface_},
+            {"kd", &ctrl_comp_.joint_kd_command_interface_}
+        };
+
+        std::unordered_map<
+            std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
+        state_interface_map_ = {
+            {"position", &ctrl_comp_.joint_position_state_interface_},
+            {"effort", &ctrl_comp_.joint_effort_state_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_state_interface_}
+        };
+
+        rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
+
+        FSMMode mode_ = FSMMode::NORMAL;
+        std::string state_name_;
+        FSMStateName next_state_name_ = FSMStateName::INVALID;
+        FSMStateList state_list_;
+        std::shared_ptr<FSMState> current_state_;
+        std::shared_ptr<FSMState> next_state_;
+    };
+}
+#endif //LEGGEDGYMCONTROLLER_H

controllers/rl_quadruped_controller/src/common/ObservationBuffer.cpp

@@ -0,0 +1,51 @@
+//
+// Created by biao on 24-10-6.
+//
+
+#include "ObservationBuffer.h"
+
+ObservationBuffer::ObservationBuffer(int num_envs,
+                                     const int num_obs,
+                                     const int include_history_steps)
+    : num_envs_(num_envs),
+      num_obs_(num_obs),
+      include_history_steps_(include_history_steps) {
+    num_obs_total_ = num_obs * include_history_steps;
+    obs_buffer_ = torch::zeros({num_envs, num_obs_total_}, dtype(torch::kFloat32));
+}
+
+void ObservationBuffer::reset(const std::vector<int> &reset_index, const torch::Tensor &new_obs) {
+    std::vector<torch::indexing::TensorIndex> indices;
+    for (int index: reset_index) {
+        indices.emplace_back(torch::indexing::Slice(index));
+    }
+    obs_buffer_.index_put_(indices, new_obs.repeat({1, include_history_steps_}));
+}
+
+void ObservationBuffer::insert(const torch::Tensor &new_obs) {
+    // Shift observations back.
+    const torch::Tensor shifted_obs = obs_buffer_.index({
+        torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(num_obs_, num_obs_ * include_history_steps_)
+    }).clone();
+    obs_buffer_.index({
+        torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(0, num_obs_ * (include_history_steps_ - 1))
+    }) = shifted_obs;
+
+    // Add new observation.
+    obs_buffer_.index({
+        torch::indexing::Slice(torch::indexing::None), torch::indexing::Slice(-num_obs_, torch::indexing::None)
+    }) = new_obs;
+}
+
+torch::Tensor ObservationBuffer::getObsVec(const std::vector<int> &obs_ids) const {
+    std::vector<torch::Tensor> obs;
+    for (int i = obs_ids.size() - 1; i >= 0; --i) {
+        const int obs_id = obs_ids[i];
+        const int slice_idx = include_history_steps_ - obs_id - 1;
+        obs.push_back(obs_buffer_.index({
+            torch::indexing::Slice(torch::indexing::None),
+            torch::indexing::Slice(slice_idx * num_obs_, (slice_idx + 1) * num_obs_)
+        }));
+    }
+    return cat(obs, -1);
+}

controllers/rl_quadruped_controller/src/common/ObservationBuffer.h

@@ -0,0 +1,32 @@
+//
+// Created by biao on 24-10-6.
+//
+
+#ifndef OBSERVATIONBUFFER_H
+#define OBSERVATIONBUFFER_H
+
+#include <torch/torch.h>
+#include <vector>
+
+class ObservationBuffer {
+public:
+    ObservationBuffer(int num_envs, int num_obs, int include_history_steps);
+
+    ~ObservationBuffer() = default;
+
+    void reset(const std::vector<int>& reset_index, const torch::Tensor &new_obs);
+
+    void insert(const torch::Tensor &new_obs);
+
+    [[nodiscard]] torch::Tensor getObsVec(const std::vector<int> &obs_ids) const;
+
+private:
+    int num_envs_;
+    int num_obs_;
+    int include_history_steps_;
+    int num_obs_total_;
+    torch::Tensor obs_buffer_;
+};
+
+
+#endif //OBSERVATIONBUFFER_H

controllers/unitree_guide_controller/README.md

@@ -35,8 +35,4 @@ Required hardware interfaces:
 ```bash
 cd ~/ros2_ws
 colcon build --packages-up-to unitree_guide_controller
-```
-
-## 3. Run
-* [Go2 in mujoco simulation](../../descriptions/unitree/go2_description)
-* [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo)
+```

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h

@@ -7,7 +7,6 @@
 
 #include <string>
 #include <utility>
-#include <rclcpp/time.hpp>
 #include <unitree_guide_controller/common/enumClass.h>
 #include <unitree_guide_controller/control/CtrlComponent.h>
 

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h

@@ -22,10 +22,10 @@ public:
 private:
     void calcTorque();
 
-    Estimator &estimator_;
-    QuadrupedRobot &robot_model_;
-    BalanceCtrl &balance_ctrl_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<Estimator> &estimator_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
+    std::shared_ptr<BalanceCtrl> &balance_ctrl_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
 
     Vec3 pcd_, pcd_init_;
     RotMat Rd_;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h

@@ -5,6 +5,8 @@
 #ifndef STATEFIXEDDOWN_H
 #define STATEFIXEDDOWN_H
 
+#include <rclcpp/time.hpp>
+
 #include "FSMState.h"
 
 class StateFixedDown final : public FSMState {
@@ -18,6 +20,7 @@ public:
     void exit() override;
 
     FSMStateName checkChange() override;
+
 private:
     double target_pos_[12] = {
         0.0473455, 1.22187, -2.44375, -0.0473455,

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -5,6 +5,8 @@
 #ifndef STATEFIXEDSTAND_H
 #define STATEFIXEDSTAND_H
 
+#include <rclcpp/time.hpp>
+
 #include "FSMState.h"
 
 class StateFixedStand final : public FSMState {

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h

@@ -20,7 +20,7 @@ public:
     FSMStateName checkChange() override;
 
 private:
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
     void calc_body_target(float row, float pitch, float yaw, float height);
 
     float row_max_, row_min_;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h

@@ -25,7 +25,7 @@ private:
 
     void torqueCtrl() const;
 
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
     float _xMin, _xMax;
     float _yMin, _yMax;
     float _zMin, _zMax;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h

@@ -47,10 +47,11 @@ private:
      */
     bool checkStepOrNot();
 
-    Estimator &estimator_;
-    QuadrupedRobot &robot_model_;
-    BalanceCtrl &balance_ctrl_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<Estimator> &estimator_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
+    std::shared_ptr<BalanceCtrl> &balance_ctrl_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
+
     GaitGenerator gait_generator_;
 
     // Robot State

controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h

@@ -5,17 +5,17 @@
 #ifndef BALANCECTRL_H
 #define BALANCECTRL_H
 
+#include <memory>
+
 #include "unitree_guide_controller/common/mathTypes.h"
 class QuadrupedRobot;
 
 class BalanceCtrl {
 public:
-    explicit BalanceCtrl();
+    explicit BalanceCtrl(const std::shared_ptr<QuadrupedRobot>& robot);
 
     ~BalanceCtrl() = default;
 
-    void init(const QuadrupedRobot &robot);
-
     /**
      * Calculate the desired feet end force
      * @param ddPcd desired body acceleration

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

@@ -41,15 +41,13 @@ struct CtrlComponent {
     control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
 
-    QuadrupedRobot robot_model_;
-    Estimator estimator_;
-    BalanceCtrl balance_ctrl_;
+    std::shared_ptr<QuadrupedRobot> robot_model_;
+    std::shared_ptr<Estimator> estimator_;
 
-    WaveGenerator wave_generator_;
+    std::shared_ptr<BalanceCtrl> balance_ctrl_;
+    std::shared_ptr<WaveGenerator> wave_generator_;
 
-    CtrlComponent() : robot_model_(*this),
-                      estimator_(*this) {
-    }
+    CtrlComponent() = default;
 
     void clear() {
         joint_torque_command_interface_.clear();

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -63,7 +63,7 @@ public:
      * @return feet velocity in world frame
      */
     Vec34 getFeetVel() {
-        const std::vector<KDL::Vector> feet_vel = robot_model_.getFeet2BVelocities();
+        const std::vector<KDL::Vector> feet_vel = robot_model_->getFeet2BVelocities();
         Vec34 result;
         for (int i(0); i < 4; ++i) {
             result.col(i) = Vec3(feet_vel[i].data) + getVelocity();
@@ -106,8 +106,8 @@ public:
 
 private:
     CtrlComponent &ctrl_component_;
-    QuadrupedRobot &robot_model_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
 
     Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h

@@ -5,6 +5,7 @@
 
 #ifndef FOOTENDCTRL_H
 #define FOOTENDCTRL_H
+#include <memory>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 
@@ -24,14 +25,13 @@ public:
 
 private:
     CtrlComponent &ctrl_component_;
-    QuadrupedRobot &robot_model_;
-    Estimator &estimator_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
+    std::shared_ptr<Estimator> &estimator_;
 
     Vec4 feet_radius_, feet_init_angle_;
 
     double k_x_, k_y_, k_yaw_;
     double t_stance_{}, t_swing_{};
-
 };
 
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h

@@ -5,6 +5,7 @@
 
 #ifndef GAITGENERATOR_H
 #define GAITGENERATOR_H
+#include <memory>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 #include "FeetEndCalc.h"
@@ -66,8 +67,8 @@ private:
      */
     [[nodiscard]] double cycloidZVelocity(double height, double phase) const;
 
-    WaveGenerator &wave_generator_;
-    Estimator &estimator_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
+    std::shared_ptr<Estimator> &estimator_;
     FeetEndCalc feet_end_calc_;
 
     double gait_height_{};

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h

@@ -17,12 +17,10 @@ inline long long getSystemTime() {
 
 class WaveGenerator {
 public:
-    WaveGenerator();
+    WaveGenerator(double period, double st_ratio, const Vec4 &bias);
 
     ~WaveGenerator() = default;
 
-    void init(double period, double st_ratio, const Vec4 &bias);
-
     void update();
 
     [[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -16,13 +16,11 @@ struct CtrlComponent;
 
 class QuadrupedRobot {
 public:
-    explicit QuadrupedRobot(CtrlComponent &ctrl_component): ctrl_component_(ctrl_component) {
-    }
+    explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+                            const std::vector<std::string> &feet_names, const std::string &base_name);
 
     ~QuadrupedRobot() = default;
 
-    void init(const std::string &robot_description, const std::vector<std::string> &feet_names, const std::string& base_name);
-
     /**
      * Calculate the joint positions based on the foot end position
      * @param pEe_list vector of foot-end position

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -29,10 +29,10 @@ StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateN
 }
 
 void StateBalanceTest::enter() {
-    pcd_init_ = estimator_.getPosition();
+    pcd_init_ = estimator_->getPosition();
     pcd_ = pcd_init_;
-    init_rotation_ = estimator_.getRotation();
-    wave_generator_.status_ = WaveStatus::STANCE_ALL;
+    init_rotation_ = estimator_->getRotation();
+    wave_generator_->status_ = WaveStatus::STANCE_ALL;
 }
 
 void StateBalanceTest::run() {
@@ -52,7 +52,7 @@ void StateBalanceTest::run() {
 }
 
 void StateBalanceTest::exit() {
-    wave_generator_.status_ = WaveStatus::SWING_ALL;
+    wave_generator_->status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateBalanceTest::checkChange() {
@@ -67,28 +67,28 @@ FSMStateName StateBalanceTest::checkChange() {
 }
 
 void StateBalanceTest::calcTorque() {
-    const auto B2G_Rotation = estimator_.getRotation();
+    const auto B2G_Rotation = estimator_->getRotation();
     const RotMat G2B_Rotation = B2G_Rotation.transpose();
 
-    const Vec3 pose_body = estimator_.getPosition();
-    const Vec3 vel_body = estimator_.getVelocity();
+    const Vec3 pose_body = estimator_->getPosition();
+    const Vec3 vel_body = estimator_->getVelocity();
 
     // expected body acceleration
     dd_pcd_ = Kp_p_ * (pcd_ - pose_body) + Kd_p_ * (Vec3(0, 0, 0) - vel_body);
 
     // expected body angular acceleration
     d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
-             Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGyroGlobal());
+             Kd_w_ * (Vec3(0, 0, 0) - estimator_->getGyroGlobal());
 
     // calculate foot force
-    const Vec34 pos_feet_2_body_global = estimator_.getFeetPos2Body();
-    const Vec34 force_feet_global = -balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
-                                                        pos_feet_2_body_global, wave_generator_.contact_);
+    const Vec34 pos_feet_2_body_global = estimator_->getFeetPos2Body();
+    const Vec34 force_feet_global = -balance_ctrl_->calF(dd_pcd_, d_wbd_, B2G_Rotation,
+                                                        pos_feet_2_body_global, wave_generator_->contact_);
     const Vec34 force_feet_body = G2B_Rotation * force_feet_global;
 
-    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
+    std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        KDL::JntArray torque = robot_model_.getTorque(force_feet_body.col(i), i);
+        KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
             ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -24,8 +24,8 @@ void StateFreeStand::enter() {
         ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
     }
 
-    init_joint_pos_ = robot_model_.current_joint_pos_;
-    init_foot_pos_ = robot_model_.getFeet2BPositions();
+    init_joint_pos_ = robot_model_->current_joint_pos_;
+    init_foot_pos_ = robot_model_->getFeet2BPositions();
 
 
     fr_init_pos_ = init_foot_pos_[0];
@@ -69,7 +69,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
     for (int i = 0; i < 4; i++) {
         goal_pos[i] = body_2_fr_pos * init_foot_pos_[i];
     }
-    target_joint_pos_ = robot_model_.getQ(goal_pos);
+    target_joint_pos_ = robot_model_->getQ(goal_pos);
 
     for (int i = 0; i < 4; i++) {
         ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -7,7 +7,9 @@
 #include "unitree_guide_controller/common/mathTools.h"
 
 StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
-    FSMStateName::SWINGTEST, "swing test", ctrl_component), robot_model_(ctrl_component.robot_model_) {
+                                                                   FSMStateName::SWINGTEST, "swing test",
+                                                                   ctrl_component),
+                                                               robot_model_(ctrl_component.robot_model_) {
     _xMin = -0.15;
     _xMax = 0.10;
     _yMin = -0.15;
@@ -29,8 +31,8 @@ void StateSwingTest::enter() {
     Kp = KDL::Vector(20, 20, 50);
     Kd = KDL::Vector(5, 5, 20);
 
-    init_joint_pos_ = robot_model_.current_joint_pos_;
-    init_foot_pos_ = robot_model_.getFeet2BPositions();
+    init_joint_pos_ = robot_model_->current_joint_pos_;
+    init_foot_pos_ = robot_model_->getFeet2BPositions();
 
     target_foot_pos_ = init_foot_pos_;
     fr_init_pos_ = init_foot_pos_[0];
@@ -80,7 +82,7 @@ FSMStateName StateSwingTest::checkChange() {
 
 void StateSwingTest::positionCtrl() {
     target_foot_pos_[0] = fr_goal_pos_;
-    target_joint_pos_ = robot_model_.getQ(target_foot_pos_);
+    target_joint_pos_ = robot_model_->getQ(target_foot_pos_);
     for (int i = 0; i < 4; i++) {
         ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
         ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
@@ -89,14 +91,14 @@ void StateSwingTest::positionCtrl() {
 }
 
 void StateSwingTest::torqueCtrl() const {
-    const KDL::Frame fr_current_pos = robot_model_.getFeet2BPositions(0);
+    const KDL::Frame fr_current_pos = robot_model_->getFeet2BPositions(0);
 
     const KDL::Vector pos_goal = fr_goal_pos_.p;
     const KDL::Vector pos0 = fr_current_pos.p;
-    const KDL::Vector vel0 = robot_model_.getFeet2BVelocities(0);
+    const KDL::Vector vel0 = robot_model_->getFeet2BVelocities(0);
 
     const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
-    KDL::JntArray torque0 = robot_model_.getTorque(force0, 0);
+    KDL::JntArray torque0 = robot_model_->getTorque(force0, 0);
 
     for (int i = 0; i < 3; i++) {
         ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque0(i));

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -27,10 +27,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
 }
 
 void StateTrotting::enter() {
-    pcd_ = estimator_.getPosition();
-    pcd_(2) = -estimator_.getFeetPos2Body()(2, 0);
+    pcd_ = estimator_->getPosition();
+    pcd_(2) = -estimator_->getFeetPos2Body()(2, 0);
     v_cmd_body_.setZero();
-    yaw_cmd_ = estimator_.getYaw();
+    yaw_cmd_ = estimator_->getYaw();
     Rd = rotz(yaw_cmd_);
     w_cmd_global_.setZero();
 
@@ -39,10 +39,10 @@ void StateTrotting::enter() {
 }
 
 void StateTrotting::run() {
-    pos_body_ = estimator_.getPosition();
-    vel_body_ = estimator_.getVelocity();
+    pos_body_ = estimator_->getPosition();
+    vel_body_ = estimator_->getVelocity();
 
-    B2G_RotMat = estimator_.getRotation();
+    B2G_RotMat = estimator_->getRotation();
     G2B_RotMat = B2G_RotMat.transpose();
 
     getUserCmd();
@@ -55,16 +55,16 @@ void StateTrotting::run() {
     calcQQd();
 
     if (checkStepOrNot()) {
-        wave_generator_.status_ = WaveStatus::WAVE_ALL;
+        wave_generator_->status_ = WaveStatus::WAVE_ALL;
     } else {
-        wave_generator_.status_ = WaveStatus::STANCE_ALL;
+        wave_generator_->status_ = WaveStatus::STANCE_ALL;
     }
 
     calcGain();
 }
 
 void StateTrotting::exit() {
-    wave_generator_.status_ = WaveStatus::SWING_ALL;
+    wave_generator_->status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateTrotting::checkChange() {
@@ -118,7 +118,7 @@ void StateTrotting::calcTau() {
 
     Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
     Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) +
-                 Kd_w_ * (w_cmd_global_ - estimator_.getGyroGlobal());
+                 Kd_w_ * (w_cmd_global_ - estimator_->getGyroGlobal());
 
     dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
     dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
@@ -128,16 +128,16 @@ void StateTrotting::calcTau() {
     d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
     d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
 
-    const Vec34 pos_feet_body_global = estimator_.getFeetPos2Body();
+    const Vec34 pos_feet_body_global = estimator_->getFeetPos2Body();
     Vec34 force_feet_global =
-            -balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_.contact_);
+            -balance_ctrl_->calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_->contact_);
 
 
-    Vec34 pos_feet_global = estimator_.getFeetPos();
-    Vec34 vel_feet_global = estimator_.getFeetVel();
+    Vec34 pos_feet_global = estimator_->getFeetPos();
+    Vec34 vel_feet_global = estimator_->getFeetVel();
 
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_(i) == 0) {
+        if (wave_generator_->contact_(i) == 0) {
             force_feet_global.col(i) =
                 Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
                 Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
@@ -146,9 +146,9 @@ void StateTrotting::calcTau() {
 
     Vec34 force_feet_body_ = G2B_RotMat * force_feet_global;
 
-    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
+    std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        KDL::JntArray torque = robot_model_.getTorque(force_feet_body_.col(i), i);
+        KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
         }
@@ -156,7 +156,7 @@ void StateTrotting::calcTau() {
 }
 
 void StateTrotting::calcQQd() {
-    const std::vector<KDL::Frame> pos_feet_body = robot_model_.getFeet2BPositions();
+    const std::vector<KDL::Frame> pos_feet_body = robot_model_->getFeet2BPositions();
 
     Vec34 pos_feet_target, vel_feet_target;
     for (int i(0); i < 4; ++i) {
@@ -164,8 +164,8 @@ void StateTrotting::calcQQd() {
         vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
     }
 
-    Vec12 q_goal = robot_model_.getQ(pos_feet_target);
-    Vec12 qd_goal = robot_model_.getQd(pos_feet_body, vel_feet_target);
+    Vec12 q_goal = robot_model_->getQ(pos_feet_target);
+    Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target);
     for (int i = 0; i < 12; i++) {
         ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
         ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
@@ -174,7 +174,7 @@ void StateTrotting::calcQQd() {
 
 void StateTrotting::calcGain() const {
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_(i) == 0) {
+        if (wave_generator_->contact_(i) == 0) {
             // swing gain
             for (int j = 0; j < 3; j++) {
                 ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -44,7 +44,7 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::return_type UnitreeGuideController::
-    update(const rclcpp::Time &time, const rclcpp::Duration &period) {
+    update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
         // auto now = std::chrono::steady_clock::now();
         // std::chrono::duration<double> time_diff = now - last_update_time_;
         // last_update_time_ = now;
@@ -53,9 +53,13 @@ namespace unitree_guide_controller {
         // update_frequency_ = 1.0 / time_diff.count();
         // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
 
-        ctrl_comp_.robot_model_.update();
-        ctrl_comp_.wave_generator_.update();
-        ctrl_comp_.estimator_.update();
+        if (ctrl_comp_.robot_model_ == nullptr) {
+            return controller_interface::return_type::OK;
+        }
+
+        ctrl_comp_.robot_model_->update();
+        ctrl_comp_.wave_generator_->update();
+        ctrl_comp_.estimator_->update();
 
         if (mode_ == FSMMode::NORMAL) {
             current_state_->run();
@@ -92,6 +96,11 @@ namespace unitree_guide_controller {
             command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
             feet_names_ =
                     auto_declare<std::vector<std::string> >("feet_names", feet_names_);
+
+            get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+            RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
+
+            ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -101,7 +110,7 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_configure(
-        const rclcpp_lifecycle::State &previous_state) {
+        const rclcpp_lifecycle::State & /*previous_state*/) {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
             "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
                 // Handle message
@@ -115,20 +124,18 @@ namespace unitree_guide_controller {
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
             "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_.init(msg->data, feet_names_, base_name_);
-                ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
+                ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
+                    ctrl_comp_, msg->data, feet_names_, base_name_);
+                ctrl_comp_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_comp_.robot_model_);
             });
 
-        get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
-        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
-
-        ctrl_comp_.wave_generator_.init(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
+        ctrl_comp_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
 
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
         // clear out vectors in case of restart
         ctrl_comp_.clear();
 
@@ -171,27 +178,27 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
-        const rclcpp_lifecycle::State &previous_state) {
+        const rclcpp_lifecycle::State & /*previous_state*/) {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_error(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_error(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
-    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(FSMStateName stateName) const {
+    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const {
         switch (stateName) {
             case FSMStateName::INVALID:
                 return state_list_.invalid;

controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp

@@ -9,18 +9,16 @@
 
 #include "quadProgpp/QuadProg++.hh"
 
-BalanceCtrl::BalanceCtrl() {
-    mass_ = 15;
+BalanceCtrl::BalanceCtrl(const std::shared_ptr<QuadrupedRobot> &robot) {
+    mass_ = robot->mass_;
+
     alpha_ = 0.001;
     beta_ = 0.1;
     g_ << 0, 0, -9.81;
     friction_ratio_ = 0.4;
     friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
             friction_ratio_, 0, 0, 1;
-}
 
-void BalanceCtrl::init(const QuadrupedRobot &robot) {
-    mass_ = robot.mass_;
     pcb_ = Vec3(0.0, 0.0, 0.0);
     Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
 

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -12,7 +12,9 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
                                                       robot_model_(ctrl_component.robot_model_),
                                                       wave_generator_(ctrl_component.wave_generator_) {
     g_ << 0, 0, -9.81;
-    dt_ = 0.002;
+    dt_ = 1.0 / ctrl_component_.frequency_;
+
+    std::cout<<"dt: "<<dt_<<std::endl;
     large_variance_ = 100;
     for (int i(0); i < Qdig.rows(); ++i) {
         Qdig(i) = i < 6 ? 0.0003 : 0.01;
@@ -146,25 +148,25 @@ double Estimator::getYaw() const {
 }
 
 void Estimator::update() {
-    if (robot_model_.mass_ == 0) return;
+    if (robot_model_->mass_ == 0) return;
 
     Q = QInit_;
     R = RInit_;
 
-    foot_poses_ = robot_model_.getFeet2BPositions();
-    foot_vels_ = robot_model_.getFeet2BVelocities();
+    foot_poses_ = robot_model_->getFeet2BPositions();
+    foot_vels_ = robot_model_->getFeet2BVelocities();
     feet_h_.setZero();
 
     // Adjust the covariance based on foot contact and phase.
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_[i] == 0) {
+        if (wave_generator_->contact_[i] == 0) {
             // foot not contact
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
             R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
             R(24 + i, 24 + i) = large_variance_;
         } else {
             // foot contact
-            const double trust = windowFunc(wave_generator_.phase_[i], 0.2);
+            const double trust = windowFunc(wave_generator_->phase_[i], 0.2);
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
                     (1 + (1 - trust) * large_variance_) *
                     QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);

controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp

@@ -15,10 +15,10 @@ FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_c
 }
 
 void FeetEndCalc::init() {
-    t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
-    t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
+    t_stance_ = ctrl_component_.wave_generator_->get_t_stance();
+    t_swing_ = ctrl_component_.wave_generator_->get_t_swing();
 
-    Vec34 feet_pos_body = estimator_.getFeetPos2Body();
+    Vec34 feet_pos_body = estimator_->getFeetPos2Body();
     // Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
     for (int i(0); i < 4; ++i) {
         feet_radius_(i) =
@@ -28,7 +28,7 @@ void FeetEndCalc::init() {
 }
 
 Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) {
-    Vec3 body_vel_global = estimator_.getVelocity();
+    Vec3 body_vel_global = estimator_->getVelocity();
     Vec3 next_step;
 
     next_step(0) = body_vel_global(0) * (1 - phase) * t_swing_ +
@@ -39,8 +39,8 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
                    k_y_ * (body_vel_global(1) - vxy_goal_global(1));
     next_step(2) = 0;
 
-    const double yaw = estimator_.getYaw();
-    const double d_yaw = estimator_.getDYaw();
+    const double yaw = estimator_->getYaw();
+    const double d_yaw = estimator_->getDYaw();
     const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 +
                             k_yaw_ * (d_yaw_global - d_yaw);
 
@@ -49,7 +49,7 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
     next_step(1) +=
             feet_radius_(index) * sin(yaw + feet_init_angle_(index) + next_yaw);
 
-    Vec3 foot_pos = estimator_.getPosition() + next_step;
+    Vec3 foot_pos = estimator_->getPosition() + next_step;
     foot_pos(2) = 0.0;
 
     return foot_pos;

controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp

@@ -22,21 +22,21 @@ void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const
 
 void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
     if (first_run_) {
-        start_p_ = estimator_.getFeetPos();
+        start_p_ = estimator_->getFeetPos();
         first_run_ = false;
     }
 
     for (int i = 0; i < 4; i++) {
-        if (wave_generator_.contact_(i) == 1) {
-            if (wave_generator_.phase_(i) < 0.5) {
+        if (wave_generator_->contact_(i) == 1) {
+            if (wave_generator_->phase_(i) < 0.5) {
                 // foot contact the ground
-                start_p_.col(i) = estimator_.getFootPos(i);
+                start_p_.col(i) = estimator_->getFootPos(i);
             }
             feet_pos.col(i) = start_p_.col(i);
             feet_vel.col(i).setZero();
         } else {
             // foot not contact, swing
-            end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i));
+            end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_->phase_(i));
             feet_pos.col(i) = getFootPos(i);
             feet_vel.col(i) = getFootVel(i);
         }
@@ -54,10 +54,10 @@ Vec3 GaitGenerator::getFootPos(const int i) {
     Vec3 foot_pos;
 
     foot_pos(0) =
-            cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+            cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
     foot_pos(1) =
-            cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
-    foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_.phase_(i));
+            cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
+    foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_->phase_(i));
 
     return foot_pos;
 }
@@ -66,10 +66,10 @@ Vec3 GaitGenerator::getFootVel(const int i) {
     Vec3 foot_vel;
 
     foot_vel(0) =
-            cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+            cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
     foot_vel(1) =
-            cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
-    foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_.phase_(i));
+            cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
+    foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_->phase_(i));
 
     return foot_vel;
 }
@@ -86,10 +86,10 @@ double GaitGenerator::cycloidZPosition(const double startZ, const double height,
 
 double GaitGenerator::cycloidXYVelocity(const double startXY, const double endXY, const double phase) const {
     const double phase_pi = 2 * M_PI * phase;
-    return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_.get_t_swing();
+    return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_->get_t_swing();
 }
 
 double GaitGenerator::cycloidZVelocity(const double height, const double phase) const {
     const double phase_pi = 2 * M_PI * phase;
-    return height * M_PI * sin(phase_pi) / wave_generator_.get_t_swing();
+    return height * M_PI * sin(phase_pi) / wave_generator_->get_t_swing();
 }

controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp

@@ -8,14 +8,13 @@
 
 #include <iostream>
 
-WaveGenerator::WaveGenerator() {
+WaveGenerator::WaveGenerator(double period, double st_ratio, const Vec4 &bias) {
+
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();
     status_past_ = WaveStatus::SWING_ALL;
     status_ = WaveStatus::SWING_ALL;
-}
 
-void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
     period_ = period;
     st_ratio_ = st_ratio;
     bias_ = bias;

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -6,8 +6,9 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names,
-                          const std::string& base_name) {
+QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+                               const std::vector<std::string> &feet_names,
+                               const std::string &base_name) : ctrl_component_(ctrl_component) {
     KDL::Tree robot_tree;
     kdl_parser::treeFromString(robot_description, robot_tree);
 

descriptions/README.md

@@ -1,21 +1,24 @@
 # Robot Descriptions
 
-This folder contains the URDF and SRDF files for the quadruped robot. 
+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/)
+    * [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/)
-
+    * [Cyberdog](xiaomi/cyberdog_description/)
+* Deep Robotics
+    * [Lite 3](deep_robotics/lite3_description/)
 
 ## 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.
+* 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
@@ -23,4 +26,22 @@ This folder contains the URDF and SRDF files for the quadruped robot.
 * use mujoco to convert the urdf file to mujoco model.
   ```
   compile robot.urdf robot.xml
-  ```
+  ```
+
+## Dependencies for Gazebo Simulation
+
+Gazebo Simulation only tested on ROS2 Jazzy. I didn't 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
+    ```

descriptions/deep_robotics/lite3_description/CMakeLists.txt

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

descriptions/deep_robotics/lite3_description/README.md

@@ -0,0 +1,52 @@
+# 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 lite3_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch a1_description ocs2_control.launch.py
+  ```
+* Legged Gym Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch a1_description rl_control.launch.py
+  ```
+
+
+### Gazebo Simulator
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch lite3_description gazebo_unitree_guide.launch.py
+  ```
+* Legged Gym Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch lite3_description gazebo_rl_control.launch.py
+  ```

descriptions/deep_robotics/lite3_description/config/gazebo.yaml

@@ -0,0 +1,143 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 2000  # Hz
+
+    # Define the available controllers
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    leg_pd_controller:
+      type: leg_pd_controller/LegPdController
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+    rl_quadruped_controller:
+      type: rl_quadruped_controller/LeggedGymController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_link"
+
+leg_pd_controller:
+  ros__parameters:
+    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
+
+    command_interfaces:
+      - effort
+
+    state_interfaces:
+      - position
+      - velocity
+
+unitree_guide_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    command_prefix: "leg_pd_controller"
+    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
+
+    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
+
+rl_quadruped_controller:
+  ros__parameters:
+    update_rate: 200  # Hz
+    config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym"
+    command_prefix: "leg_pd_controller"
+    joints:
+      - FL_hip_joint
+      - FL_thigh_joint
+      - FL_calf_joint
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    imu_name: "imu_sensor"
+    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

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
+   }
+}

descriptions/deep_robotics/lite3_description/config/ocs2/reference.info

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

descriptions/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              /tmp/ocs2_quadruped_controller/a1
+}
+
+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.33     ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+   (12,0) -0.20   ; FL_hip_joint
+   (13,0)  0.72   ; FL_thigh_joint
+   (14,0) -1.44   ; FL_calf_joint
+   (15,0) -0.20   ; RL_hip_joint
+   (16,0)  0.72   ; RL_thigh_joint
+   (17,0) -1.44   ; RL_calf_joint
+   (18,0)  0.20   ; FR_hip_joint
+   (19,0)  0.72   ; FR_thigh_joint
+   (20,0) -1.44   ; FR_calf_joint
+   (21,0)  0.20   ; RR_hip_joint
+   (22,0)  0.72   ; RR_thigh_joint
+   (23,0) -1.44   ; RR_calf_joint
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   100.0    ; vcom_z
+  (3,3)   10.0     ; L_x / robotMass
+  (4,4)   30.0     ; L_y / robotMass
+  (5,5)   30.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   1000.0   ; p_base_x
+  (7,7)   1000.0   ; p_base_y
+  (8,8)   1500.0   ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 300.0    ; theta_base_y
+  (11,11) 300.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+  (12,12) 5.0     ; FL_hip_joint
+  (13,13) 5.0     ; FL_thigh_joint
+  (14,14) 2.5     ; FL_calf_joint
+  (15,15) 5.0     ; RL_hip_joint
+  (16,16) 5.0     ; RL_thigh_joint
+  (17,17) 2.5     ; RL_calf_joint
+  (18,18) 5.0     ; FR_hip_joint
+  (19,19) 5.0     ; FR_thigh_joint
+  (20,20) 2.5     ; FR_calf_joint
+  (21,21) 5.0     ; RR_hip_joint
+  (22,22) 5.0     ; RR_thigh_joint
+  (23,23) 2.5     ; RR_calf_joint
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, RL, RR] ;;
+  (0,0)   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, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.3
+
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "FL_calf, FR_calf"
+    [1] "RL_calf, RR_calf"
+    [2] "FL_calf, RL_calf"
+    [3] "FR_calf, RR_calf"
+    [4] "FL_foot, FR_foot"
+    [5] "RL_foot, RR_foot"
+    [6] "FL_foot, RL_foot"
+    [7] "FR_foot, RR_foot"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  33.5     ; HAA
+   (1,0)  33.5     ; HFE
+   (2,0)  33.5     ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}

descriptions/deep_robotics/lite3_description/config/robot_control.yaml

@@ -0,0 +1,76 @@
+# Controller Manager configuration
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    # Define the available controllers
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+    ocs2_quadruped_controller:
+      type: ocs2_quadruped_controller/Ocs2QuadrupedController
+
+    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: 500  # 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
+
+    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

descriptions/deep_robotics/lite3_description/config/visualize_urdf.rviz

@@ -1,15 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /Status1
-        - /Grid1
         - /RobotModel1
-      Splitter Ratio: 0.5558823347091675
-    Tree Height: 462
+      Splitter Ratio: 0.5
+    Tree Height: 295
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -59,188 +58,99 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: robot_description
+        Value: /robot_description
       Enabled: true
       Links:
-        AI_camera_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         All Links Enabled: true
-        D435_camera_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
-        FL_abad:
+        FL_FOOT:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FL_abad_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        FL_foot:
+        FL_HIP:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FL_hip:
+        FL_SHANK:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FL_hip_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        FL_hip_shoulder:
+        FL_THIGH:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FL_knee:
+        FR_FOOT:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FL_knee_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        FR_abad:
+        FR_HIP:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FR_abad_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        FR_foot:
+        FR_SHANK:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FR_hip:
+        FR_THIGH:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FR_hip_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        FR_hip_shoulder:
+        HL_FOOT:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FR_knee:
+        HL_HIP:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        FR_knee_rotor:
+        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
-        RGB_camera_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RL_abad:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RL_abad_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        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
-        RL_hip_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RL_knee:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RL_knee_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        RR_abad:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RR_abad_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        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
-        RR_hip_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RR_knee:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        RR_knee_rotor:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        body:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        head:
+        TORSO:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -250,10 +160,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        lidar_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
@@ -265,7 +171,7 @@ Visualization Manager:
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_link
+    Fixed Frame: TORSO
     Frame Rate: 30
   Name: root
   Tools:
@@ -308,33 +214,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.1026314496994019
+      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
-        Y: 0
-        Z: 0
+        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.33039766550064087
+      Pitch: 0.42539793252944946
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.6804034113883972
+      Yaw: 0.6953961849212646
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 659
-  Hide Left Dock: true
+    collapsed: false
+  Height: 691
+  Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000025cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003f0000025c000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000025cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000025c000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003620000003efc0100000002fb0000000800540069006d00650100000000000003620000026f00fffffffb0000000800540069006d0065010000000000000450000000000000000000000362000001f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000001f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001f1000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bb0000005efc0100000002fb0000000800540069006d00650100000000000003bb0000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000025f000001f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -343,6 +249,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 866
-  X: 618
-  Y: 136
+  Width: 955
+  X: 773
+  Y: 168

descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py

@@ -5,30 +5,32 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import PathJoinSubstitution
 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 = "quadruped_gazebo"
+package_description = "lite3_description"
 
 
-def process_xacro(context):
-    robot_type_value = context.launch_configurations['robot_type']
+def process_xacro():
     pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'urdf', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
-    return (robot_description_config.toxml(), robot_type_value)
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
+    return robot_description_config.toxml()
 
 
-def launch_setup(context, *args, **kwargs):
-    (robot_description, robot_type) = process_xacro(context)
+def generate_launch_description():
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    robot_description = process_xacro()
+
     gz_spawn_entity = Node(
         package='ros_gz_sim',
         executable='create',
         output='screen',
         arguments=['-topic', 'robot_description', '-name',
-                   robot_type, '-allow_renaming', 'true', '-z', '0.5'],
+                   'lite3', '-allow_renaming', 'true', '-z', '0.4'],
     )
 
     robot_state_publisher = Node(
@@ -72,42 +74,14 @@ def launch_setup(context, *args, **kwargs):
         arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
     )
 
-    return [
-        robot_state_publisher,
-        gz_spawn_entity,
-        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=[leg_pd_controller],
-            )
-        ),
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                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,
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        ),
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
@@ -120,12 +94,13 @@ def generate_launch_description():
                                        'launch',
                                        'gz_sim.launch.py'])]),
             launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        OpaqueFunction(function=launch_setup),
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        )
+        robot_state_publisher,
+        gz_spawn_entity,
+        leg_pd_controller,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=leg_pd_controller,
+                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
+            )
+        ),
     ])

descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py

@@ -0,0 +1,93 @@
+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 = "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()
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        output="both",
+    )
+
+    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
+            }
+        ],
+    )
+
+    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 LaunchDescription([
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        ),
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster, unitree_guide_controller],
+            )
+        ),
+    ])

descriptions/deep_robotics/lite3_description/launch/visualize.launch.py

@@ -3,23 +3,30 @@ import os
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch_ros.actions import Node
 
 import xacro
 
-package_description = "quadruped_gazebo"
+package_description = "lite3_description"
 
-def process_xacro(context):
-    robot_type_value = context.launch_configurations['robot_type']
+def process_xacro():
     pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'urdf', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    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()
 
-def launch_setup(context, *args, **kwargs):
-    robot_description = process_xacro(context)
-    return [
+    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',
@@ -39,27 +46,4 @@ def launch_setup(context, *args, **kwargs):
             name='joint_state_publisher',
             output='screen',
         )
-    ]
-
-
-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]
-        )
     ])

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>

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>

descriptions/deep_robotics/lite3_description/xacro/body.xacro

@@ -0,0 +1,436 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find lite3_description)/xacro/imu_link.xacro"/>
+
+  <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>
+  </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://$(find 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 lower="-0.42" upper="0.42" effort="24" 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://$(find 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 lower="-2.67" upper="0.314" effort="24" 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://$(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="FL_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="FL_THIGH"/>
+    <child link="FL_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="0.6" upper="2.72" effort="36" 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://$(find 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 lower="-0.42" upper="0.42" effort="24" 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://$(find 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 lower="-2.67" upper="0.314" effort="24" 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://$(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="FR_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="FR_THIGH"/>
+    <child link="FR_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="0.6" upper="2.72" effort="36" 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://$(find 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 lower="-0.42" upper="0.42" effort="24" 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://$(find 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 lower="-2.67" upper="0.314" effort="24" 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://$(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="HL_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="HL_THIGH"/>
+    <child link="HL_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="0.6" upper="2.72" effort="36" 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://$(find 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 lower="-0.42" upper="0.42" effort="24" 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://$(find 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 lower="-2.67" upper="0.314" effort="24" 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://$(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="HR_Knee" type="revolute">
+    <origin xyz="0 0 -0.20"/>
+    <parent link="HR_THIGH"/>
+    <child link="HR_SHANK"/>
+    <axis xyz="0 -1 0"/>
+    <limit lower="0.6" upper="2.72" effort="36" 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>
+
+
+</robot>

descriptions/deep_robotics/lite3_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 lite3_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>

descriptions/deep_robotics/lite3_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>

descriptions/deep_robotics/lite3_description/xacro/robot.xacro

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<robot name="lite3" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find lite3_description)/xacro/body.xacro"/>
+  <xacro:include filename="$(find lite3_description)/xacro/transmission.xacro"/>
+
+  <xacro:arg name="GAZEBO" default="false"/>
+  <xacro:if value="$(arg GAZEBO)">
+    <xacro:include filename="$(find lite3_description)/xacro/gazebo.xacro"/>
+  </xacro:if>
+  <xacro:unless value="$(arg GAZEBO)">
+    <xacro:include filename="$(find lite3_description)/xacro/ros2_control.xacro"/>
+  </xacro:unless>
+
+</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>

descriptions/deep_robotics/lite3_description/xacro/transmission.xacro

@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- 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>
+
+</robot>

descriptions/quadruped_gazebo/LICENSE

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

descriptions/quadruped_gazebo/README.md

@@ -1,37 +0,0 @@
-# Quadruped Gazebo
-
-This package contains the description of the quadruped robot from Unitree Robotics, and the launch file to load them into the gazebo empty world.
-
-Tested environment:
-* Ubuntu 24.04
-  * ROS2 Jazzy
-  * Gazebo Harmonic
-
-## Build
-```bash
-cd ~/ros2_ws
-colcon build --packages-up-to quadruped_gazebo
-```
-
-## 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 quadruped_gazebo visualize.launch.py
-```
-The default model is a1. To use another robot model, add the following argument:
-```bash
-source ~/ros2_ws/install/setup.bash
-ros2 launch quadruped_gazebo visualize.launch.py robot_type:=go1
-```
-
-## Launch the robot in gazebo
-```bash
-source ~/ros2_ws/install/setup.bash
-ros2 launch quadruped_gazebo gazebo.launch.py
-```
-To use another robot model, add the following argument:
-```bash
-source ~/ros2_ws/install/setup.bash
-ros2 launch quadruped_gazebo gazebo.launch.py robot_type:=go1
-```

descriptions/quadruped_gazebo/package.xml

@@ -1,19 +0,0 @@
-<?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>quadruped_gazebo</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>imu_sensor_broadcaster</exec_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

descriptions/quadruped_gazebo/urdf/a1/const.xacro

@@ -1,105 +0,0 @@
-<?xml version="1.0"?>
-
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="a1_description">
-
-    <!-- Constants for robot dimensions -->
-    <xacro:property name="stick_mass" value="0.00001"/>
-
-    <!-- simplified collision value -->
-    <xacro:property name="trunk_width" value="0.194"/>
-    <xacro:property name="trunk_length" value="0.267"/>
-    <xacro:property name="trunk_height" value="0.114"/>
-    <xacro:property name="hip_radius" value="0.046"/>
-    <xacro:property name="hip_length" value="0.04"/>
-    <xacro:property name="thigh_shoulder_radius" value="0.041"/>
-    <xacro:property name="thigh_shoulder_length" value="0.04"/>
-    <xacro:property name="thigh_shoulder_y_offset" value="-0.008"/>
-
-    <xacro:property name="thigh_width" value="0.03"/>
-    <xacro:property name="thigh_height" value="0.034"/>
-    <xacro:property name="thigh_x_offset" value="-0.015"/>
-    <xacro:property name="calf_width" value="0.016"/>
-    <xacro:property name="calf_height" value="0.016"/>
-    <xacro:property name="calf_x_offset" value="0.0"/>
-    <xacro:property name="foot_radius" value="0.02"/>
-
-    <!-- kinematic value -->
-    <xacro:property name="thigh_offset" value="0.0838"/>
-    <xacro:property name="thigh_length" value="0.2"/>
-    <xacro:property name="calf_length" value="0.2"/>
-
-    <!-- leg offset from trunk center value -->
-    <xacro:property name="leg_offset_x" value="0.1805"/>
-    <xacro:property name="leg_offset_y" value="0.047"/>
-    <xacro:property name="trunk_offset_z" value="0.01675"/>
-    <xacro:property name="hip_offset" value="0.065"/>
-
-    <!-- joint limits -->
-    <xacro:property name="damping" value="0"/>
-    <xacro:property name="friction" value="0"/>
-    <xacro:property name="hip_position_max" value="${46*pi/180.0}"/>
-    <xacro:property name="hip_position_min" value="${-46*pi/180.0}"/>
-    <xacro:property name="hip_velocity_max" value="21"/>
-    <xacro:property name="hip_torque_max" value="33.5"/>
-    <xacro:property name="thigh_position_max" value="${240*pi/180.0}"/>
-    <xacro:property name="thigh_position_min" value="${-60*pi/180.0}"/>
-    <xacro:property name="thigh_velocity_max" value="21"/>
-    <xacro:property name="thigh_torque_max" value="33.5"/>
-    <xacro:property name="calf_position_max" value="${-52.5*pi/180.0}"/>
-    <xacro:property name="calf_position_min" value="${-154.5*pi/180.0}"/>
-    <xacro:property name="calf_velocity_max" value="21"/>
-    <xacro:property name="calf_torque_max" value="33.5"/>
-
-    <!-- dynamics inertial value -->
-    <!-- trunk -->
-    <xacro:property name="trunk_mass" value="6.0"/>
-    <xacro:property name="trunk_com_x" value="0.0000"/>
-    <xacro:property name="trunk_com_y" value="0.0041"/>
-    <xacro:property name="trunk_com_z" value="-0.0005"/>
-    <xacro:property name="trunk_ixx" value="0.0158533"/>
-    <xacro:property name="trunk_ixy" value="-0.0000366"/>
-    <xacro:property name="trunk_ixz" value="-0.0000611"/>
-    <xacro:property name="trunk_iyy" value="0.0377999"/>
-    <xacro:property name="trunk_iyz" value="-0.0000275"/>
-    <xacro:property name="trunk_izz" value="0.0456542"/>
-
-    <!-- hip (left front) -->
-    <xacro:property name="hip_mass" value="0.595"/>
-    <xacro:property name="hip_com_x" value="-0.003875"/>
-    <xacro:property name="hip_com_y" value="0.001622"/>
-    <xacro:property name="hip_com_z" value="0.000042"/>
-    <xacro:property name="hip_ixx" value="0.000402747"/>
-    <xacro:property name="hip_ixy" value="-0.000008709"/>
-    <xacro:property name="hip_ixz" value="-0.000000297"/>
-    <xacro:property name="hip_iyy" value="0.000691123"/>
-    <xacro:property name="hip_iyz" value="-0.000000545"/>
-    <xacro:property name="hip_izz" value="0.000487919"/>
-
-    <!-- thigh -->
-    <xacro:property name="thigh_mass" value="0.888"/>
-    <xacro:property name="thigh_com_x" value="-0.003574"/>
-    <xacro:property name="thigh_com_y" value="-0.019529"/>
-    <xacro:property name="thigh_com_z" value="-0.030323"/>
-    <xacro:property name="thigh_ixx" value="0.005251806"/>
-    <xacro:property name="thigh_ixy" value="-0.000002168"/>
-    <xacro:property name="thigh_ixz" value="0.000346889"/>
-    <xacro:property name="thigh_iyy" value="0.005000475"/>
-    <xacro:property name="thigh_iyz" value="-0.000028174"/>
-    <xacro:property name="thigh_izz" value="0.001110200"/>
-
-    <!-- calf -->
-    <xacro:property name="calf_mass" value="0.151"/>
-    <xacro:property name="calf_com_x" value="0.007105"/>
-    <xacro:property name="calf_com_y" value="-0.000239"/>
-    <xacro:property name="calf_com_z" value="-0.096933"/>
-    <xacro:property name="calf_ixx" value="0.002344758"/>
-    <xacro:property name="calf_ixy" value="0.0"/>
-    <xacro:property name="calf_ixz" value="-0.000141275"/>
-    <xacro:property name="calf_iyy" value="0.002360755"/>
-    <xacro:property name="calf_iyz" value="0.0"/>
-    <xacro:property name="calf_izz" value="0.000031158"/>
-
-    <!-- foot -->
-    <xacro:property name="foot_mass" value="0.06"/>
-
-</robot>

descriptions/quadruped_gazebo/urdf/aliengo/const.xacro

@@ -1,105 +0,0 @@
-<?xml version="1.0"?>
-
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="aliengo_description">
-
-    <!-- Constants for robot dimensions -->
-    <xacro:property name="stick_mass" value="0.00001"/>
-
-    <!-- simplified collision value -->
-    <xacro:property name="trunk_width" value="0.150"/>
-    <xacro:property name="trunk_length" value="0.647"/>
-    <xacro:property name="trunk_height" value="0.112"/>
-    <xacro:property name="hip_radius" value="0.046"/>
-    <xacro:property name="hip_length" value="0.0418"/>
-    <xacro:property name="thigh_shoulder_radius" value="0.044"/>
-    <xacro:property name="thigh_shoulder_length" value="0.03"/>
-    <xacro:property name="thigh_shoulder_y_offset" value="0.0"/>
-
-    <xacro:property name="thigh_width" value="0.0374"/>
-    <xacro:property name="thigh_height" value="0.043"/>
-    <xacro:property name="thigh_x_offset" value="-0.015"/>
-    <xacro:property name="calf_width" value="0.04"/>
-    <xacro:property name="calf_height" value="0.03"/>
-    <xacro:property name="calf_x_offset" value="0.008"/>
-    <xacro:property name="foot_radius" value="0.0265"/>
-
-    <!-- kinematic value -->
-    <xacro:property name="thigh_offset" value="0.0868"/>
-    <xacro:property name="thigh_length" value="0.25"/>
-    <xacro:property name="calf_length" value="0.25"/>
-
-    <!-- leg offset from trunk center value -->
-    <xacro:property name="leg_offset_x" value="0.2407"/>
-    <xacro:property name="leg_offset_y" value="0.051"/>
-    <xacro:property name="trunk_offset_z" value="0.01675"/>
-    <xacro:property name="hip_offset" value="0.083"/>
-
-    <!-- joint limits -->
-    <xacro:property name="damping" value="0.05"/>
-    <xacro:property name="friction" value="0.01"/>
-    <xacro:property name="hip_position_max" value="${70*pi/180.0}"/>
-    <xacro:property name="hip_position_min" value="${-70*pi/180.0}"/>
-    <xacro:property name="hip_velocity_max" value="20"/>
-    <xacro:property name="hip_torque_max" value="35.278"/>
-    <xacro:property name="thigh_position_max" value="1e22"/>
-    <xacro:property name="thigh_position_min" value="-1e22"/>
-    <xacro:property name="thigh_velocity_max" value="20"/>
-    <xacro:property name="thigh_torque_max" value="35.278"/>
-    <xacro:property name="calf_position_max" value="${-37*pi/180.0}"/>
-    <xacro:property name="calf_position_min" value="${-159*pi/180.0}"/>
-    <xacro:property name="calf_velocity_max" value="15.89"/>
-    <xacro:property name="calf_torque_max" value="44.4"/>
-
-    <!-- dynamics inertial value    total 22.0kg-->
-    <!-- trunk -->
-    <xacro:property name="trunk_mass" value="11.041"/>
-    <xacro:property name="trunk_com_x" value="0.008465"/>
-    <xacro:property name="trunk_com_y" value="0.004045"/>
-    <xacro:property name="trunk_com_z" value="-0.000763"/>
-    <xacro:property name="trunk_ixx" value="0.050874"/>
-    <xacro:property name="trunk_ixy" value="-0.000451628"/>
-    <xacro:property name="trunk_ixz" value="0.000487603"/>
-    <xacro:property name="trunk_iyy" value="0.64036"/>
-    <xacro:property name="trunk_iyz" value="0.000048356"/>
-    <xacro:property name="trunk_izz" value="0.65655"/>
-
-    <!-- hip (left front) -->
-    <xacro:property name="hip_mass" value="1.993"/>
-    <xacro:property name="hip_com_x" value="-0.022191"/>
-    <xacro:property name="hip_com_y" value="0.015144"/>
-    <xacro:property name="hip_com_z" value="-0.000015"/>
-    <xacro:property name="hip_ixx" value="0.002903894"/>
-    <xacro:property name="hip_ixy" value="-0.000071850"/>
-    <xacro:property name="hip_ixz" value="-0.000001262"/>
-    <xacro:property name="hip_iyy" value="0.004907517"/>
-    <xacro:property name="hip_iyz" value="-0.00000175"/>
-    <xacro:property name="hip_izz" value="0.005586944"/>
-
-    <!-- thigh -->
-    <xacro:property name="thigh_mass" value="0.639"/>
-    <xacro:property name="thigh_com_x" value="-0.005607"/>
-    <xacro:property name="thigh_com_y" value="-0.003877"/>
-    <xacro:property name="thigh_com_z" value="-0.048199"/>
-    <xacro:property name="thigh_ixx" value="0.005666803"/>
-    <xacro:property name="thigh_ixy" value="0.000003597"/>
-    <xacro:property name="thigh_ixz" value="0.000491446"/>
-    <xacro:property name="thigh_iyy" value="0.005847229"/>
-    <xacro:property name="thigh_iyz" value="0.000010086"/>
-    <xacro:property name="thigh_izz" value="0.000369811"/>
-
-    <!-- calf -->
-    <xacro:property name="calf_mass" value="0.207"/>
-    <xacro:property name="calf_com_x" value="0.002781"/>
-    <xacro:property name="calf_com_y" value="0.000063"/>
-    <xacro:property name="calf_com_z" value="-0.142518"/>
-    <xacro:property name="calf_ixx" value="0.006341369"/>
-    <xacro:property name="calf_ixy" value="-0.000000003"/>
-    <xacro:property name="calf_ixz" value="-0.000087951"/>
-    <xacro:property name="calf_iyy" value="0.006355157"/>
-    <xacro:property name="calf_iyz" value="-0.000001336"/>
-    <xacro:property name="calf_izz" value="0.000039188"/>
-
-    <!-- foot -->
-    <xacro:property name="foot_mass" value="0.06"/>
-
-</robot>