diff --git a/README.md b/README.md index 2bac5a4..1e6d3d8 100644 --- a/README.md +++ b/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 diff --git a/commands/readme.md b/commands/README.md similarity index 100% rename from commands/readme.md rename to commands/README.md diff --git a/controllers/leg_pd_controller/README.md b/controllers/leg_pd_controller/README.md index 20100c1..413d93c 100644 --- a/controllers/leg_pd_controller/README.md +++ b/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 -``` \ No newline at end of file +``` + +## 3. Run +* [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo) \ No newline at end of file diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index 4fc1da0..7046c84 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/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) diff --git a/controllers/unitree_guide_controller/README.md b/controllers/unitree_guide_controller/README.md index 6ad2457..a1324c6 100644 --- a/controllers/unitree_guide_controller/README.md +++ b/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 -source ~/ros2_ws/install/setup.bash -ros2 launch unitree_guide_controller controller.launch.py -``` \ No newline at end of file +## 3. Run +* [Go2 in mujoco simulation](../../descriptions/go2_description) +* [Go1/A1 in gazebo simulation](../../descriptions/quadruped_gazebo) \ No newline at end of file diff --git a/controllers/unitree_guide_controller/config/controller.yaml b/controllers/unitree_guide_controller/config/controller.yaml deleted file mode 100644 index e21fb2a..0000000 --- a/controllers/unitree_guide_controller/config/controller.yaml +++ /dev/null @@ -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 \ No newline at end of file diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h old mode 100755 new mode 100644 diff --git a/controllers/unitree_guide_controller/launch/controller.launch.py b/controllers/unitree_guide_controller/launch/controller.launch.py deleted file mode 100644 index 8382500..0000000 --- a/controllers/unitree_guide_controller/launch/controller.launch.py +++ /dev/null @@ -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) diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp index 86469a0..2ca1761 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/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_kd_command_interface_[i].get().set_value(3.5); + ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0); + ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 1efaa69..eaedf75 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/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); } diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index af646d7..50c666b 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/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() { diff --git a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp index 9c7423e..ac69bd2 100644 --- a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp +++ b/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_ = WaveStatus::SWING_ALL; + status_past_ = WaveStatus::STANCE_ALL; + status_ = WaveStatus::STANCE_ALL; } void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) { diff --git a/descriptions/go1_description/config/robot_control.yaml b/descriptions/go1_description/config/robot_control.yaml index db8e7b3..af3529e 100644 --- a/descriptions/go1_description/config/robot_control.yaml +++ b/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: diff --git a/descriptions/quadruped_gazebo/config/jtc.yaml b/descriptions/quadruped_gazebo/config/robot_control.yaml similarity index 100% rename from descriptions/quadruped_gazebo/config/jtc.yaml rename to descriptions/quadruped_gazebo/config/robot_control.yaml diff --git a/descriptions/quadruped_gazebo/urdf/common/leg.xacro b/descriptions/quadruped_gazebo/urdf/common/leg.xacro index 8eb5e06..df9f6bb 100644 --- a/descriptions/quadruped_gazebo/urdf/common/leg.xacro +++ b/descriptions/quadruped_gazebo/urdf/common/leg.xacro @@ -178,21 +178,25 @@ 0.2 0.2 - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.000000 1.500000 - + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + 0.2 0.2 0 - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.000000 1.500000 - + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + @@ -201,22 +205,26 @@ 0.6 0.6 1 - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.000000 1.500000 - + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + 0.6 0.6 1 - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.000000 1.500000 - + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + diff --git a/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro b/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro index 4ba6fd3..819a32c 100644 --- a/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro +++ b/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro @@ -133,7 +133,7 @@ - $(find quadruped_gazebo)/config/jtc.yaml + $(find quadruped_gazebo)/config/robot_control.yaml diff --git a/descriptions/quadruped_gazebo/urdf/go1/const.xacro b/descriptions/quadruped_gazebo/urdf/go1/const.xacro index da16e3e..e7b6831 100644 --- a/descriptions/quadruped_gazebo/urdf/go1/const.xacro +++ b/descriptions/quadruped_gazebo/urdf/go1/const.xacro @@ -129,11 +129,13 @@ - - .175 .175 .175 1.0 - .175 .175 .175 1.0 - .175 .175 .175 1.000000 1.500000 - + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md index c2ff9ab..e9ad656 100644 --- a/hardwares/hardware_unitree_mujoco/README.md +++ b/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