Merge pull request #1 from legubiao/gazebo

Gazebo

README.md

@@ -1,5 +1,13 @@
-# Quadruped ROS2
+# Quadruped ROS2 Control
 
+This repository contains the ros2-control based controllers for the quadruped robot. 
+* [Controllers](controllers): contains the ros2-control controllers 
+* [Commands](commands): contains command node used to send command to the controller
+* [Descriptions](descriptions): contains the urdf model of the robot
+* [Hardwares](hardwares): contains the ros2-control hardware interface for the robot
+
+
+## 1. Build
 * rosdep
 ```bash
 cd ~/ros2_ws

controllers/leg_pd_controller/CMakeLists.txt

@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.8)
+project(leg_pd_controller)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set(CONTROLLER_INCLUDE_DEPENDS
+        pluginlib
+        rcpputils
+        controller_interface
+        realtime_tools
+)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
+    find_package(${Dependency} REQUIRED)
+endforeach ()
+
+add_library(${PROJECT_NAME} SHARED
+        src/LegPdController.cpp
+)
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+ament_target_dependencies(
+        ${PROJECT_NAME} PUBLIC
+        ${CONTROLLER_INCLUDE_DEPENDS}
+)
+
+pluginlib_export_plugin_description_file(controller_interface leg_pd_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(${CONTROLLER_INCLUDE_DEPENDS})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+if (BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    # the following line skips the linter which checks for copyrights
+    # comment the line when a copyright and license is added to all source files
+    set(ament_cmake_copyright_FOUND TRUE)
+    # the following line skips cpplint (only works in a git repo)
+    # comment the line when this package is in a git repo and when
+    # a copyright and license is added to all source files
+    set(ament_cmake_cpplint_FOUND TRUE)
+    ament_lint_auto_find_test_dependencies()
+endif ()
+
+ament_package()

controllers/leg_pd_controller/LICENSE

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

controllers/leg_pd_controller/README.md

@@ -0,0 +1,28 @@
+# Leg PD Controller
+
+This package contains a simple PD controller for the leg joints of the quadruped robot. By using this controller, other ros2-control based on position control can also work on the hardware interface which only contain effort control (for example, gazebo ros2 control).
+
+## 1. Interfaces
+
+Provided interfaces:
+* joint position
+* joint velocity
+* joint effort
+* KP
+* KD
+
+Required hardware interfaces:
+* command:
+  * joint effort
+* state:
+  * joint position
+  * joint velocity
+
+## 2. Build
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to leg_pd_controller
+```
+
+## 3. Run
+* [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo)

controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h

