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/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..413d93c --- /dev/null +++ b/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) \ 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..06ecc8b --- /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 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 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..de5ba0c --- /dev/null +++ b/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 >("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; + } + + 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 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 + "/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_EXPORT_CLASS(leg_pd_controller::LegPdController, controller_interface::ChainableControllerInterface); 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/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h index 743e6db..5cc4f93 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h @@ -81,7 +81,9 @@ namespace unitree_guide_controller { std::vector state_interface_types_; std::string imu_name_; + std::string command_prefix_; std::vector imu_interface_types_; + std::vector feet_names_; rclcpp::Subscription::SharedPtr control_input_subscription_; rclcpp::Subscription::SharedPtr robot_description_subscription_; @@ -89,7 +91,7 @@ namespace unitree_guide_controller { std::unordered_map< std::string, std::vector > *> 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_}, 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/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/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index 76bdd18..ae14238 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/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 &feet_names); /** * Calculate the joint positions based on the foot end position diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h index 885dad7..27cd6a6 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h @@ -10,7 +10,6 @@ #include #include #include -#include #include class RobotLeg { 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/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..2ca1761 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp +++ b/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_kp_command_interface_[i].get().set_value(50.0); - ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5); + ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0); + ctrl_comp_.joint_kp_command_interface_[i].get().set_value(30.0); + ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.5); } } 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/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/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..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() { @@ -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..3eac9a7 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/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("imu_name", imu_name_); imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); + command_prefix_ = auto_declare("command_prefix", command_prefix_); + feet_names_ = + auto_declare >("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( - "~/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(ctrl_comp_); state_list_.fixedDown = std::make_shared(ctrl_comp_); state_list_.fixedStand = std::make_shared(ctrl_comp_); 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/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 508ac71..3a96ab8 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/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 &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", "FL_foot", fl_chain_); - robot_tree.getChain("base", "RR_foot", rr_chain_); - robot_tree.getChain("base", "RL_foot", rl_chain_); + robot_tree.getChain("base", feet_names[0], fr_chain_); + robot_tree.getChain("base", feet_names[1], fl_chain_); + robot_tree.getChain("base", feet_names[2], rr_chain_); + robot_tree.getChain("base", feet_names[3], rl_chain_); + robot_legs_.resize(4); robot_legs_[0] = std::make_shared(fr_chain_); @@ -70,6 +71,7 @@ std::vector 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; } diff --git a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp index aad5e07..331d97f 100644 --- a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp +++ b/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; } 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/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index db8e7b3..af3529e 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/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: diff --git a/descriptions/go2_description/launch/hardware.launch.py b/descriptions/go2_description/launch/hardware.launch.py index 249c04f..ae7513c 100644 --- a/descriptions/go2_description/launch/hardware.launch.py +++ b/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", ) diff --git a/descriptions/quadruped_gazebo/readme.md b/descriptions/quadruped_gazebo/README.md similarity index 100% rename from descriptions/quadruped_gazebo/readme.md rename to descriptions/quadruped_gazebo/README.md diff --git a/descriptions/quadruped_gazebo/config/jtc.yaml b/descriptions/quadruped_gazebo/config/jtc.yaml deleted file mode 100644 index 513bccc..0000000 --- a/descriptions/quadruped_gazebo/config/jtc.yaml +++ /dev/null @@ -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" \ No newline at end of file diff --git a/descriptions/quadruped_gazebo/config/robot_control.yaml b/descriptions/quadruped_gazebo/config/robot_control.yaml new file mode 100644 index 0000000..00f32b9 --- /dev/null +++ b/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 \ 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..5a604e7 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,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( - 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], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=leg_pd_controller, + on_exit=[unitree_guide_controller], + ) + ), ] diff --git a/descriptions/quadruped_gazebo/urdf/common/leg.xacro b/descriptions/quadruped_gazebo/urdf/common/leg.xacro index 04a487e..df9f6bb 100644 --- a/descriptions/quadruped_gazebo/urdf/common/leg.xacro +++ b/descriptions/quadruped_gazebo/urdf/common/leg.xacro @@ -8,7 +8,7 @@ - + @@ -40,7 +40,7 @@ - + @@ -81,7 +81,7 @@ scale="1 1 1"/> - + @@ -121,7 +121,7 @@ - + @@ -151,7 +151,7 @@ - + @@ -178,26 +178,53 @@ 0.2 0.2 - Gazebo/DarkGrey + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + 0.2 0.2 0 - Gazebo/DarkGrey + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + + 0.6 0.6 1 + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + + 0.6 0.6 1 - Gazebo/DarkGrey + + + .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 b52f7b2..819a32c 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 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -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 8ac87b1..e7b6831 100644 --- a/descriptions/quadruped_gazebo/urdf/go1/const.xacro +++ b/descriptions/quadruped_gazebo/urdf/go1/const.xacro @@ -128,4 +128,14 @@ + + + + .5 .5 .5 1.0 + .5 .5 .5 1.0 + .5 .5 .5 1.0 + + + + diff --git a/descriptions/quadruped_gazebo/urdf/robot.xacro b/descriptions/quadruped_gazebo/urdf/robot.xacro index 4278ea4..b4671fb 100644 --- a/descriptions/quadruped_gazebo/urdf/robot.xacro +++ b/descriptions/quadruped_gazebo/urdf/robot.xacro @@ -13,6 +13,21 @@ + + + + + + + + + + + + + + + @@ -35,7 +50,7 @@ - + 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 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..85d442e 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, "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 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_),