EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add gz quadruped hardware

This commit is contained in:
Huang Zhenbiao 2025-02-25 22:18:55 +08:00
parent 69e9e1f477
commit c3797c8d6e
21 changed files with 2147 additions and 293 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h
View File

@ -4,6 +4,7 @@
#pragma once #pragma once
#include "StateEstimateBase.h" #include "StateEstimateBase.h"
#include <realtime_tools/realtime_buffer.h>
namespace ocs2::legged_robot namespace ocs2::legged_robot
{ {
@ -17,7 +18,6 @@ namespace ocs2::legged_robot
protected: protected:
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_; rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
vector3_t position_; realtime_tools::RealtimeBuffer<nav_msgs::msg::Odometry> buffer_;
vector3_t linear_velocity_;
}; };
}; };

8
controllers/ocs2_quadruped_controller/launch/gazebo.launch.py
View File

@ -101,7 +101,13 @@ def launch_setup(context, *args, **kwargs):
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=leg_pd_controller, 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],
) )
), ),
] ]

50
controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp
View File

@ -4,38 +4,38 @@
#include "ocs2_quadruped_controller/estimator/FromOdomTopic.h" #include "ocs2_quadruped_controller/estimator/FromOdomTopic.h"
namespace ocs2::legged_robot namespace ocs2::legged_robot {
{
FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent &ctrl_component, FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node) : const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) : StateEstimateBase(
StateEstimateBase(
std::move(info), ctrl_component, std::move(info), ctrl_component,
node) node) {
{
odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>( odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) "/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {
{ buffer_.writeFromNonRT(*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,
};
}); });
} }
vector_t FromOdomTopic::update(const rclcpp::Time& time, const rclcpp::Duration& period) vector_t FromOdomTopic::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
{ const nav_msgs::msg::Odometry odom = *buffer_.readFromRT();
updateJointStates();
updateImu();
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<scalar_t, 3, 1>(
odom.twist.twist.angular.x,
odom.twist.twist.angular.y,
odom.twist.twist.angular.z));
updateLinear(Eigen::Matrix<scalar_t, 3, 1>(
odom.pose.pose.position.x,
odom.pose.pose.position.y,
odom.pose.pose.position.z),
Eigen::Matrix<scalar_t, 3, 1>(
odom.twist.twist.linear.x,
odom.twist.twist.linear.y,
odom.twist.twist.linear.z));
return rbd_state_; return rbd_state_;
} }
} }

1
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp
View File

@ -68,6 +68,7 @@ namespace ocs2::legged_robot {
const vector3_t zyx = quatToZyx(quat_) - zyx_offset_; const vector3_t zyx = quatToZyx(quat_) - zyx_offset_;
const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>( const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(
zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat_), angular_vel_local_)); zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(quatToZyx(quat_), angular_vel_local_));
updateAngular(zyx, angularVelGlobal); updateAngular(zyx, angularVelGlobal);
} }

5
descriptions/unitree/a1_description/README.md
View File

@ -60,6 +60,11 @@ ros2 launch a1_description visualize.launch.py
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=a1_description height:=0.43 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 * RL Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash

57
descriptions/unitree/a1_description/config/gazebo.yaml
View File

@ -16,6 +16,9 @@ controller_manager:
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
ocs2_quadruped_controller:
type: ocs2_quadruped_controller/Ocs2QuadrupedController
rl_quadruped_controller: rl_quadruped_controller:
type: rl_quadruped_controller/LeggedGymController type: rl_quadruped_controller/LeggedGymController
@ -98,6 +101,60 @@ unitree_guide_controller:
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - 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: rl_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz

11
descriptions/unitree/a1_description/xacro/gazebo.xacro
View File

@ -111,6 +111,16 @@
<plugin filename="gz-sim-imu-system" <plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu"> name="gz::sim::systems::Imu">
</plugin> </plugin>
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>500</odom_publish_frequency>
<odom_topic>odom</odom_topic>
<dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
<tf_topic>tf</tf_topic>
</plugin>
</gazebo> </gazebo>
<gazebo reference="trunk"> <gazebo reference="trunk">
@ -184,5 +194,4 @@
<gazebo reference="imu_joint"> <gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping> <disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo> </gazebo>
</robot> </robot>