@@ -0,0 +1,68 @@
+//
+// Created by tlab-uav on 24-9-19.
+//
+
+#ifndef LEGPDCONTROLLER_H
+#define LEGPDCONTROLLER_H
+#include <controller_interface/chainable_controller_interface.hpp>
+#include <controller_interface/controller_interface.hpp>
+
+
+namespace leg_pd_controller {
+    class LegPdController final : public controller_interface::ChainableControllerInterface {
+    public:
+        controller_interface::CallbackReturn on_init() override;
+
+        controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+        controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+        controller_interface::CallbackReturn on_configure(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        controller_interface::CallbackReturn on_activate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        controller_interface::CallbackReturn on_deactivate(
+            const rclcpp_lifecycle::State &previous_state) override;
+
+        bool on_set_chained_mode(bool chained_mode) override;
+
+        controller_interface::return_type update_and_write_commands(
+            const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+    protected:
+        std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+        controller_interface::return_type update_reference_from_subscribers(
+            const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+        std::vector<double> joint_effort_command_;
+        std::vector<double> joint_position_command_;
+        std::vector<double> joint_velocities_command_;
+        std::vector<double> joint_kp_command_;
+        std::vector<double> joint_kd_command_;
+
+        std::vector<std::string> joint_names_;
+
+        std::vector<std::string> state_interface_types_;
+        std::vector<std::string> reference_interface_types_;
+
+        std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+        joint_effort_command_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::unordered_map<
+            std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
+        state_interface_map_ = {
+            {"position", &joint_position_state_interface_},
+            {"velocity", &joint_velocity_state_interface_}
+        };
+    };
+}
+
+
+#endif //LEGPDCONTROLLER_H

controllers/leg_pd_controller/leg_pd_controller.xml

@@ -0,0 +1,9 @@
+<library path="leg_pd_controller">
+    <class name="leg_pd_controller/LegPdController"
+           type="leg_pd_controller::LegPdController"
+           base_class_type="controller_interface::ChainableControllerInterface">
+        <description>
+            PD controller for simulation which only support effort command input
+        </description>
+    </class>
+</library>

controllers/leg_pd_controller/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>leg_pd_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_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/leg_pd_controller/src/LegPdController.cpp

@@ -0,0 +1,148 @@
+//
+// Created by tlab-uav on 24-9-19.
+//
+
+#include "leg_pd_controller/LegPdController.h"
+
+namespace leg_pd_controller {
+    using config_type = controller_interface::interface_configuration_type;
+
+    controller_interface::CallbackReturn LegPdController::on_init() {
+        try {
+            joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
+            reference_interface_types_ =
+                    auto_declare<std::vector<std::string> >("reference_interfaces", reference_interface_types_);
+            state_interface_types_ = auto_declare<std::vector<
+                std::string> >("state_interfaces", state_interface_types_);
+        } catch (const std::exception &e) {
+            fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+            return controller_interface::CallbackReturn::ERROR;
+        }
+
+        const size_t joint_num = joint_names_.size();
+        joint_effort_command_.assign(joint_num, 0);
+        joint_position_command_.assign(joint_num, 0);
+        joint_velocities_command_.assign(joint_num, 0);
+        joint_kp_command_.assign(joint_num, 0);
+        joint_kd_command_.assign(joint_num, 0);
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const {
+        controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+        conf.names.reserve(joint_names_.size());
+        for (const auto &joint_name: joint_names_) {
+            conf.names.push_back(joint_name + "/effort");
+        }
+
+        return conf;
+    }
+
+    controller_interface::InterfaceConfiguration LegPdController::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);
+            }
+        }
+        return conf;
+    }
+
+    controller_interface::CallbackReturn LegPdController::on_configure(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn LegPdController::on_activate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+
+        joint_effort_command_interface_.clear();
+        joint_position_state_interface_.clear();
+        joint_velocity_state_interface_.clear();
+
+        // assign effort command interface
+        for (auto &interface: command_interfaces_) {
+            joint_effort_command_interface_.emplace_back(interface);
+        }
+
+        // assign state interfaces
+        for (auto &interface: state_interfaces_) {
+            state_interface_map_[interface.get_interface_name()]->push_back(interface);
+        }
+
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn LegPdController::on_deactivate(
+        const rclcpp_lifecycle::State & /*previous_state*/) {
+        release_interfaces();
+        return CallbackReturn::SUCCESS;
+    }
+
+    bool LegPdController::on_set_chained_mode(bool /*chained_mode*/) {
+        return true;
+    }
+
+    controller_interface::return_type LegPdController::update_and_write_commands(
+        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        if (joint_names_.size() != joint_effort_command_.size() ||
+            joint_names_.size() != joint_kp_command_.size() ||
+            joint_names_.size() != joint_position_command_.size() ||
+            joint_names_.size() != joint_position_state_interface_.size() ||
+            joint_names_.size() != joint_velocity_state_interface_.size() ||
+            joint_names_.size() != joint_effort_command_interface_.size()) {
+
+            std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
+            std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
+            std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
+            std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl;
+            std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << std::endl;
+            std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << std::endl;
+            std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << std::endl;
+
+            throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
+        }
+
+        for (size_t i = 0; i < joint_names_.size(); ++i) {
+            // PD Controller
+            const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
+                                      joint_position_command_[i] - joint_position_state_interface_[i].get().get_value())
+                                  +
+                                  joint_kd_command_[i] * (
+                                      joint_velocities_command_[i] - joint_velocity_state_interface_[i].get().
+                                      get_value());
+            joint_effort_command_interface_[i].get().set_value(torque);
+        }
+
+        return controller_interface::return_type::OK;
+    }
+
+    std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces() {
+        std::vector<hardware_interface::CommandInterface> reference_interfaces;
+
+        int ind = 0;
+        std::string controller_name = get_node()->get_name();
+        for (const auto &joint_name: joint_names_) {
+            reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
+            reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
+                                              &joint_velocities_command_[ind]);
+            reference_interfaces.emplace_back(controller_name, joint_name + "/effort", &joint_effort_command_[ind]);
+            reference_interfaces.emplace_back(controller_name, joint_name + "/kp", &joint_kp_command_[ind]);
+            reference_interfaces.emplace_back(controller_name, joint_name + "/kd", &joint_kd_command_[ind]);
+            ind++;
+        }
+
+        return reference_interfaces;
+    }
+
+    controller_interface::return_type LegPdController::update_reference_from_subscribers(
+        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        return controller_interface::return_type::OK;
+    }
+}
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(leg_pd_controller::LegPdController, controller_interface::ChainableControllerInterface);

