fix go1 gazebo visualization

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/README.md

@@ -1,6 +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/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/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/StateFixedDown.cpp

@@ -26,8 +26,8 @@ void StateFixedDown::run() {
             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(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/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/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() {

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) {

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/quadruped_gazebo/urdf/common/leg.xacro

@@ -178,21 +178,25 @@
         <gazebo reference="${prefix}_hip">
             <mu1>0.2</mu1>
             <mu2>0.2</mu2>
+            <visual>
                 <material>
-                <ambient>.175 .175 .175  1.0</ambient>
+                    <ambient>.5 .5 .5  1.0</ambient>
-                <diffuse>.175 .175 .175  1.0</diffuse>
+                    <diffuse>.5 .5 .5  1.0</diffuse>
-                <specular>.175 .175 .175 1.000000 1.500000</specular>
+                    <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>
+            <visual>
                 <material>
                     <ambient>.175 .175 .175  1.0</ambient>
                     <diffuse>.175 .175 .175  1.0</diffuse>
-                <specular>.175 .175 .175 1.000000 1.500000</specular>
+                    <specular>.175 .175 .175 1.0</specular>
                 </material>
+            </visual>
             <kp value="1000000.0"/>
             <kd value="100.0"/>
         </gazebo>
@@ -201,22 +205,26 @@
             <mu1>0.6</mu1>
             <mu2>0.6</mu2>
             <self_collide>1</self_collide>
+            <visual>
                 <material>
-                <ambient>.175 .175 .175  1.0</ambient>
+                    <ambient>.5 .5 .5  1.0</ambient>
-                <diffuse>.175 .175 .175  1.0</diffuse>
+                    <diffuse>.5 .5 .5  1.0</diffuse>
-                <specular>.175 .175 .175 1.000000 1.500000</specular>
+                    <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>
+            <visual>
                 <material>
                     <ambient>.175 .175 .175  1.0</ambient>
                     <diffuse>.175 .175 .175  1.0</diffuse>
-                <specular>.175 .175 .175 1.000000 1.500000</specular>
+                    <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

@@ -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

@@ -129,11 +129,13 @@
     <xacro:property name="foot_mass" value="0.06"/>
 
     <gazebo reference="trunk">
+        <visual name="DarkGrey">
             <material>
-            <ambient>.175 .175 .175  1.0</ambient>
+                <ambient>.5 .5 .5  1.0</ambient>
-            <diffuse>.175 .175 .175  1.0</diffuse>
+                <diffuse>.5 .5 .5  1.0</diffuse>
-            <specular>.175 .175 .175 1.000000 1.500000</specular>
+                <specular>.5 .5 .5  1.0</specular>
             </material>
+        </visual>
     </gazebo>
 
 </robot>

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