29
descriptions/unitree/go2_description/config/gazebo.yaml
View File

@ -10,9 +10,6 @@ controller_manager:
imu_sensor_broadcaster: imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
@ -27,35 +24,10 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" 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: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 100 # Hz
robot_pkg: "go2_description" robot_pkg: "go2_description"
command_prefix: "leg_pd_controller"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -108,7 +80,6 @@ ocs2_quadruped_controller:
unitree_guide_controller: unitree_guide_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz
command_prefix: "leg_pd_controller"
joints: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint

79
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -2,88 +2,137 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<ros2_control name="GazeboSystem" type="system"> <ros2_control name="GazeboSystem" type="system">
<hardware> <hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin> <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_thigh_joint"> <joint name="FR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_calf_joint"> <joint name="FR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_hip_joint"> <joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_thigh_joint"> <joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_calf_joint"> <joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_hip_joint"> <joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_thigh_joint"> <joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_calf_joint"> <joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_hip_joint"> <joint name="RL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_thigh_joint"> <joint name="RL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_calf_joint"> <joint name="RL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
@ -104,7 +153,7 @@
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin"> <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters> <parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin> </plugin>
<plugin filename="gz-sim-imu-system" <plugin filename="gz-sim-imu-system"
@ -114,7 +163,7 @@
name="gz::sim::systems::OdometryPublisher"> name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame> <odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame> <robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>200</odom_publish_frequency> <odom_publish_frequency>1000</odom_publish_frequency>
<odom_topic>odom</odom_topic> <odom_topic>odom</odom_topic>
<dimensions>3</dimensions> <dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic> <odom_covariance_topic>odom_with_covariance</odom_covariance_topic>

98
hardwares/gz_quadruped_hardware/CMakeLists.txt Normal file
View File

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

191
hardwares/gz_quadruped_hardware/LICENSE Normal file
View File

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

10
hardwares/gz_quadruped_hardware/gz_quadruped_hardware.xml Normal file
View File

@ -0,0 +1,10 @@
<library path="gz_quadruped_plugins">
<class
name="gz_quadruped_hardware/GazeboSimSystem"
type="gz_quadruped_hardware::GazeboSimSystem"
base_class_type="gz_quadruped_hardware::GazeboSimSystemInterface">
<description>
Gazebo ros2_control plugin for quadruped robot.
</description>
</class>
</library>

57
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp Normal file
View File

@ -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 <memory>
#include <gz/sim/System.hh>
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<const sdf::Element> &_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<GazeboSimQuadrupedPluginPrivate> dataPtr;
};
} // namespace gz_ros2_control

83
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp Normal file
View File

@ -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 <map>
#include <memory>
#include <string>
#include <vector>
#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<hardware_interface::StateInterface> export_state_interfaces() override;
// Documentation Inherited
std::vector<hardware_interface::CommandInterface> 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<std::string, sim::Entity> &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<GazeboSimSystemPrivate> dataPtr;
};
} // namespace gz_ros2_control

125
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp Normal file
View File

@ -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 <map>
#include <memory>
#include <string>
#include <gz/sim/System.hh>
namespace sim = gz::sim;
#include <hardware_interface/system_interface.hpp>
#include <rclcpp/rclcpp.hpp>
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<enum ControlMethod_> 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<class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::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<std::string, sim::Entity> &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_> ControlMethod;
protected:
rclcpp::Node::SharedPtr nh_;
};
} // namespace gz_ros2_control

30
hardwares/gz_quadruped_hardware/package.xml Normal file
View File