controllers/unitree_guide_controller/CMakeLists.txt

@@ -77,11 +77,6 @@ install(
         RUNTIME DESTINATION bin
 )
 
-install(
-        DIRECTORY launch config
-        DESTINATION share/${PROJECT_NAME}/
-)
-
 ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 

controllers/unitree_guide_controller/README.md

@@ -1,13 +1,34 @@
 # Unitree Guide Controller
 
-This is a ros2-control controller based on unitree guide. The original unitree guide project could be found [here](https://github.com/unitreerobotics/unitree_guide)
+This is a ros2-control controller based on unitree guide. The original unitree guide project could be
+found [here](https://github.com/unitreerobotics/unitree_guide). I used KDL for the kinematic and dynamic calculation, so
+the controller performance has difference with the original one (sometimes very unstable).
 
+## 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
+
+## 2. Build
 ```bash
 cd ~/ros2_ws
 colcon build --packages-up-to unitree_guide_controller
 ```
 
-```bash
+## 3. Run
-source ~/ros2_ws/install/setup.bash
+* [Go2 in mujoco simulation](../../descriptions/go2_description)
-ros2 launch unitree_guide_controller controller.launch.py
+* [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo)
-```

controllers/unitree_guide_controller/config/controller.yaml

@@ -1,28 +0,0 @@
-unitree_guide_controller:
-  ros__parameters:
-    type: unitree_guide_controller/UnitreeGuideController
-    joints:
-      - FR_hip_joint
-      - FR_thigh_joint
-      - FR_calf_joint
-      - FL_hip_joint
-      - FL_thigh_joint
-      - FL_calf_joint
-      - RR_hip_joint
-      - RR_thigh_joint
-      - RR_calf_joint
-      - RL_hip_joint
-      - RL_thigh_joint
-      - RL_calf_joint
-
-    command_interfaces:
-      - effort
-      - position
-      - velocity
-      - kp
-      - kd
-
-    state_interfaces:
-      - effort
-      - position
-      - velocity

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -81,7 +81,9 @@ namespace unitree_guide_controller {
         std::vector<std::string> state_interface_types_;
 
         std::string imu_name_;
+        std::string command_prefix_;
         std::vector<std::string> imu_interface_types_;
+        std::vector<std::string> feet_names_;
 
         rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
         rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
@@ -89,7 +91,7 @@ namespace unitree_guide_controller {
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
         command_interface_map_ = {
-            {"effort", &ctrl_comp_.joint_effort_command_interface_},
+            {"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_},

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

@@ -17,7 +17,7 @@
 
 struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_effort_command_interface_;
+    joint_torque_command_interface_;
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
     joint_position_command_interface_;
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
@@ -54,7 +54,7 @@ struct CtrlComponent {
     }
 
     void clear() {
-        joint_effort_command_interface_.clear();
+        joint_torque_command_interface_.clear();
         joint_position_command_interface_.clear();
         joint_velocity_command_interface_.clear();
         joint_kd_command_interface_.clear();

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

@@ -21,7 +21,7 @@ public:
 
     ~QuadrupedRobot() = default;
 
-    void init(const std::string &robot_description);
+    void init(const std::string &robot_description, const std::vector<std::string> &feet_names);
 
     /**
      * Calculate the joint positions based on the foot end position

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h

@@ -10,7 +10,6 @@
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
 #include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 class RobotLeg {

controllers/unitree_guide_controller/launch/controller.launch.py

@@ -1,43 +0,0 @@
-# Copyright 2023 ros2_control Development Team
-#
-# 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.
-
-from launch import LaunchDescription
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
-
-
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare("unitree_guide_controller"),
-            "config",
-            "controller.yaml",
-        ]
-    )
-
-    robot_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--param-file", robot_controllers],
-    )
-
-    nodes = [
-        robot_controller_spawner,
-    ]
-
-    return LaunchDescription(nodes)

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -89,7 +89,7 @@ void StateBalanceTest::calcTorque() {
     for (int i = 0; i < 4; i++) {
         KDL::JntArray torque = robot_model_.getTorque(force_feet_body.col(i), i);
         for (int j = 0; j < 3; j++) {
-            ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(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/StateFixedDown.cpp

@@ -25,9 +25,9 @@ void StateFixedDown::run() {
         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_effort_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(50.0);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
+        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5);
     }
 }
 

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -25,7 +25,7 @@ void StateFixedStand::run() {
         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_effort_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);

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -20,7 +20,7 @@ StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStat
 
 void StateFreeStand::enter() {
     for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
+        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(100);
         ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
     }
 

controllers/unitree_guide_controller/src/FSM/StatePassive.cpp

@@ -12,7 +12,7 @@ StatePassive::StatePassive(CtrlComponent &ctrlComp) : FSMState(
 }
 
 void StatePassive::enter() {
-    for (auto i: ctrl_comp_.joint_effort_command_interface_) {
+    for (auto i: ctrl_comp_.joint_torque_command_interface_) {
         i.get().set_value(0);
     }
     for (auto i: ctrl_comp_.joint_position_command_interface_) {

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -99,6 +99,6 @@ void StateSwingTest::torqueCtrl() const {
     KDL::JntArray torque0 = robot_model_.getTorque(force0, 0);
 
     for (int i = 0; i < 3; i++) {
-        ctrl_comp_.joint_effort_command_interface_[i].get().set_value(torque0(i));
+        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque0(i));
     }
 }

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -64,7 +64,7 @@ void StateTrotting::run() {
 }
 
 void StateTrotting::exit() {
-    wave_generator_.status_ = WaveStatus::SWING_ALL;
+    wave_generator_.status_ = WaveStatus::STANCE_ALL;
 }
 
 FSMStateName StateTrotting::checkChange() {
@@ -150,7 +150,7 @@ void StateTrotting::calcTau() {
     for (int i = 0; i < 4; i++) {
         KDL::JntArray torque = robot_model_.getTorque(force_feet_body_.col(i), i);
         for (int j = 0; j < 3; j++) {
-            ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
+            ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
         }
     }
 }

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -15,7 +15,11 @@ namespace unitree_guide_controller {
         conf.names.reserve(joint_names_.size() * command_interface_types_.size());
         for (const auto &joint_name: joint_names_) {
             for (const auto &interface_type: command_interface_types_) {
-                conf.names.push_back(joint_name + "/" += interface_type);
+                if (!command_prefix_.empty()) {
+                    conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
+                } else {
+                    conf.names.push_back(joint_name + "/" += interface_type);
+                }
             }
         }
 
@@ -84,6 +88,9 @@ namespace unitree_guide_controller {
             // 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_);
+            command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
+            feet_names_ =
+                    auto_declare<std::vector<std::string> >("feet_names", feet_names_);
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -105,9 +112,9 @@ namespace unitree_guide_controller {
             });
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
-            "~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
+            "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_.init(msg->data);
+                ctrl_comp_.robot_model_.init(msg->data, feet_names_);
                 ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
             });
 
@@ -126,7 +133,12 @@ namespace unitree_guide_controller {
 
         // assign command interfaces
         for (auto &interface: command_interfaces_) {
-            command_interface_map_[interface.get_interface_name()]->push_back(interface);
+            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
@@ -138,6 +150,7 @@ namespace unitree_guide_controller {
             }
         }
 
+        // 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_);

controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp

@@ -11,8 +11,8 @@
 WaveGenerator::WaveGenerator() {
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();
-    status_past_ = WaveStatus::SWING_ALL;
+    status_past_ = WaveStatus::STANCE_ALL;
-    status_ = WaveStatus::SWING_ALL;
+    status_ = WaveStatus::STANCE_ALL;
 }
 
 void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -6,14 +6,15 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-void QuadrupedRobot::init(const std::string &robot_description) {
+void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names) {
     KDL::Tree robot_tree;
     kdl_parser::treeFromString(robot_description, robot_tree);
 
-    robot_tree.getChain("base", "FR_foot", fr_chain_);
+    robot_tree.getChain("base", feet_names[0], fr_chain_);
-    robot_tree.getChain("base", "FL_foot", fl_chain_);
+    robot_tree.getChain("base", feet_names[1], fl_chain_);
-    robot_tree.getChain("base", "RR_foot", rr_chain_);
+    robot_tree.getChain("base", feet_names[2], rr_chain_);
-    robot_tree.getChain("base", "RL_foot", rl_chain_);
+    robot_tree.getChain("base", feet_names[3], rl_chain_);
+
 
     robot_legs_.resize(4);
     robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
@@ -70,6 +71,7 @@ std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
     result.resize(4);
     for (int i = 0; i < 4; i++) {
         result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
+        result[i].M = KDL::Rotation::Identity();
     }
     return result;
 }

controllers/unitree_guide_controller/src/robot/RobotLeg.cpp

@@ -18,7 +18,6 @@ RobotLeg::RobotLeg(const KDL::Chain &chain) {
 KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
     KDL::Frame pEe;
     fk_pose_solver_->JntToCart(joint_positions, pEe);
-    pEe.M = KDL::Rotation::Identity();
     return pEe;
 }
 

descriptions/go1_description/config/robot_control.yaml

@@ -47,6 +47,12 @@ unitree_guide_controller:
       - position
       - velocity
 
+    feet_names:
+      - FR_foot
+      - FL_foot
+      - RR_foot
+      - RL_foot
+
     imu_name: "imu_sensor"
 
     imu_interfaces:

descriptions/go2_description/config/robot_control.yaml

@@ -47,6 +47,12 @@ unitree_guide_controller:
       - position
       - velocity
 
+    feet_names:
+      - FR_foot
+      - FL_foot
+      - RR_foot
+      - RL_foot
+
     imu_name: "imu_sensor"
 
     imu_interfaces:

descriptions/go2_description/launch/hardware.launch.py

@@ -48,9 +48,6 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_controllers],
-        remappings=[
-            ("~/robot_description", "/robot_description"),
-        ],
         output="both",
     )
 

descriptions/quadruped_gazebo/config/jtc.yaml

@@ -1,17 +0,0 @@
-# Controller Manager configuration
-controller_manager:
-  ros__parameters:
-    update_rate: 1000  # Hz
-    use_sim_time: true  # If running in simulation
-
-    # Define the available controllers
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-    imu_sensor_broadcaster:
-      type: imu_sensor_broadcaster/IMUSensorBroadcaster
-
-imu_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: "imu_sensor"
-    frame_id: "imu_sensor"

descriptions/quadruped_gazebo/config/robot_control.yaml

@@ -0,0 +1,94 @@
+# 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
+
+    leg_pd_controller:
+      type: leg_pd_controller/LegPdController
+
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_sensor"
+
+leg_pd_controller:
+  ros__parameters:
+    joints:
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+
+    command_interfaces:
+      - effort
+
+    state_interfaces:
+      - position
+      - velocity
+
+unitree_guide_controller:
+  ros__parameters:
+    command_prefix: "leg_pd_controller"
+    joints:
+      - RF_HAA
+      - RF_HFE
+      - RF_KFE
+      - LF_HAA
+      - LF_HFE
+      - LF_KFE
+      - RH_HAA
+      - RH_HFE
+      - RH_KFE
+      - LH_HAA
+      - LH_HFE
+      - LH_KFE
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
+
+    feet_names:
+      - RF_FOOT
+      - LF_FOOT
+      - RH_FOOT
+      - LH_FOOT
+
+    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/quadruped_gazebo/config/visualize_urdf.rviz

@@ -10,7 +10,7 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Description Topic1
       Splitter Ratio: 0.5
-    Tree Height: 543
+    Tree Height: 301
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -235,10 +235,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1061
+  Height: 555
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000031bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000031b0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000031bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000031b0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065a0000005efc0100000002fb0000000800540069006d006501000000000000065a0000047a00fffffffb0000000800540069006d006501000000000000045000000000000000000000065a0000031b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000031bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000031b000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000031bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000031b000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002e50000005efc0100000002fb0000000800540069006d00650100000000000002e50000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e50000016900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -247,6 +247,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1626
+  Width: 741
-  X: 787
+  X: 773
-  Y: 356
+  Y: 267

descriptions/quadruped_gazebo/launch/gazebo.launch.py

@@ -3,9 +3,10 @@ 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
+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_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -29,33 +30,70 @@ def launch_setup(context, *args, **kwargs):
         arguments=['-topic', 'robot_description', '-name',
                    robot_type, '-allow_renaming', 'true', '-z', '0.5'],
     )
+
+    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"],
+    )
+
+    leg_pd_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["leg_pd_controller",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
+
     return [
-        Node(
+        robot_state_publisher,
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            name='robot_state_publisher',
-            parameters=[
-                {
-                    'use_sim_time': True,
-                    'publish_frequency': 100.0,
-                    'use_tf_static': True,
-                    'robot_description': robot_description
-                }
-            ],
-        ),
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["joint_state_broadcaster",
-                       "--controller-manager", "/controller_manager"],
-        ),
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["imu_sensor_broadcaster",
-                       "--controller-manager", "/controller_manager"],
-        ),
         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],
+            )
+        ),
     ]
 
 

descriptions/quadruped_gazebo/urdf/common/leg.xacro

@@ -8,7 +8,7 @@
 
         <joint name="${prefix}_HAA" type="revolute">
             <xacro:insert_block name="origin"/>
-            <parent link="base"/>
+            <parent link="trunk"/>
             <child link="${prefix}_hip"/>
             <axis xyz="1 0 0"/>
             <dynamics damping="${damping}" friction="${friction}"/>
@@ -40,7 +40,7 @@
                     <mesh filename="${mesh_path}/hip.dae"
                           scale="1 1 1"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="${pi/2.0} 0 0" xyz="0 0 0"/>
@@ -81,7 +81,7 @@
                               scale="1 1 1"/>
                     </xacro:if>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 ${pi/2.0+0.1} 0" xyz="${thigh_x_offset} 0 ${-thigh_length/2.0}"/>
@@ -121,7 +121,7 @@
                     <mesh filename="${mesh_path}/calf.dae"
                           scale="1 1 1"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 ${pi/2.0} 0" xyz="${calf_x_offset} 0 ${-calf_length/2.0}"/>
@@ -151,7 +151,7 @@
                 <geometry>
                     <sphere radius="${foot_radius-0.01}"/>
                 </geometry>
-                <material name="orange"/>
+<!--                <material name="orange"/>-->
             </visual>
             <collision>
                 <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -178,26 +178,53 @@
         <gazebo reference="${prefix}_hip">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
-            <material>Gazebo/DarkGrey</material>
+            <visual>
+                <material>
+                    <ambient>.5 .5 .5  1.0</ambient>
+                    <diffuse>.5 .5 .5  1.0</diffuse>
+                    <specular>.5 .5 .5  1.0</specular>
+                </material>
+            </visual>
         </gazebo>
         <gazebo reference="${prefix}_thigh">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
             <self_collide>0</self_collide>
-            <material>Gazebo/DarkGrey</material>
+            <visual>
+                <material>
+                    <ambient>.175 .175 .175  1.0</ambient>
+                    <diffuse>.175 .175 .175  1.0</diffuse>
+                    <specular>.175 .175 .175 1.0</specular>
+                </material>
+            </visual>
             <kp value="1000000.0"/>
             <kd value="100.0"/>
         </gazebo>
+
         <gazebo reference="${prefix}_calf">
             <mu1>0.6</mu1>
             <mu2>0.6</mu2>
             <self_collide>1</self_collide>
+            <visual>
+                <material>
+                    <ambient>.5 .5 .5  1.0</ambient>
+                    <diffuse>.5 .5 .5  1.0</diffuse>
+                    <specular>.5 .5 .5  1.0</specular>
+                </material>
+            </visual>
         </gazebo>
+
         <gazebo reference="${prefix}_FOOT">
             <mu1>0.6</mu1>
             <mu2>0.6</mu2>
             <self_collide>1</self_collide>
-            <material>Gazebo/DarkGrey</material>
+            <visual>
+                <material>
+                    <ambient>.175 .175 .175  1.0</ambient>
+                    <diffuse>.175 .175 .175  1.0</diffuse>
+                    <specular>.175 .175 .175 1.0</specular>
+                </material>
+            </visual>
             <kp value="1000000.0"/>
             <kd value="100.0"/>
 

descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro

@@ -12,21 +12,63 @@
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
-        </joint>  
+        </joint>
+
+        <joint name="RF_HFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RF_KFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
 
         <joint name="LF_HAA">
             <command_interface name="effort" initial_value="0.0"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
-        </joint>  
+        </joint>
+
+        <joint name="LF_HFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="LF_KFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
 
         <joint name="RH_HAA">
             <command_interface name="effort" initial_value="0.0"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
-        </joint>  
+        </joint>
+
+        <joint name="RH_HFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RH_KFE">
+            <command_interface name="effort" initial_value="0.0"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
 
         <joint name="LH_HAA">
             <command_interface name="effort" initial_value="0.0"/>
@@ -35,54 +77,12 @@
             <state_interface name="effort"/>
         </joint>
 
-        <joint name="RF_HFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
-
-        <joint name="LF_HFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
-
-        <joint name="RH_HFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
-
         <joint name="LH_HFE">
             <command_interface name="effort" initial_value="0.0"/>
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
-        </joint>  
+        </joint>
-
-        <joint name="RF_KFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
-
-        <joint name="LF_KFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
-
-        <joint name="RH_KFE">
-            <command_interface name="effort" initial_value="0.0"/>
-            <state_interface name="position"/>
-            <state_interface name="velocity"/>
-            <state_interface name="effort"/>
-        </joint>  
 
         <joint name="LH_KFE">
             <command_interface name="effort" initial_value="0.0"/>
@@ -133,7 +133,7 @@
     <!-- Gazebo's ros2_control plugin  -->
     <gazebo>
         <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-            <parameters>$(find quadruped_gazebo)/config/jtc.yaml</parameters>
+            <parameters>$(find quadruped_gazebo)/config/robot_control.yaml</parameters>
         </plugin>
         <plugin filename="gz-sim-imu-system"
                 name="gz::sim::systems::Imu">

descriptions/quadruped_gazebo/urdf/go1/const.xacro

@@ -128,4 +128,14 @@
     <!-- foot -->
     <xacro:property name="foot_mass" value="0.06"/>
 
+    <gazebo reference="trunk">
+        <visual name="DarkGrey">
+            <material>
+                <ambient>.5 .5 .5  1.0</ambient>
+                <diffuse>.5 .5 .5  1.0</diffuse>
+                <specular>.5 .5 .5  1.0</specular>
+            </material>
+        </visual>
+    </gazebo>
+
 </robot>

descriptions/quadruped_gazebo/urdf/robot.xacro

@@ -13,6 +13,21 @@
     <xacro:property name="mesh_path" value="file://$(find quadruped_gazebo)/meshes/$(arg robot_type)"/>
 
     <link name="base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry>
+                <box size="0.001 0.001 0.001"/>
+            </geometry>
+        </visual>
+    </link>
+
+    <joint name="floating_base" type="fixed">
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <parent link="base"/>
+        <child link="trunk"/>
+    </joint>
+
+    <link name="trunk">
         <visual>
             <origin rpy="0 0 0" xyz="0 0 0"/>
             <geometry>
@@ -35,7 +50,7 @@
         </inertial>
     </link>
 
-    <xacro:IMU connected_to="base" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <xacro:IMU connected_to="trunk" imu_name="base_imu" xyz="0. 0. 0." rpy="0. 0. 0."/>
 
 
     <xacro:leg prefix="LF" mesh_path="${mesh_path}" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">

hardwares/hardware_unitree_mujoco/README.md

@@ -1,5 +1,9 @@
 # Hardware Unitree Mujoco
 
+This package contains the hardware interface based on [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) to control the Unitree robot in Mujoco simulator. 
+
+In theory, it also can communicate with real robot, but it is not tested yet. You can use go2 simulation in [unitree_mujoco](https://github.com/unitreerobotics/unitree_mujoco).
+
 * build
 ```bash
 cd ~/ros2_ws

hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h

@@ -26,7 +26,7 @@ public:
     hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
 
 protected:
-    std::vector<double> joint_effort_command_;
+    std::vector<double> joint_torque_command_;
     std::vector<double> joint_position_command_;
     std::vector<double> joint_velocities_command_;
     std::vector<double> joint_kp_command_;

hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp

@@ -18,7 +18,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
         return CallbackReturn::ERROR;
     }
 
-    joint_effort_command_.assign(12, 0);
+    joint_torque_command_.assign(12, 0);
     joint_position_command_.assign(12, 0);
     joint_velocities_command_.assign(12, 0);
     joint_kp_command_.assign(12, 0);
@@ -100,7 +100,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
 
     ind = 0;
     for (const auto &joint_name: joint_interfaces["effort"]) {
-        command_interfaces.emplace_back(joint_name, "effort", &joint_effort_command_[ind]);
+        command_interfaces.emplace_back(joint_name, "effort", &joint_torque_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]);
         ind++;
@@ -108,7 +108,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
     return command_interfaces;
 }
 
-return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Duration &period) {
+return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
     // joint states
     for (int i(0); i < 12; ++i) {
         joint_position_[i] = low_state_.motor_state()[i].q();
@@ -131,7 +131,7 @@ return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Durati
     return return_type::OK;
 }
 
-return_type HardwareUnitree::write(const rclcpp::Time &, const rclcpp::Duration &) {
+return_type HardwareUnitree::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
     // send command
     for (int i(0); i < 12; ++i) {
         low_cmd_.motor_cmd()[i].mode() = 0x01;
@@ -139,7 +139,7 @@ return_type HardwareUnitree::write(const rclcpp::Time &, const rclcpp::Duration
         low_cmd_.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]);
         low_cmd_.motor_cmd()[i].kp() = static_cast<float>(joint_kp_command_[i]);
         low_cmd_.motor_cmd()[i].kd() = static_cast<float>(joint_kd_command_[i]);
-        low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_effort_command_[i]);
+        low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_torque_command_[i]);
     }
 
     low_cmd_.crc() = crc32_core(reinterpret_cast<uint32_t *>(&low_cmd_),