diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h index d88b3fc..c23c858 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h @@ -4,6 +4,7 @@ #pragma once #include "StateEstimateBase.h" +#include namespace ocs2::legged_robot { @@ -17,7 +18,6 @@ namespace ocs2::legged_robot protected: rclcpp::Subscription::SharedPtr odom_sub_; - vector3_t position_; - vector3_t linear_velocity_; + realtime_tools::RealtimeBuffer buffer_; }; }; diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index 004dbbc..b18ffb9 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -101,7 +101,13 @@ def launch_setup(context, *args, **kwargs): RegisterEventHandler( event_handler=OnProcessExit( target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, ocs2_controller], + on_exit=[imu_sensor_broadcaster, joint_state_publisher], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[ocs2_controller], ) ), ] diff --git a/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp b/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp index 7abf9f5..918c494 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp @@ -4,38 +4,38 @@ #include "ocs2_quadruped_controller/estimator/FromOdomTopic.h" -namespace ocs2::legged_robot -{ - FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent& ctrl_component, - const rclcpp_lifecycle::LifecycleNode::SharedPtr& node) : - StateEstimateBase( - std::move(info), ctrl_component, - node) - { +namespace ocs2::legged_robot { + FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent &ctrl_component, + const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) : StateEstimateBase( + std::move(info), ctrl_component, + node) { odom_sub_ = node_->create_subscription( - "/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) - { - // Handle message - position_ = { - msg->pose.pose.position.x, - msg->pose.pose.position.y, - msg->pose.pose.position.z, - }; - - linear_velocity_ = { - msg->twist.twist.linear.x, - msg->twist.twist.linear.y, - msg->twist.twist.linear.z, - }; + "/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + buffer_.writeFromNonRT(*msg); }); } - vector_t FromOdomTopic::update(const rclcpp::Time& time, const rclcpp::Duration& period) - { - updateJointStates(); - updateImu(); + vector_t FromOdomTopic::update(const rclcpp::Time &time, const rclcpp::Duration &period) { + const nav_msgs::msg::Odometry odom = *buffer_.readFromRT(); - updateLinear(position_, linear_velocity_); + updateJointStates(); + updateAngular(quatToZyx(Eigen::Quaternion( + odom.pose.pose.orientation.w, + odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z)), + Eigen::Matrix( + odom.twist.twist.angular.x, + odom.twist.twist.angular.y, + odom.twist.twist.angular.z)); + updateLinear(Eigen::Matrix( + odom.pose.pose.position.x, + odom.pose.pose.position.y, + odom.pose.pose.position.z), + Eigen::Matrix( + odom.twist.twist.linear.x, + odom.twist.twist.linear.y, + odom.twist.twist.linear.z)); return rbd_state_; } } diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index d869562..3df6974 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -68,6 +68,7 @@ namespace ocs2::legged_robot { const vector3_t zyx = quatToZyx(quat_) - zyx_offset_; const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat_), angular_vel_local_)); + updateAngular(zyx, angularVelGlobal); } diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index f1538f4..eaca11e 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -60,6 +60,11 @@ ros2 launch a1_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=a1_description height:=0.43 ``` +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=a1_description + ``` * RL Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index 3504584..e81c73d 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -16,6 +16,9 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController + ocs2_quadruped_controller: + type: ocs2_quadruped_controller/Ocs2QuadrupedController + rl_quadruped_controller: type: rl_quadruped_controller/LeggedGymController @@ -98,6 +101,60 @@ unitree_guide_controller: - linear_acceleration.y - linear_acceleration.z +ocs2_quadruped_controller: + ros__parameters: + update_rate: 500 # Hz + robot_pkg: "a1_description" + command_prefix: "leg_pd_controller" + joints: + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet: + - FL_foot + - FR_foot + - RL_foot + - RR_foot + + imu_name: "imu_sensor" + base_name: "base" + + 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 + + estimator_type: "odom_topic" + rl_quadruped_controller: ros__parameters: update_rate: 200 # Hz diff --git a/descriptions/unitree/a1_description/xacro/gazebo.xacro b/descriptions/unitree/a1_description/xacro/gazebo.xacro index e8ce4f7..ba9dd39 100644 --- a/descriptions/unitree/a1_description/xacro/gazebo.xacro +++ b/descriptions/unitree/a1_description/xacro/gazebo.xacro @@ -111,6 +111,16 @@ + + odom + base + 500 + odom + 3 + odom_with_covariance + tf + @@ -184,5 +194,4 @@ true - diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index 9fdcef3..d15c0d7 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -10,9 +10,6 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster - leg_pd_controller: - type: leg_pd_controller/LegPdController - unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController @@ -27,35 +24,10 @@ imu_sensor_broadcaster: sensor_name: "imu_sensor" frame_id: "imu_link" -leg_pd_controller: - ros__parameters: - update_rate: 1000 # Hz - 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 - - state_interfaces: - - position - - velocity - ocs2_quadruped_controller: ros__parameters: update_rate: 100 # Hz robot_pkg: "go2_description" - command_prefix: "leg_pd_controller" joints: - FL_hip_joint - FL_thigh_joint @@ -108,7 +80,6 @@ ocs2_quadruped_controller: unitree_guide_controller: ros__parameters: update_rate: 200 # Hz - command_prefix: "leg_pd_controller" joints: - FR_hip_joint - FR_thigh_joint diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index d770678..6442c9d 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -1,230 +1,279 @@ - - - gz_ros2_control/GazeboSimSystem - + + + gz_quadruped_hardware/GazeboSimSystem + - - - - - - + + + + + + - - - - - - + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + - - - $(find go2_description)/config/gazebo.yaml - - - - - odom - base - 200 - odom - 3 - odom_with_covariance - tf - + + + + + + + + + + + + + + + + + $(find go2_description)/config/gazebo.yaml + + + + + odom + base + 1000 + odom + 3 + odom_with_covariance + tf + + + + + 0.6 + 0.6 + 1 + + + + + 1 + 500 + true + imu + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + true + + + + + + + + 2.094 + + 1280 + 720 + + + 0.1 + 15 + + + gaussian + + 0.0 + 0.007 + + front_camera + camera/camera_info + + true + 15 + true + camera/image + - - - 0.6 - 0.6 - 1 - - - - - 1 - 500 - true - imu - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - - true - - - - - - - - 2.094 - - 1280 - 720 - - - 0.1 - 15 - - - gaussian - - 0.0 - 0.007 - - front_camera - camera/camera_info - - true - 15 - true - camera/image - - - + diff --git a/hardwares/gz_quadruped_hardware/CMakeLists.txt b/hardwares/gz_quadruped_hardware/CMakeLists.txt new file mode 100644 index 0000000..a4e6ac3 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.8) +project(gz_quadruped_hardware) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Compiler options +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(controller_manager REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) + +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED) + +include_directories(include) + +add_library(${PROJECT_NAME}-system SHARED + src/gz_quadruped_plugin.cpp +) + +target_link_libraries(${PROJECT_NAME}-system + gz-sim::gz-sim + gz-plugin::register +) +ament_target_dependencies(${PROJECT_NAME}-system + ament_index_cpp + controller_manager + hardware_interface + pluginlib + rclcpp + yaml_cpp_vendor + rclcpp_lifecycle +) + +######### + +add_library(gz_quadruped_plugins SHARED + src/gz_system.cpp +) +ament_target_dependencies(gz_quadruped_plugins + rclcpp_lifecycle + hardware_interface + rclcpp +) +target_link_libraries(gz_quadruped_plugins + gz-sim::gz-sim +) + +## Install +install(TARGETS + gz_quadruped_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY + include/ + DESTINATION include +) + +# Testing and linting +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}-system gz_quadruped_plugins) + +# Install directories +install(TARGETS ${PROJECT_NAME}-system + DESTINATION lib +) + +pluginlib_export_plugin_description_file(gz_quadruped_hardware gz_quadruped_hardware.xml) + +# Setup the project +ament_package() diff --git a/hardwares/gz_quadruped_hardware/LICENSE b/hardwares/gz_quadruped_hardware/LICENSE new file mode 100644 index 0000000..6d8d58f --- /dev/null +++ b/hardwares/gz_quadruped_hardware/LICENSE @@ -0,0 +1,191 @@ + + Apache License + Version 2.0, January 2004 + https://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 + + Copyright 2013-2018 Docker, Inc. + + 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 + + https://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/hardwares/gz_quadruped_hardware/gz_quadruped_hardware.xml b/hardwares/gz_quadruped_hardware/gz_quadruped_hardware.xml new file mode 100644 index 0000000..453ecf2 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/gz_quadruped_hardware.xml @@ -0,0 +1,10 @@ + + + + Gazebo ros2_control plugin for quadruped robot. + + + diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp new file mode 100644 index 0000000..92d90e1 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp @@ -0,0 +1,57 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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. +#pragma once + +#include + +#include +namespace sim = gz::sim; + +namespace gz_quadruped_hardware { + // Forward declarations. + class GazeboSimQuadrupedPluginPrivate; + + class GazeboSimQuadrupedPlugin + : public sim::System, + public sim::ISystemConfigure, + public sim::ISystemPreUpdate, + public sim::ISystemPostUpdate { + public: + /// \brief Constructor + GazeboSimQuadrupedPlugin(); + + /// \brief Destructor + ~GazeboSimQuadrupedPlugin() override; + + // Documentation inherited + void Configure( + const sim::Entity &_entity, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &_ecm, + sim::EventManager &_eventMgr) override; + + // Documentation inherited + void PreUpdate( + const sim::UpdateInfo &_info, + sim::EntityComponentManager &_ecm) override; + + void PostUpdate( + const sim::UpdateInfo &_info, + const sim::EntityComponentManager &_ecm) override; + + private: + /// \brief Private data pointer. + std::unique_ptr dataPtr; + }; +} // namespace gz_ros2_control diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp new file mode 100644 index 0000000..f376201 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp @@ -0,0 +1,83 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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. +#pragma once + + +#include +#include +#include +#include + +#include "gz_quadruped_hardware/gz_system_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace gz_quadruped_hardware { + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + // Forward declaration + class GazeboSimSystemPrivate; + + // These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a + // simulated `ros2_control` `hardware_interface::SystemInterface`. + + class GazeboSimSystem final : public GazeboSimSystemInterface { + public: + // Documentation Inherited + CallbackReturn on_init(const hardware_interface::HardwareInfo &system_info) + override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + + // Documentation Inherited + std::vector export_state_interfaces() override; + + // Documentation Inherited + std::vector export_command_interfaces() override; + + // Documentation Inherited + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + + // Documentation Inherited + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + // Documentation Inherited + hardware_interface::return_type read( + const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + // Documentation Inherited + hardware_interface::return_type write( + const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + // Documentation Inherited + bool initSim( + rclcpp::Node::SharedPtr &model_nh, + std::map &joints, + const hardware_interface::HardwareInfo &hardware_info, + sim::EntityComponentManager &_ecm, + unsigned int update_rate) override; + + private: + // Register a sensor (for now just IMUs) + // \param[in] hardware_info hardware information where the data of + // the sensors is extract. + void registerSensors( + const hardware_interface::HardwareInfo &hardware_info); + + /// \brief Private data class + std::unique_ptr dataPtr; + }; +} // namespace gz_ros2_control diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp new file mode 100644 index 0000000..e2955b5 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp @@ -0,0 +1,125 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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. +#pragma once + +#include +#include +#include + +#include +namespace sim = gz::sim; + +#include + +#include + +namespace gz_quadruped_hardware { + /// \brief This class allows us to handle flags easily, instead of using strings + /// + /// For example + /// enum ControlMethod_ + /// { + /// NONE = 0, + /// POSITION = (1 << 0), + /// VELOCITY = (1 << 1), + /// EFFORT = (1 << 2), + /// }; + /// typedef SafeEnum ControlMethod; + /// + /// ControlMethod foo; + /// foo |= POSITION // Foo has the position flag active + /// foo & POSITION -> True // Check if position is active in the flag + /// foo & VELOCITY -> False // Check if velocity is active in the flag + + template::type> + class SafeEnum { + public: + SafeEnum() + : mFlags(0) { + } + + explicit SafeEnum(ENUM singleFlag) + : mFlags(singleFlag) { + } + + SafeEnum(const SafeEnum &original) + : mFlags(original.mFlags) { + } + + SafeEnum &operator|=(ENUM addValue) { + mFlags |= addValue; + return *this; + } + + SafeEnum operator|(ENUM addValue) { + SafeEnum result(*this); + result |= addValue; + return result; + } + + SafeEnum &operator&=(ENUM maskValue) { + mFlags &= maskValue; + return *this; + } + + SafeEnum operator&(ENUM maskValue) { + SafeEnum result(*this); + result &= maskValue; + return result; + } + + SafeEnum operator~() { + SafeEnum result(*this); + result.mFlags = ~result.mFlags; + return result; + } + + explicit operator bool() { return mFlags != 0; } + + protected: + UNDERLYING mFlags; + }; + + // SystemInterface provides API-level access to read and command joint properties. + class GazeboSimSystemInterface + : public hardware_interface::SystemInterface { + public: + /// \brief Initialize the system interface + /// param[in] model_nh Pointer to the ros2 node + /// param[in] joints Map with the name of the joint as the key and the value is + /// related with the entity in Gazebo + /// param[in] hardware_info structure with data from URDF. + /// param[in] _ecm Entity-component manager. + /// param[in] update_rate controller update rate + virtual bool initSim( + rclcpp::Node::SharedPtr &model_nh, + std::map &joints, + const hardware_interface::HardwareInfo &hardware_info, + sim::EntityComponentManager &_ecm, + unsigned int update_rate) = 0; + + // Methods used to control a joint. + enum ControlMethod_ { + NONE = 0, + POSITION = (1 << 0), + VELOCITY = (1 << 1), + EFFORT = (1 << 2), + }; + + typedef SafeEnum ControlMethod; + + protected: + rclcpp::Node::SharedPtr nh_; + }; +} // namespace gz_ros2_control diff --git a/hardwares/gz_quadruped_hardware/package.xml b/hardwares/gz_quadruped_hardware/package.xml new file mode 100644 index 0000000..1d1a0d8 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/package.xml @@ -0,0 +1,30 @@ + + + gz_quadruped_hardware + 2.0.6 + Gazebo ros2_control package for quadruped robot. + Alejandro Hernández + Bence Magyar + Alejandro Hernández + Apache 2 + + ament_cmake + + ament_index_cpp + + gz_sim_vendor + gz_plugin_vendor + pluginlib + rclcpp + yaml_cpp_vendor + rclcpp_lifecycle + hardware_interface + controller_manager + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp new file mode 100644 index 0000000..d8e270e --- /dev/null +++ b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp @@ -0,0 +1,522 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include +#include +#include + +#include + +#include + +#include "gz_quadruped_hardware/gz_quadruped_plugin.hpp" +#include "gz_quadruped_hardware/gz_system.hpp" + +namespace gz_quadruped_hardware { + class GZResourceManager : public hardware_interface::ResourceManager { + public: + GZResourceManager( + rclcpp::Node::SharedPtr &node, + sim::EntityComponentManager &ecm, + std::map enabledJoints) + : hardware_interface::ResourceManager( + node->get_node_clock_interface(), node->get_node_logging_interface()), + gz_system_loader_("gz_quadruped_hardware", "gz_quadruped_hardware::GazeboSimSystemInterface"), + logger_(node->get_logger().get_child("GZResourceManager")) { + node_ = node; + ecm_ = &ecm; + enabledJoints_ = enabledJoints; + } + + GZResourceManager(const GZResourceManager &) = delete; + + // Called from Controller Manager when robot description is initialized from callback + bool load_and_initialize_components( + const std::string &urdf, + unsigned int update_rate) override { + components_are_loaded_and_initialized_ = true; + + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + + for (const auto &individual_hardware_info: hardware_info) { + std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name; + RCLCPP_DEBUG( + logger_, "Load hardware interface %s ...", + robot_hw_sim_type_str_.c_str()); + + // Load hardware + std::unique_ptr gzSimSystem; + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + try { + gzSimSystem = std::unique_ptr( + gz_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_)); + } catch (pluginlib::PluginlibException &ex) { + RCLCPP_ERROR( + logger_, + "The plugin failed to load for some reason. Error: %s\n", + ex.what()); + continue; + } + + // initialize simulation requirements + if (!gzSimSystem->initSim( + node_, + enabledJoints_, + individual_hardware_info, + *ecm_, + update_rate)) { + RCLCPP_FATAL( + logger_, "Could not initialize robot simulation interface"); + components_are_loaded_and_initialized_ = false; + break; + } + RCLCPP_DEBUG( + logger_, "Initialized robot simulation interface %s!", + robot_hw_sim_type_str_.c_str()); + + // initialize hardware + import_component(std::move(gzSimSystem), individual_hardware_info); + } + + return components_are_loaded_and_initialized_; + } + + private: + std::shared_ptr node_; + sim::EntityComponentManager *ecm_; + std::map enabledJoints_; + + /// \brief Interface loader + pluginlib::ClassLoader gz_system_loader_; + + rclcpp::Logger logger_; + }; + + ////////////////////////////////////////////////// + class GazeboSimQuadrupedPluginPrivate { + public: + /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no + /// joint names are specified in the plugin configuration, all valid 1-axis + /// joints are returned + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _ecm Gazebo Entity Component Manager + /// \return List of entities containing all enabled joints + std::map GetEnabledJoints( + const sim::Entity &_entity, + sim::EntityComponentManager &_ecm) const; + + /// \brief Entity ID for sensor within Gazebo. + sim::Entity entity_; + + /// \brief Node Handles + std::shared_ptr node_{nullptr}; + + /// \brief Thread where the executor will spin + std::thread thread_executor_spin_; + + /// \brief Executor to spin the controller + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + + /// \brief Timing + rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + + /// \brief Controller manager + std::shared_ptr + controller_manager_{nullptr}; + + /// \brief Last time the update method was called + rclcpp::Time last_update_sim_time_ros_ = + rclcpp::Time((int64_t) 0, RCL_ROS_TIME); + + /// \brief ECM pointer + sim::EntityComponentManager *ecm{nullptr}; + + /// \brief controller update rate + int update_rate; + }; + + ////////////////////////////////////////////////// + std::map + GazeboSimQuadrupedPluginPrivate::GetEnabledJoints( + const sim::Entity &_entity, + sim::EntityComponentManager &_ecm) const { + std::map output; + + std::vector enabledJoints; + + // Get all available joints + auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto &jointEntity: jointEntities) { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto *jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: { + // Supported joint type + break; + } + case sdf::JointType::FIXED: { + RCLCPP_INFO( + node_->get_logger(), + "[gz_quadruped_hardware] Fixed joint [%s] (Entity=%lu)] is skipped", + jointName.c_str(), jointEntity); + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: { + RCLCPP_WARN( + node_->get_logger(), + "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unsupported type." + " Only joints with a single axis are supported.", + jointName.c_str(), jointEntity); + continue; + } + default: { + RCLCPP_WARN( + node_->get_logger(), + "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unknown type", + jointName.c_str(), jointEntity); + continue; + } + } + output[jointName] = jointEntity; + } + + return output; + } + + ////////////////////////////////////////////////// + GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin() + : dataPtr(std::make_unique()) { + } + + ////////////////////////////////////////////////// + GazeboSimQuadrupedPlugin::~GazeboSimQuadrupedPlugin() { + // Stop controller manager thread + if (!this->dataPtr->controller_manager_) { + return; + } + this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); + this->dataPtr->executor_->cancel(); + this->dataPtr->thread_executor_spin_.join(); + } + + ////////////////////////////////////////////////// + void GazeboSimQuadrupedPlugin::Configure( + const sim::Entity &_entity, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &_ecm, + sim::EventManager &) { + rclcpp::Logger logger = rclcpp::get_logger("GazeboSimROS2ControlPlugin"); + // Make sure the controller is attached to a valid model + const auto model = sim::Model(_entity); + if (!model.Valid(_ecm)) { + RCLCPP_ERROR( + logger, + "[Gazebo ROS 2 Control] Failed to initialize because [%s] (Entity=%lu)] is not a model." + "Please make sure that Gazebo ROS 2 Control is attached to a valid model.", + model.Name(_ecm).c_str(), _entity); + return; + } + + // Get params from SDF + auto param_file_name = _sdf->Get("parameters"); + + if (param_file_name.empty()) { + RCLCPP_ERROR( + logger, + "Gazebo quadruped ros2 control found an empty parameters file. Failed to initialize."); + return; + } + + // Get params from SDF + std::vector arguments = {"--ros-args"}; + + auto sdfPtr = const_cast(_sdf.get()); + + sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_PARAM_FILE_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("parameters"); + } + + // Get controller manager node name + std::string controllerManagerNodeName{"controller_manager"}; + + if (sdfPtr->HasElement("controller_manager_name")) { + controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get(); + } + + std::string ns = "/"; + + // Hold joints if no control mode is active? + bool hold_joints = true; // default + if (sdfPtr->HasElement("hold_joints")) { + hold_joints = + sdfPtr->GetElement("hold_joints")->Get(); + } + double position_proportional_gain = 0.1; // default + if (sdfPtr->HasElement("position_proportional_gain")) { + position_proportional_gain = + sdfPtr->GetElement("position_proportional_gain")->Get(); + } + + if (sdfPtr->HasElement("ros")) { + sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); + + // Set namespace if tag is present + if (sdfRos->HasElement("namespace")) { + ns = sdfRos->GetElement("namespace")->Get(); + // prevent exception: namespace must be absolute, it must lead with a '/' + if (ns.empty() || ns[0] != '/') { + ns = '/' + ns; + } + } + + // Get list of remapping rules from SDF + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + + arguments.push_back(RCL_ROS_ARGS_FLAG); + while (argument_sdf) { + auto argument = argument_sdf->Get(); + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("remapping"); + } + } + } + + std::vector argv; + for (const auto &arg: arguments) { + argv.push_back(arg.data()); + } + // Create a default context, if not already + if (!rclcpp::ok()) { + init( + static_cast(argv.size()), argv.data(), rclcpp::InitOptions(), + rclcpp::SignalHandlerOptions::None); + } + + std::string node_name = "gz_quadruped"; + + this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); + this->dataPtr->executor_ = std::make_shared(); + this->dataPtr->executor_->add_node(this->dataPtr->node_); + auto spin = [this]() { + this->dataPtr->executor_->spin(); + }; + this->dataPtr->thread_executor_spin_ = std::thread(spin); + + RCLCPP_DEBUG_STREAM( + this->dataPtr->node_->get_logger(), "[Gazebo Quadruped ROS2 Control] Setting up controller for [" << + model.Name(_ecm) << "] (Entity=" << _entity << ")]."); + + // Get list of enabled joints + auto enabledJoints = this->dataPtr->GetEnabledJoints( + _entity, + _ecm); + + if (enabledJoints.size() == 0) { + RCLCPP_DEBUG_STREAM( + this->dataPtr->node_->get_logger(), + "[Gazebo Quadruped ROS2 Control] There are no available Joints."); + return; + } + + try { + this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s", + e.what()); + } + + try { + this->dataPtr->node_->declare_parameter( + "position_proportional_gain", + rclcpp::ParameterValue(position_proportional_gain)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' has invalid name, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' value is invalid, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), + "Parameter 'position_proportional_gain' value has wrong type, %s", + e.what()); + } + + std::unique_ptr resource_manager_ = + std::make_unique(this->dataPtr->node_, _ecm, enabledJoints); + + // Create the controller manager + RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); + rclcpp::NodeOptions options = controller_manager::get_cm_node_options(); + arguments.push_back("-r"); + arguments.push_back("__node:=" + controllerManagerNodeName); + arguments.push_back("-r"); + arguments.push_back("__ns:=" + ns); + options.arguments(arguments); + this->dataPtr->controller_manager_.reset( + new controller_manager::ControllerManager( + std::move(resource_manager_), + this->dataPtr->executor_, + controllerManagerNodeName, + this->dataPtr->node_->get_namespace(), options)); + this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); + + this->dataPtr->update_rate = this->dataPtr->controller_manager_->get_update_rate(); + this->dataPtr->control_period_ = rclcpp::Duration( + std::chrono::duration_cast( + std::chrono::duration(1.0 / static_cast(this->dataPtr->update_rate)))); + + // Force setting of use_sim_time parameter + this->dataPtr->controller_manager_->set_parameter( + rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true))); + + // Wait for CM to receive robot description from the topic and then initialize Resource Manager + while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) { + RCLCPP_WARN( + this->dataPtr->node_->get_logger(), + "Waiting RM to load and initialize hardware..."); + std::this_thread::sleep_for(std::chrono::microseconds(2000000)); + } + + this->dataPtr->entity_ = _entity; + } + + ////////////////////////////////////////////////// + void GazeboSimQuadrupedPlugin::PreUpdate( + const sim::UpdateInfo &_info, + sim::EntityComponentManager & /*_ecm*/) { + if (!this->dataPtr->controller_manager_) { + return; + } + static bool warned{false}; + if (!warned) { + rclcpp::Duration gazebo_period(_info.dt); + + // Check the period against the simulation period + if (this->dataPtr->control_period_ < _info.dt) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node_->get_logger(), + "Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } else if (this->dataPtr->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + this->dataPtr->node_->get_logger(), + " Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + warned = true; + } + + const rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + // Always set commands on joints, otherwise at low control frequencies the joints tremble + // as they are updated at a fraction of gazebo sim time + this->dataPtr->controller_manager_->write(sim_time_ros, sim_period); + } + + ////////////////////////////////////////////////// + void GazeboSimQuadrupedPlugin::PostUpdate( + const sim::UpdateInfo &_info, + const sim::EntityComponentManager & /*_ecm*/) { + if (!this->dataPtr->controller_manager_) { + return; + } + // Get the simulation time and period + const rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + + if (sim_period >= this->dataPtr->control_period_) { + this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; + auto gz_controller_manager = + std::dynamic_pointer_cast( + this->dataPtr->controller_manager_); + this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); + this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); + } + } +} // namespace gz_quadruped_hardware + +GZ_ADD_PLUGIN( + gz_quadruped_hardware::GazeboSimQuadrupedPlugin, + gz::sim::System, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate) diff --git a/hardwares/gz_quadruped_hardware/src/gz_system.cpp b/hardwares/gz_quadruped_hardware/src/gz_system.cpp new file mode 100644 index 0000000..f326090 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/src/gz_system.cpp @@ -0,0 +1,640 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// 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. + +#include "gz_quadruped_hardware/gz_system.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define GZ_TRANSPORT_NAMESPACE gz::transport:: +#define GZ_MSGS_NAMESPACE gz::msgs:: + +#include +#include +#include + +struct jointData { + /// \brief Joint's names. + std::string name; + + /// \brief Joint's type. + sdf::JointType joint_type; + + /// \brief Joint's axis. + sdf::JointAxis joint_axis; + + /// \brief Current joint position + double joint_position; + + /// \brief Current joint velocity + double joint_velocity; + + /// \brief Current joint effort + double joint_effort; + + /// \brief Current cmd joint position + double joint_position_cmd; + + /// \brief Current cmd joint velocity + double joint_velocity_cmd; + + /// \brief Current cmd joint effort + double joint_effort_cmd; + + double joint_kp_cmd; + + double joint_kd_cmd; + + /// \brief flag if joint is actuated (has command interfaces) or passive + bool is_actuated; + + /// \brief handles to the joints from within Gazebo + sim::Entity sim_joint; + + /// \brief Control method defined in the URDF for each joint. + gz_quadruped_hardware::GazeboSimSystemInterface::ControlMethod joint_control_method; +}; + +class ImuData { +public: + /// \brief imu's name. + std::string name{}; + + /// \brief imu's topic name. + std::string topicName{}; + + /// \brief handles to the imu from within Gazebo + sim::Entity sim_imu_sensors_ = sim::kNullEntity; + + /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration + std::array imu_sensor_data_; + + /// \brief callback to get the IMU topic values + void OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg); +}; + +void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg) { + this->imu_sensor_data_[0] = _msg.orientation().x(); + this->imu_sensor_data_[1] = _msg.orientation().y(); + this->imu_sensor_data_[2] = _msg.orientation().z(); + this->imu_sensor_data_[3] = _msg.orientation().w(); + this->imu_sensor_data_[4] = _msg.angular_velocity().x(); + this->imu_sensor_data_[5] = _msg.angular_velocity().y(); + this->imu_sensor_data_[6] = _msg.angular_velocity().z(); + this->imu_sensor_data_[7] = _msg.linear_acceleration().x(); + this->imu_sensor_data_[8] = _msg.linear_acceleration().y(); + this->imu_sensor_data_[9] = _msg.linear_acceleration().z(); +} + +class gz_quadruped_hardware::GazeboSimSystemPrivate { +public: + GazeboSimSystemPrivate() = default; + + ~GazeboSimSystemPrivate() = default; + + /// \brief Degrees od freedom. + size_t n_dof_; + + /// \brief last time the write method was called. + rclcpp::Time last_update_sim_time_ros_; + + /// \brief vector with the joint's names. + std::vector joints_; + + /// \brief vector with the imus . + std::vector > imus_; + + /// \brief state interfaces that will be exported to the Resource Manager + std::vector state_interfaces_; + + /// \brief command interfaces that will be exported to the Resource Manager + std::vector command_interfaces_; + + /// \brief Entity component manager, ECM shouldn't be accessed outside those + /// methods, otherwise the app will crash + sim::EntityComponentManager *ecm; + + /// \brief controller update rate + unsigned int update_rate; + + /// \brief Gazebo communication node. + GZ_TRANSPORT_NAMESPACE Node node; + + /// \brief Gain which converts position error to a velocity command + double position_proportional_gain_; +}; + +namespace gz_quadruped_hardware { + bool GazeboSimSystem::initSim( + rclcpp::Node::SharedPtr &model_nh, + std::map &enableJoints, + const hardware_interface::HardwareInfo &hardware_info, + sim::EntityComponentManager &_ecm, + unsigned int update_rate) { + this->dataPtr = std::make_unique(); + this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); + + this->nh_ = model_nh; + this->dataPtr->ecm = &_ecm; + this->dataPtr->n_dof_ = hardware_info.joints.size(); + + this->dataPtr->update_rate = update_rate; + + + RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_); + + this->dataPtr->joints_.resize(this->dataPtr->n_dof_); + + try { + this->dataPtr->position_proportional_gain_ = + this->nh_->get_parameter("position_proportional_gain").as_double(); + } catch (rclcpp::exceptions::ParameterUninitializedException &ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException &ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); + } catch (rclcpp::ParameterTypeException &ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'position_proportional_gain' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), + "Using default value: " << this->dataPtr->position_proportional_gain_); + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "The position_proportional_gain has been set to: " << + this->dataPtr->position_proportional_gain_); + + if (this->dataPtr->n_dof_ == 0) { + RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available"); + return false; + } + + for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { + auto &joint_info = hardware_info.joints[j]; + std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; + + auto it_joint = enableJoints.find(joint_name); + if (it_joint == enableJoints.end()) { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << + "' which is not in the gazebo model."); + continue; + } + + sim::Entity simjoint = enableJoints[joint_name]; + this->dataPtr->joints_[j].sim_joint = simjoint; + this->dataPtr->joints_[j].joint_type = _ecm.Component( + simjoint)->Data(); + this->dataPtr->joints_[j].joint_axis = _ecm.Component( + simjoint)->Data(); + + // Create joint position component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + sim::components::JointPosition().TypeId())) { + _ecm.CreateComponent(simjoint, sim::components::JointPosition()); + } + + // Create joint velocity component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + sim::components::JointVelocity().TypeId())) { + _ecm.CreateComponent(simjoint, sim::components::JointVelocity()); + } + + // Create joint transmitted wrench component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + sim::components::JointTransmittedWrench().TypeId())) { + _ecm.CreateComponent(simjoint, sim::components::JointTransmittedWrench()); + } + + // Accept this joint and continue configuration + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); + + // check if joint is mimicked + auto it = std::find_if( + hardware_info.mimic_joints.begin(), + hardware_info.mimic_joints.end(), + [j](const hardware_interface::MimicJoint &mj) { + return mj.joint_index == j; + }); + + if (it != hardware_info.mimic_joints.end()) { + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Joint '" << joint_name << "'is mimicking joint '" << + this->dataPtr->joints_[it->mimicked_joint_index].name << + "' with multiplier: " << it->multiplier << " and offset: " << it->offset); + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); + + auto get_initial_value = + [this, joint_name](const hardware_interface::InterfaceInfo &interface_info) { + double initial_value{0.0}; + if (!interface_info.initial_value.empty()) { + try { + initial_value = hardware_interface::stod(interface_info.initial_value); + RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value); + } catch (std::invalid_argument &) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "Failed converting initial_value string to real number for the joint " + << joint_name + << " and state interface " << interface_info.name + << ". Actual value of parameter: " << interface_info.initial_value + << ". Initial value will be set to 0.0"); + throw std::invalid_argument("Failed converting initial_value string"); + } + } + return initial_value; + }; + + double initial_position = std::numeric_limits::quiet_NaN(); + double initial_velocity = std::numeric_limits::quiet_NaN(); + double initial_effort = std::numeric_limits::quiet_NaN(); + + // register the state handles + for (unsigned int i = 0; i < joint_info.state_interfaces.size(); ++i) { + if (joint_info.state_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position); + initial_position = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_position = initial_position; + } + if (joint_info.state_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity); + initial_velocity = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_velocity = initial_velocity; + } + if (joint_info.state_interfaces[i].name == "effort") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort); + initial_effort = get_initial_value(joint_info.state_interfaces[i]); + this->dataPtr->joints_[j].joint_effort = initial_effort; + } + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); + + // register the command handles + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); ++i) { + if (joint_info.command_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position_cmd); + if (!std::isnan(initial_position)) { + this->dataPtr->joints_[j].joint_position_cmd = initial_position; + } + } else if (joint_info.command_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity_cmd); + if (!std::isnan(initial_velocity)) { + this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; + } + } else if (joint_info.command_interfaces[i].name == "effort") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort_cmd); + if (!std::isnan(initial_effort)) { + this->dataPtr->joints_[j].joint_effort_cmd = initial_effort; + } + } else if (joint_info.command_interfaces[i].name == "kp") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t kp"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + "kp", + &this->dataPtr->joints_[j].joint_kp_cmd); + this->dataPtr->joints_[j].joint_kp_cmd = 0.0; + } else if (joint_info.command_interfaces[i].name == "kd") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t kd"); + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + "kd", + &this->dataPtr->joints_[j].joint_kd_cmd); + this->dataPtr->joints_[j].joint_kd_cmd = 0.0; + } + + // independently of existence of command interface set initial value if defined + if (!std::isnan(initial_position)) { + this->dataPtr->joints_[j].joint_position = initial_position; + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[j].sim_joint, + sim::components::JointPositionReset({initial_position})); + } + if (!std::isnan(initial_velocity)) { + this->dataPtr->joints_[j].joint_velocity = initial_velocity; + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[j].sim_joint, + sim::components::JointVelocityReset({initial_velocity})); + } + } + + // check if joint is actuated (has command interfaces) or passive + this->dataPtr->joints_[j].is_actuated = (joint_info.command_interfaces.size() > 0); + } + + registerSensors(hardware_info); + + return true; + } + + void GazeboSimSystem::registerSensors( + const hardware_interface::HardwareInfo &hardware_info) { + // Collect gazebo sensor handles + size_t n_sensors = hardware_info.sensors.size(); + std::vector sensor_components_; + + for (unsigned int j = 0; j < n_sensors; j++) { + hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + sensor_components_.push_back(component); + } + // This is split in two steps: Count the number and type of sensor and associate the interfaces + // So we have resize only once the structures where the data will be stored, and we can safely + // use pointers to the structures + + this->dataPtr->ecm->Each( + [&](const sim::Entity &_entity, + const sim::components::Imu *, + const sim::components::Name *_name) -> bool { + auto imuData = std::make_shared(); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(_entity); + if (sensorTopicComp) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + imuData->name = _name->Data(); + imuData->sim_imu_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + for (auto &comp: sensor_components_) { + if (comp.name == _name->Data()) { + component = comp; + } + } + + static const std::map interface_name_map = { + {"orientation.x", 0}, + {"orientation.y", 1}, + {"orientation.z", 2}, + {"orientation.w", 3}, + {"angular_velocity.x", 4}, + {"angular_velocity.y", 5}, + {"angular_velocity.z", 6}, + {"linear_acceleration.x", 7}, + {"linear_acceleration.y", 8}, + {"linear_acceleration.z", 9}, + }; + + for (const auto &state_interface: component.state_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); + + size_t data_index = interface_name_map.at(state_interface.name); + this->dataPtr->state_interfaces_.emplace_back( + imuData->name, + state_interface.name, + &imuData->imu_sensor_data_[data_index]); + } + this->dataPtr->imus_.push_back(imuData); + return true; + }); + } + + CallbackReturn + GazeboSimSystem::on_init(const hardware_interface::HardwareInfo &info) { + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn GazeboSimSystem::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_INFO( + this->nh_->get_logger(), "System Successfully configured!"); + + return CallbackReturn::SUCCESS; + } + + std::vector + GazeboSimSystem::export_state_interfaces() { + return std::move(this->dataPtr->state_interfaces_); + } + + std::vector + GazeboSimSystem::export_command_interfaces() { + return std::move(this->dataPtr->command_interfaces_); + } + + CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State &previous_state) { + return CallbackReturn::SUCCESS; + } + + CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State &previous_state) { + return CallbackReturn::SUCCESS; + } + + hardware_interface::return_type GazeboSimSystem::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + continue; + } + + // Get the joint velocity + const auto *jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + // Get the joint force via joint transmitted wrench + const auto *jointWrench = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + // Get the joint position + const auto *jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; + this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; + gz::physics::Vector3d force_or_torque; + if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) { + force_or_torque = { + jointWrench->Data().force().x(), + jointWrench->Data().force().y(), + jointWrench->Data().force().z() + }; + } else { + // REVOLUTE and CONTINUOUS + force_or_torque = { + jointWrench->Data().torque().x(), + jointWrench->Data().torque().y(), + jointWrench->Data().torque().z() + }; + } + // Calculate the scalar effort along the joint axis + this->dataPtr->joints_[i].joint_effort = force_or_torque.dot( + gz::physics::Vector3d{ + this->dataPtr->joints_[i].joint_axis.Xyz()[0], + this->dataPtr->joints_[i].joint_axis.Xyz()[1], + this->dataPtr->joints_[i].joint_axis.Xyz()[2] + }); + } + + for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { + if (this->dataPtr->imus_[i]->topicName.empty()) { + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); + if (sensorTopicComp) { + this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->imus_[i]->topicName, &ImuData::OnIMU, + this->dataPtr->imus_[i].get()); + } + } + } + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type GazeboSimSystem::write( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + continue; + } + if (!this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint)) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[i].sim_joint, + sim::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + const double torque = this->dataPtr->joints_[i].joint_effort_cmd + + this->dataPtr->joints_[i].joint_kp_cmd * ( + this->dataPtr->joints_[i].joint_position_cmd - + this->dataPtr->joints_[i].joint_position) + + + this->dataPtr->joints_[i].joint_kd_cmd * ( + this->dataPtr->joints_[i].joint_velocity_cmd - + this->dataPtr->joints_[i].joint_velocity); + + *jointEffortCmd = sim::components::JointForceCmd( + {torque}); + } + } + + // set values of all mimic joints with respect to mimicked joint + for (const auto &mimic_joint: this->info_.mimic_joints) { + // Get the joint position + double position_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + + double position_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + + double position_error = + position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; + + double velocity_sp = (-1.0) * position_error * this->dataPtr->update_rate; + + auto vel = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + sim::components::JointVelocityCmd({velocity_sp})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = velocity_sp; + } + } + + return hardware_interface::return_type::OK; + } +} // namespace gz_quadruped_hardware + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + gz_quadruped_hardware::GazeboSimSystem, gz_quadruped_hardware::GazeboSimSystemInterface) diff --git a/libraries/gz_quadruped_playground/README.md b/libraries/gz_quadruped_playground/README.md index 56e2150..c0e712b 100644 --- a/libraries/gz_quadruped_playground/README.md +++ b/libraries/gz_quadruped_playground/README.md @@ -14,16 +14,18 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install ``` ## Launch Simulation + * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch gz_quadruped_playground gazebo.launch.py controller:=unitree_guide ``` -* Unitree Guide Controller +* OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 ``` ## Related Materials + * [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details) \ No newline at end of file diff --git a/libraries/gz_quadruped_playground/launch/ocs2.launch.py b/libraries/gz_quadruped_playground/launch/ocs2.launch.py index c4c231e..ffd591e 100644 --- a/libraries/gz_quadruped_playground/launch/ocs2.launch.py +++ b/libraries/gz_quadruped_playground/launch/ocs2.launch.py @@ -19,13 +19,6 @@ def generate_launch_description(): "--controller-manager", "/controller_manager"] ) - leg_pd_controller = Node( - package="controller_manager", - executable="spawner", - arguments=["leg_pd_controller", - "--controller-manager", "/controller_manager"] - ) - ocs2_controller = Node( package="controller_manager", executable="spawner", @@ -34,11 +27,17 @@ def generate_launch_description(): return LaunchDescription([ - leg_pd_controller, + joint_state_publisher, RegisterEventHandler( event_handler=OnProcessExit( - target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, ocs2_controller], + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=imu_sensor_broadcaster, + on_exit=[ocs2_controller], ) ), ]) diff --git a/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py index 29b972d..1530ced 100644 --- a/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py +++ b/libraries/gz_quadruped_playground/launch/unitree_guide.launch.py @@ -19,13 +19,6 @@ def generate_launch_description(): "--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", @@ -33,11 +26,17 @@ def generate_launch_description(): ) return LaunchDescription([ - leg_pd_controller, + joint_state_publisher, RegisterEventHandler( event_handler=OnProcessExit( - target_action=leg_pd_controller, - on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=imu_sensor_broadcaster, + on_exit=[unitree_guide_controller], ) ), ])