From 5825c3cac22e7b169e7caebbd7b2180d911229a0 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Thu, 19 Sep 2024 22:45:08 +0800 Subject: [PATCH] add pd controller --- controllers/leg_pd_controller/CMakeLists.txt | 66 ++++++ controllers/leg_pd_controller/LICENSE | 202 ++++++++++++++++++ controllers/leg_pd_controller/README.md | 6 + .../leg_pd_controller/LegPdController.h | 68 ++++++ .../leg_pd_controller/leg_pd_controller.xml | 9 + controllers/leg_pd_controller/package.xml | 18 ++ .../leg_pd_controller/src/LegPdController.cpp | 117 ++++++++++ .../UnitreeGuideController.h | 2 +- .../control/CtrlComponent.h | 4 +- .../src/FSM/StateBalanceTest.cpp | 2 +- .../src/FSM/StateFixedDown.cpp | 2 +- .../src/FSM/StateFixedStand.cpp | 2 +- .../src/FSM/StatePassive.cpp | 2 +- .../src/FSM/StateSwingTest.cpp | 2 +- .../src/FSM/StateTrotting.cpp | 2 +- .../src/UnitreeGuideController.cpp | 1 + .../go2_description/config/robot_control.yaml | 2 +- .../go2_description/xacro/ros2_control.xacro | 24 +-- descriptions/quadruped_gazebo/config/jtc.yaml | 74 ++++++- .../config/visualize_urdf.rviz | 12 +- .../quadruped_gazebo/launch/gazebo.launch.py | 79 ++++--- .../urdf/common/ros2_control.xacro | 92 ++++---- .../hardware_unitree_mujoco/HardwareUnitree.h | 2 +- .../src/HardwareUnitree.cpp | 10 +- 24 files changed, 691 insertions(+), 109 deletions(-) create mode 100644 controllers/leg_pd_controller/CMakeLists.txt create mode 100644 controllers/leg_pd_controller/LICENSE create mode 100644 controllers/leg_pd_controller/README.md create mode 100644 controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h create mode 100644 controllers/leg_pd_controller/leg_pd_controller.xml create mode 100644 controllers/leg_pd_controller/package.xml create mode 100644 controllers/leg_pd_controller/src/LegPdController.cpp diff --git a/controllers/leg_pd_controller/CMakeLists.txt b/controllers/leg_pd_controller/CMakeLists.txt new file mode 100644 index 0000000..609f950 --- /dev/null +++ b/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 + "$" + "$") +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() diff --git a/controllers/leg_pd_controller/LICENSE b/controllers/leg_pd_controller/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/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. diff --git a/controllers/leg_pd_controller/README.md b/controllers/leg_pd_controller/README.md new file mode 100644 index 0000000..20100c1 --- /dev/null +++ b/controllers/leg_pd_controller/README.md @@ -0,0 +1,6 @@ +# Leg PD Controller + +```bash +cd ~/ros2_ws +colcon build --packages-up-to leg_pd_controller +``` \ No newline at end of file diff --git a/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h b/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h new file mode 100644 index 0000000..13d26a5 --- /dev/null +++ b/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 +#include + + +namespace leg_pd_controller { + class LegPdController : 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 on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + std::vector joint_effort_command_; + std::vector joint_position_command_; + std::vector joint_velocities_command_; + std::vector joint_kp_command_; + std::vector joint_kd_command_; + + std::vector joint_names_; + + std::vector state_interface_types_; + std::vector reference_interface_types_; + + std::vector > + joint_effort_command_interface_; + std::vector > + joint_position_state_interface_; + std::vector > + joint_velocity_state_interface_; + + std::unordered_map< + std::string, std::vector > *> + state_interface_map_ = { + {"position", &joint_position_state_interface_}, + {"velocity", &joint_velocity_state_interface_} + }; + }; +} + + +#endif //LEGPDCONTROLLER_H diff --git a/controllers/leg_pd_controller/leg_pd_controller.xml b/controllers/leg_pd_controller/leg_pd_controller.xml new file mode 100644 index 0000000..6e34e61 --- /dev/null +++ b/controllers/leg_pd_controller/leg_pd_controller.xml @@ -0,0 +1,9 @@ + + + + PD controller for simulation which only support effort command input + + + diff --git a/controllers/leg_pd_controller/package.xml b/controllers/leg_pd_controller/package.xml new file mode 100644 index 0000000..70308c0 --- /dev/null +++ b/controllers/leg_pd_controller/package.xml @@ -0,0 +1,18 @@ + + + + leg_pd_controller + 0.0.0 + TODO: Package description + Huang Zhenbiao + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/controllers/leg_pd_controller/src/LegPdController.cpp b/controllers/leg_pd_controller/src/LegPdController.cpp new file mode 100644 index 0000000..7f2d08b --- /dev/null +++ b/controllers/leg_pd_controller/src/LegPdController.cpp @@ -0,0 +1,117 @@ +// +// 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 >("joints", joint_names_); + reference_interface_types_ = + auto_declare >("reference_interfaces", reference_interface_types_); + state_interface_types_ = auto_declare >("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; + } + 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*/) { + // 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*/) { + 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 LegPdController::on_export_reference_interfaces() { + std::vector 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 + "/torque", &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_EXPORT_CLASS(leg_pd_controller::LegPdController, controller_interface::ChainableControllerInterface); diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index 743e6db..0be4c35 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -89,7 +89,7 @@ namespace unitree_guide_controller { std::unordered_map< std::string, std::vector > *> command_interface_map_ = { - {"effort", &ctrl_comp_.joint_effort_command_interface_}, + {"torque", &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_}, diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h index e78bb7e..3c2475c 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h @@ -17,7 +17,7 @@ struct CtrlComponent { std::vector > - joint_effort_command_interface_; + joint_torque_command_interface_; std::vector > joint_position_command_interface_; std::vector > @@ -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(); diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index ac425e8..64a907c 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/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)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp index af2adf4..86469a0 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp @@ -25,7 +25,7 @@ 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_kd_command_interface_[i].get().set_value(3.5); } diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp index 4512683..92b4e04 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp +++ b/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); diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp index ff87aca..f4af125 100644 --- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp +++ b/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_) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index 87d41ce..d8c6cfe 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/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)); } } diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index 0645601..af646d7 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -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)); } } } diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index c4bd976..7b03428 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -138,6 +138,7 @@ namespace unitree_guide_controller { } } + // Create FSM List state_list_.passive = std::make_shared(ctrl_comp_); state_list_.fixedDown = std::make_shared(ctrl_comp_); state_list_.fixedStand = std::make_shared(ctrl_comp_); diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index db8e7b3..b843a00 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -36,7 +36,7 @@ unitree_guide_controller: - RL_calf_joint command_interfaces: - - effort + - torque - position - velocity - kp diff --git a/descriptions/go2_description/xacro/ros2_control.xacro b/descriptions/go2_description/xacro/ros2_control.xacro index ca6a705..75bd85e 100644 --- a/descriptions/go2_description/xacro/ros2_control.xacro +++ b/descriptions/go2_description/xacro/ros2_control.xacro @@ -10,7 +10,7 @@ - + @@ -22,7 +22,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -58,7 +58,7 @@ - + @@ -70,7 +70,7 @@ - + @@ -82,7 +82,7 @@ - + @@ -94,7 +94,7 @@ - + @@ -106,7 +106,7 @@ - + @@ -118,7 +118,7 @@ - + @@ -130,7 +130,7 @@ - + @@ -142,7 +142,7 @@ - + diff --git a/descriptions/quadruped_gazebo/config/jtc.yaml b/descriptions/quadruped_gazebo/config/jtc.yaml index 513bccc..2ed362a 100644 --- a/descriptions/quadruped_gazebo/config/jtc.yaml +++ b/descriptions/quadruped_gazebo/config/jtc.yaml @@ -1,8 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 1000 # Hz - use_sim_time: true # If running in simulation + update_rate: 500 # Hz # Define the available controllers joint_state_broadcaster: @@ -11,7 +10,76 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster + leg_pd_controller: + type: leg_pd_controller/LegPdController + imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" - frame_id: "imu_sensor" \ No newline at end of file + frame_id: "imu_sensor" + +leg_pd_controller: + ros__parameters: + update_rate: 500 # Hz + joints: + - RF_HAA + - RH_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: + joints: + - RF_HAA + - RH_HFE + - RF_KFE + - LF_HAA + - LF_HFE + - LF_KFE + - RH_HAA + - RH_HFE + - RH_KFE + - LH_HAA + - LH_HFE + - LH_KFE + + command_interfaces: + - torque + - 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 \ No newline at end of file diff --git a/descriptions/quadruped_gazebo/config/visualize_urdf.rviz b/descriptions/quadruped_gazebo/config/visualize_urdf.rviz index 20bb048..815cd46 100644 --- a/descriptions/quadruped_gazebo/config/visualize_urdf.rviz +++ b/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 - X: 787 - Y: 356 + Width: 741 + X: 773 + Y: 267 diff --git a/descriptions/quadruped_gazebo/launch/gazebo.launch.py b/descriptions/quadruped_gazebo/launch/gazebo.launch.py index 4b7223a..0ac0624 100644 --- a/descriptions/quadruped_gazebo/launch/gazebo.launch.py +++ b/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,59 @@ 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"], + ) + + return [ - Node( - 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"], - ), + 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], + ) + ), ] diff --git a/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro b/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro index b52f7b2..4ba6fd3 100644 --- a/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro +++ b/descriptions/quadruped_gazebo/urdf/common/ros2_control.xacro @@ -12,21 +12,63 @@ - + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + @@ -35,54 +77,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h index 08418d3..a40ce9c 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/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 joint_effort_command_; + std::vector joint_torque_command_; std::vector joint_position_command_; std::vector joint_velocities_command_; std::vector joint_kp_command_; diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index d000e75..826259a 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/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 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, "torque", &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 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(joint_velocities_command_[i]); low_cmd_.motor_cmd()[i].kp() = static_cast(joint_kp_command_[i]); low_cmd_.motor_cmd()[i].kd() = static_cast(joint_kd_command_[i]); - low_cmd_.motor_cmd()[i].tau() = static_cast(joint_effort_command_[i]); + low_cmd_.motor_cmd()[i].tau() = static_cast(joint_torque_command_[i]); } low_cmd_.crc() = crc32_core(reinterpret_cast(&low_cmd_),