@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="3">
<name>gz_quadruped_hardware</name>
<version>2.0.6</version>
<description>Gazebo ros2_control package for quadruped robot.</description>
<maintainer email="alejandro@openrobotics.com">Alejandro Hernández</maintainer>
<maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
<author>Alejandro Hernández</author>
<license>Apache 2</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>ament_index_cpp</depend>
<!-- default version to use in official ROS 2 packages is Gazebo Harmonic for ROS 2 Rolling -->
<depend>gz_sim_vendor</depend>
<depend>gz_plugin_vendor</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>yaml_cpp_vendor</depend>
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

522
hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp Normal file
View File

@ -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 <unistd.h>
#include <chrono>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include <gz/sim/components/Joint.hh>
#include <gz/sim/components/JointType.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/World.hh>
#include <gz/sim/Model.hh>
#include <gz/plugin/Register.hh>
#include <controller_manager/controller_manager.hpp>
#include <hardware_interface/resource_manager.hpp>
#include <hardware_interface/component_parser.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_loader.hpp>
#include <rclcpp/rclcpp.hpp>
#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<std::string, sim::Entity> 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<GazeboSimSystemInterface> gzSimSystem;
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
try {
gzSimSystem = std::unique_ptr<GazeboSimSystemInterface>(
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<rclcpp::Node> node_;
sim::EntityComponentManager *ecm_;
std::map<std::string, sim::Entity> enabledJoints_;
/// \brief Interface loader
pluginlib::ClassLoader<GazeboSimSystemInterface> 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<std::string, sim::Entity> 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<rclcpp::Node> 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::ControllerManager>
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<std::string, sim::Entity>
GazeboSimQuadrupedPluginPrivate::GetEnabledJoints(
const sim::Entity &_entity,
sim::EntityComponentManager &_ecm) const {
std::map<std::string, sim::Entity> output;
std::vector<std::string> 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<sim::components::Name>(
jointEntity)->Data();
// Make sure the joint type is supported, i.e. it has exactly one
// actuated axis
const auto *jointType = _ecm.Component<sim::components::JointType>(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<GazeboSimQuadrupedPluginPrivate>()) {
}
//////////////////////////////////////////////////
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<const sdf::Element> &_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<std::string>("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<std::string> arguments = {"--ros-args"};
auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());
sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters");
while (argument_sdf) {
std::string argument = argument_sdf->Get<std::string>();
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>();
}
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<bool>();
}
double position_proportional_gain = 0.1; // default
if (sdfPtr->HasElement("position_proportional_gain")) {
position_proportional_gain =
sdfPtr->GetElement("position_proportional_gain")->Get<double>();
}
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<std::string>();
// 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<std::string>();
arguments.push_back(RCL_REMAP_FLAG);
arguments.push_back(argument);
argument_sdf = argument_sdf->GetNextElement("remapping");
}
}
}
std::vector<const char *> argv;
for (const auto &arg: arguments) {
argv.push_back(arg.data());
}
// Create a default context, if not already
if (!rclcpp::ok()) {
init(
static_cast<int>(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<rclcpp::executors::MultiThreadedExecutor>();
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<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<GZResourceManager>(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::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(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<std::chrono::nanoseconds>(
_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<std::chrono::nanoseconds>(
_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<GazeboSimSystemInterface>(
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)

640
hardwares/gz_quadruped_hardware/src/gz_system.cpp Normal file
View File

@ -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 <gz/msgs/imu.pb.h>
#include <limits>
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <gz/physics/Geometry.hh>
#include <gz/sim/components/AngularVelocity.hh>
#include <gz/sim/components/Imu.hh>
#include <gz/sim/components/JointAxis.hh>
#include <gz/sim/components/JointForceCmd.hh>
#include <gz/sim/components/JointPosition.hh>
#include <gz/sim/components/JointPositionReset.hh>
#include <gz/sim/components/JointTransmittedWrench.hh>
#include <gz/sim/components/JointType.hh>
#include <gz/sim/components/JointVelocityCmd.hh>
#include <gz/sim/components/JointVelocity.hh>
#include <gz/sim/components/JointVelocityReset.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/Sensor.hh>
#include <gz/transport/Node.hh>
#define GZ_TRANSPORT_NAMESPACE gz::transport::
#define GZ_MSGS_NAMESPACE gz::msgs::
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/lexical_casts.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
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<double, 10> 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<jointData> joints_;
/// \brief vector with the imus .
std::vector<std::shared_ptr<ImuData> > imus_;
/// \brief state interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::StateInterface> state_interfaces_;
/// \brief command interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::CommandInterface> 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<std::string, sim::Entity> &enableJoints,
const hardware_interface::HardwareInfo &hardware_info,
sim::EntityComponentManager &_ecm,
unsigned int update_rate) {
this->dataPtr = std::make_unique<GazeboSimSystemPrivate>();
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<sim::components::JointType>(
simjoint)->Data();
this->dataPtr->joints_[j].joint_axis = _ecm.Component<sim::components::JointAxis>(
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<double>::quiet_NaN();
double initial_velocity = std::numeric_limits<double>::quiet_NaN();
double initial_effort = std::numeric_limits<double>::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<hardware_interface::ComponentInfo> 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<sim::components::Imu,
sim::components::Name>(
[&](const sim::Entity &_entity,
const sim::components::Imu *,
const sim::components::Name *_name) -> bool {
auto imuData = std::make_shared<ImuData>();
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<std::string, size_t> 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<hardware_interface::StateInterface>
GazeboSimSystem::export_state_interfaces() {
return std::move(this->dataPtr->state_interfaces_);
}
std::vector<hardware_interface::CommandInterface>
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<sim::components::JointVelocity>(
this->dataPtr->joints_[i].sim_joint);
// Get the joint force via joint transmitted wrench
const auto *jointWrench =
this->dataPtr->ecm->Component<sim::components::JointTransmittedWrench>(
this->dataPtr->joints_[i].sim_joint);
// Get the joint position
const auto *jointPositions =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
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<sim::components::JointForceCmd>(
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<sim::components::JointForceCmd>(
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<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
double position_mimic_joint =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
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<sim::components::JointVelocityCmd>(
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)

4
libraries/gz_quadruped_playground/README.md
View File

@ -14,16 +14,18 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install
``` ```
## Launch Simulation ## Launch Simulation
* Unitree Guide Controller * Unitree Guide Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground gazebo.launch.py controller:=unitree_guide ros2 launch gz_quadruped_playground gazebo.launch.py controller:=unitree_guide
``` ```
* Unitree Guide Controller * OCS2 Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2
``` ```
## Related Materials ## Related Materials
* [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details) * [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details)

19
libraries/gz_quadruped_playground/launch/ocs2.launch.py
View File

@ -19,13 +19,6 @@ def generate_launch_description():
"--controller-manager", "/controller_manager"] "--controller-manager", "/controller_manager"]
) )
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"]
)
ocs2_controller = Node( ocs2_controller = Node(
package="controller_manager", package="controller_manager",
executable="spawner", executable="spawner",
@ -34,11 +27,17 @@ def generate_launch_description():
return LaunchDescription([ return LaunchDescription([
leg_pd_controller, joint_state_publisher,
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=leg_pd_controller, target_action=joint_state_publisher,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, ocs2_controller], on_exit=[imu_sensor_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=imu_sensor_broadcaster,
on_exit=[ocs2_controller],
) )
), ),
]) ])

19
libraries/gz_quadruped_playground/launch/unitree_guide.launch.py
View File

@ -19,13 +19,6 @@ def generate_launch_description():
"--controller-manager", "/controller_manager"] "--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( unitree_guide_controller = Node(
package="controller_manager", package="controller_manager",
executable="spawner", executable="spawner",
@ -33,11 +26,17 @@ def generate_launch_description():
) )
return LaunchDescription([ return LaunchDescription([
leg_pd_controller, joint_state_publisher,
RegisterEventHandler( RegisterEventHandler(
event_handler=OnProcessExit( event_handler=OnProcessExit(
target_action=leg_pd_controller, target_action=joint_state_publisher,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], on_exit=[imu_sensor_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=imu_sensor_broadcaster,
on_exit=[unitree_guide_controller],
) )
), ),
]) ])