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

tried to add foot force sensor

This commit is contained in:
Huang Zhenbiao 2025-02-26 01:18:37 +08:00
parent 1f1dfce57d
commit d8c4d8eee5
10 changed files with 883 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
.clang-tidy Normal file
View File

@ -0,0 +1,147 @@
# Generated from CLion Inspection settings
---
Checks: '-*,
bugprone-argument-comment,
bugprone-assert-side-effect,
bugprone-bad-signal-to-kill-thread,
bugprone-branch-clone,
bugprone-copy-constructor-init,
bugprone-dangling-handle,
bugprone-dynamic-static-initializers,
bugprone-fold-init-type,
bugprone-forward-declaration-namespace,
bugprone-forwarding-reference-overload,
bugprone-inaccurate-erase,
bugprone-incorrect-roundings,
bugprone-integer-division,
bugprone-lambda-function-name,
bugprone-macro-parentheses,
bugprone-macro-repeated-side-effects,
bugprone-misplaced-operator-in-strlen-in-alloc,
bugprone-misplaced-pointer-arithmetic-in-alloc,
bugprone-misplaced-widening-cast,
bugprone-move-forwarding-reference,
bugprone-multiple-statement-macro,
bugprone-no-escape,
bugprone-parent-virtual-call,
bugprone-posix-return,
bugprone-reserved-identifier,
bugprone-sizeof-container,
bugprone-sizeof-expression,
bugprone-spuriously-wake-up-functions,
bugprone-string-constructor,
bugprone-string-integer-assignment,
bugprone-string-literal-with-embedded-nul,
bugprone-suspicious-enum-usage,
bugprone-suspicious-include,
bugprone-suspicious-memset-usage,
bugprone-suspicious-missing-comma,
bugprone-suspicious-semicolon,
bugprone-suspicious-string-compare,
bugprone-suspicious-memory-comparison,
bugprone-suspicious-realloc-usage,
bugprone-swapped-arguments,
bugprone-terminating-continue,
bugprone-throw-keyword-missing,
bugprone-too-small-loop-variable,
bugprone-undefined-memory-manipulation,
bugprone-undelegated-constructor,
bugprone-unhandled-self-assignment,
bugprone-unused-raii,
bugprone-unused-return-value,
bugprone-use-after-move,
bugprone-virtual-near-miss,
cert-dcl21-cpp,
cert-dcl58-cpp,
cert-err34-c,
cert-err52-cpp,
cert-err60-cpp,
cert-flp30-c,
cert-msc50-cpp,
cert-msc51-cpp,
cert-str34-c,
cppcoreguidelines-interfaces-global-init,
cppcoreguidelines-narrowing-conversions,
cppcoreguidelines-pro-type-member-init,
cppcoreguidelines-pro-type-static-cast-downcast,
cppcoreguidelines-slicing,
google-default-arguments,
google-explicit-constructor,
google-runtime-operator,
hicpp-exception-baseclass,
hicpp-multiway-paths-covered,
misc-misplaced-const,
misc-new-delete-overloads,
misc-no-recursion,
misc-non-copyable-objects,
misc-throw-by-value-catch-by-reference,
misc-unconventional-assign-operator,
misc-uniqueptr-reset-release,
modernize-avoid-bind,
modernize-concat-nested-namespaces,
modernize-deprecated-headers,
modernize-deprecated-ios-base-aliases,
modernize-loop-convert,
modernize-make-shared,
modernize-make-unique,
modernize-pass-by-value,
modernize-raw-string-literal,
modernize-redundant-void-arg,
modernize-replace-auto-ptr,
modernize-replace-disallow-copy-and-assign-macro,
modernize-replace-random-shuffle,
modernize-return-braced-init-list,
modernize-shrink-to-fit,
modernize-unary-static-assert,
modernize-use-auto,
modernize-use-bool-literals,
modernize-use-emplace,
modernize-use-equals-default,
modernize-use-equals-delete,
modernize-use-nodiscard,
modernize-use-noexcept,
modernize-use-nullptr,
modernize-use-override,
modernize-use-transparent-functors,
modernize-use-uncaught-exceptions,
mpi-buffer-deref,
mpi-type-mismatch,
openmp-use-default-none,
performance-faster-string-find,
performance-for-range-copy,
performance-implicit-conversion-in-loop,
performance-inefficient-algorithm,
performance-inefficient-string-concatenation,
performance-inefficient-vector-operation,
performance-move-const-arg,
performance-move-constructor-init,
performance-no-automatic-move,
performance-noexcept-move-constructor,
performance-trivially-destructible,
performance-type-promotion-in-math-fn,
performance-unnecessary-copy-initialization,
performance-unnecessary-value-param,
portability-simd-intrinsics,
readability-avoid-const-params-in-decls,
readability-const-return-type,
readability-container-size-empty,
readability-convert-member-functions-to-static,
readability-delete-null-pointer,
readability-deleted-default,
readability-inconsistent-declaration-parameter-name,
readability-make-member-function-const,
readability-misleading-indentation,
readability-misplaced-array-index,
readability-non-const-parameter,
readability-redundant-control-flow,
readability-redundant-declaration,
readability-redundant-function-ptr-dereference,
readability-redundant-smartptr-get,
readability-redundant-string-cstr,
readability-redundant-string-init,
readability-simplify-subscript-expr,
readability-static-accessed-through-instance,
readability-static-definition-in-anonymous-namespace,
readability-string-compare,
readability-uniqueptr-delete-release,
readability-use-anyofallof'

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

@ -75,7 +75,12 @@ ocs2_quadruped_controller:
- linear_acceleration.y
- linear_acceleration.z
estimator_type: "odom_topic"
foot_force_name: "foot_force"
foot_force_interfaces:
- FL_ft_sensor
- RL_ft_sensor
- FR_ft_sensor
- RR_ft_sensor
unitree_guide_controller:
ros__parameters:

22
descriptions/unitree/go2_description/xacro/ft_sensor.xacro Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ft_sensor" params="name">
<gazebo reference="${name}_foot_fixed">
<!-- Enable feedback for this joint -->
<provideFeedback>true</provideFeedback>
<!-- Prevent ros2_control from lumping this fixed joint with others -->
<disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_ft_sensor" type="force_torque">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>${name}_ft</topic>
<force_torque>
<frame>child</frame>
<!-- <measure_direction>child_to_parent</measure_direction>-->
</force_torque>
</sensor>
</gazebo>
</xacro:macro>
</robot>

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

@ -1,279 +1,294 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware>
<xacro:include filename="$(find go2_description)/xacro/ft_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware>
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<joint name="FR_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
</ros2_control>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</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>1000</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>
<sensor name="foot_force">
<state_interface name="FR_ft_sensor"/>
<state_interface name="FL_ft_sensor"/>
<state_interface name="RR_ft_sensor"/>
<state_interface name="RL_ft_sensor"/>
</sensor>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
</ros2_control>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
</image>
<clip>
<near>0.1</near>
<far>15</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<optical_frame_id>front_camera</optical_frame_id>
<camera_info_topic>camera/camera_info</camera_info_topic>
</camera>
<always_on>true</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<topic>camera/image</topic>
</sensor>
<gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
<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>1000</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>
</xacro:if>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="FL"/>
<xacro:ft_sensor name="RR"/>
<xacro:ft_sensor name="RL"/>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">
<sensor name="front_camera" type="camera">
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
</image>
<clip>
<near>0.1</near>
<far>15</far>
</clip>
<noise>
<type>gaussian</type>
<!-- Noise is sampled independently per pixel on each frame.
That pixel's noise value is added to each of its color
channels, which at that point lie in the range [0,1]. -->
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
<optical_frame_id>front_camera</optical_frame_id>
<camera_info_topic>camera/camera_info</camera_info_topic>
</camera>
<always_on>true</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<topic>camera/image</topic>
</sensor>
</gazebo>
</xacro:if>
</robot>

358
descriptions/unitree/go2_description/xacro/leg.xacro
View File

@ -2,202 +2,202 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">0
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<gazebo reference="${name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="${name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

8
hardwares/gz_quadruped_hardware/README.md Normal file
View File

@ -0,0 +1,8 @@
# Gazebo Quadruped ROS2 Control Plugin
This repository is a modified version of [gz_ros2_control](https://github.com/ros-controls/gz_ros2_control)
```bash
cd ~/ros2_ws
colcon build --packages-up-to gz_quadruped_hardware --symlink-install
```

373
hardwares/gz_quadruped_hardware/src/gz_system.cpp
View File

@ -15,6 +15,7 @@
#include "gz_quadruped_hardware/gz_system.hpp"
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/wrench.pb.h>
#include <limits>
#include <map>
@ -32,7 +33,7 @@
#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/ForceTorque.hh>
#include <gz/sim/components/JointVelocity.hh>
#include <gz/sim/components/JointVelocityReset.hh>
#include <gz/sim/components/Name.hh>
@ -46,7 +47,8 @@
#include <hardware_interface/lexical_casts.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
struct jointData {
struct jointData
{
/// \brief Joint's names.
std::string name;
@ -88,7 +90,8 @@ struct jointData {
gz_quadruped_hardware::GazeboSimSystemInterface::ControlMethod joint_control_method;
};
class ImuData {
class ImuData
{
public:
/// \brief imu's name.
std::string name{};
@ -103,10 +106,11 @@ public:
std::array<double, 10> imu_sensor_data_;
/// \brief callback to get the IMU topic values
void OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg);
void OnIMU(const GZ_MSGS_NAMESPACE IMU& _msg);
};
void ImuData::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();
@ -119,7 +123,41 @@ void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg) {
this->imu_sensor_data_[9] = _msg.linear_acceleration().z();
}
class gz_quadruped_hardware::GazeboSimSystemPrivate {
class ForceTorqueData
{
public:
/// \brief force-torque sensor's name.
std::string name{};
/// \brief force-torque sensor's topic name.
std::string topicName{};
/// \brief handles to the force torque from within Gazebo
sim::Entity sim_ft_sensors_ = sim::kNullEntity;
/// \brief An array per FT
std::array<double, 6> ft_sensor_data_;
/// \brief Current foot end effort
double foot_effort;
/// \brief callback to get the Force Torque topic values
void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench& _msg);
};
void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench& _msg)
{
this->ft_sensor_data_[0] = _msg.force().x();
this->ft_sensor_data_[1] = _msg.force().y();
this->ft_sensor_data_[2] = _msg.force().z();
this->ft_sensor_data_[3] = _msg.torque().x();
this->ft_sensor_data_[4] = _msg.torque().y();
this->ft_sensor_data_[5] = _msg.torque().z();
this->foot_effort = sqrt(pow(_msg.force().x(), 2) + pow(_msg.force().y(), 2) + pow(_msg.force().z(), 2));
}
class gz_quadruped_hardware::GazeboSimSystemPrivate
{
public:
GazeboSimSystemPrivate() = default;
@ -135,7 +173,10 @@ public:
std::vector<jointData> joints_;
/// \brief vector with the imus .
std::vector<std::shared_ptr<ImuData> > imus_;
std::vector<std::shared_ptr<ImuData>> imus_;
/// \brief vector with the foot force-torque sensors.
std::vector<std::shared_ptr<ForceTorqueData>> ft_sensors_;
/// \brief state interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::StateInterface> state_interfaces_;
@ -145,25 +186,24 @@ public:
/// \brief Entity component manager, ECM shouldn't be accessed outside those
/// methods, otherwise the app will crash
sim::EntityComponentManager *ecm;
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 {
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) {
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();
@ -178,48 +218,20 @@ namespace gz_quadruped_hardware {
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) {
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];
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()) {
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.");
@ -236,21 +248,24 @@ namespace gz_quadruped_hardware {
// Create joint position component if one doesn't exist
if (!_ecm.EntityHasComponentType(
simjoint,
sim::components::JointPosition().TypeId())) {
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())) {
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())) {
sim::components::JointTransmittedWrench().TypeId()))
{
_ecm.CreateComponent(simjoint, sim::components::JointTransmittedWrench());
}
@ -261,11 +276,13 @@ namespace gz_quadruped_hardware {
auto it = std::find_if(
hardware_info.mimic_joints.begin(),
hardware_info.mimic_joints.end(),
[j](const hardware_interface::MimicJoint &mj) {
[j](const hardware_interface::MimicJoint& mj)
{
return mj.joint_index == j;
});
if (it != hardware_info.mimic_joints.end()) {
if (it != hardware_info.mimic_joints.end())
{
RCLCPP_INFO_STREAM(
this->nh_->get_logger(),
"Joint '" << joint_name << "'is mimicking joint '" <<
@ -276,13 +293,18 @@ namespace gz_quadruped_hardware {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
auto get_initial_value =
[this, joint_name](const hardware_interface::InterfaceInfo &interface_info) {
[this, joint_name](const hardware_interface::InterfaceInfo& interface_info)
{
double initial_value{0.0};
if (!interface_info.initial_value.empty()) {
try {
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 &) {
}
catch (std::invalid_argument&)
{
RCLCPP_ERROR_STREAM(
this->nh_->get_logger(),
"Failed converting initial_value string to real number for the joint "
@ -301,8 +323,10 @@ namespace gz_quadruped_hardware {
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") {
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,
@ -311,7 +335,8 @@ namespace gz_quadruped_hardware {
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") {
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,
@ -320,7 +345,8 @@ namespace gz_quadruped_hardware {
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") {
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,
@ -334,42 +360,55 @@ namespace gz_quadruped_hardware {
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") {
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)) {
if (!std::isnan(initial_position))
{
this->dataPtr->joints_[j].joint_position_cmd = initial_position;
}
} else if (joint_info.command_interfaces[i].name == "velocity") {
}
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)) {
if (!std::isnan(initial_velocity))
{
this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity;
}
} else if (joint_info.command_interfaces[i].name == "effort") {
}
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)) {
if (!std::isnan(initial_effort))
{
this->dataPtr->joints_[j].joint_effort_cmd = initial_effort;
}
} else if (joint_info.command_interfaces[i].name == "kp") {
}
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") {
}
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,
@ -379,13 +418,15 @@ namespace gz_quadruped_hardware {
}
// independently of existence of command interface set initial value if defined
if (!std::isnan(initial_position)) {
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)) {
if (!std::isnan(initial_velocity))
{
this->dataPtr->joints_[j].joint_velocity = initial_velocity;
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[j].sim_joint,
@ -403,12 +444,14 @@ namespace gz_quadruped_hardware {
}
void GazeboSimSystem::registerSensors(
const hardware_interface::HardwareInfo &hardware_info) {
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++) {
for (unsigned int j = 0; j < n_sensors; j++)
{
hardware_interface::ComponentInfo component = hardware_info.sensors[j];
sensor_components_.push_back(component);
}
@ -416,17 +459,20 @@ namespace gz_quadruped_hardware {
// So we have resize only once the structures where the data will be stored, and we can safely
// use pointers to the structures
// IMU Sensors
this->dataPtr->ecm->Each<sim::components::Imu,
sim::components::Name>(
[&](const sim::Entity &_entity,
const sim::components::Imu *,
const sim::components::Name *_name) -> bool {
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) {
if (sensorTopicComp)
{
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data());
}
@ -436,8 +482,10 @@ namespace gz_quadruped_hardware {
imuData->sim_imu_sensors_ = _entity;
hardware_interface::ComponentInfo component;
for (auto &comp: sensor_components_) {
if (comp.name == _name->Data()) {
for (auto& comp : sensor_components_)
{
if (comp.name == _name->Data())
{
component = comp;
}
}
@ -455,7 +503,8 @@ namespace gz_quadruped_hardware {
{"linear_acceleration.z", 9},
};
for (const auto &state_interface: component.state_interfaces) {
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);
@ -467,75 +516,111 @@ namespace gz_quadruped_hardware {
this->dataPtr->imus_.push_back(imuData);
return true;
});
// Foot Force torque sensor
this->dataPtr->ecm->Each<sim::components::ForceTorque,
sim::components::Name>(
[&](const sim::Entity& _entity,
const sim::components::ForceTorque*,
const sim::components::Name* _name) -> bool
{
auto ftData = std::make_shared<ForceTorqueData>();
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading Foot Force 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());
}
ftData->name = _name->Data();
ftData->sim_ft_sensors_ = _entity;
this->dataPtr->state_interfaces_.emplace_back(
"foot_force",
ftData->name,
&ftData->foot_effort);
this->dataPtr->ft_sensors_.push_back(ftData);
return true;
});
}
CallbackReturn GazeboSimSystem::on_init(const hardware_interface::HardwareInfo &info) {
if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
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*/) {
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() {
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() {
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) {
CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State& previous_state)
{
return CallbackReturn::SUCCESS;
}
CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State &previous_state) {
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) {
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);
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);
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);
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) {
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 {
}
else
{
// REVOLUTE and CONTINUOUS
force_or_torque = {
jointWrench->Data().torque().x(),
@ -552,11 +637,14 @@ namespace gz_quadruped_hardware {
});
}
for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) {
if (this->dataPtr->imus_[i]->topicName.empty()) {
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) {
if (sensorTopicComp)
{
this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data();
RCLCPP_INFO_STREAM(
this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name <<
@ -568,34 +656,61 @@ namespace gz_quadruped_hardware {
}
}
}
for (unsigned int i = 0; i < this->dataPtr->ft_sensors_.size(); ++i)
{
if (this->dataPtr->ft_sensors_[i]->topicName.empty())
{
auto sensorTopicComp = this->dataPtr->ecm->Component<
sim::components::SensorTopic>(this->dataPtr->ft_sensors_[i]->sim_ft_sensors_);
if (sensorTopicComp)
{
this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data();
RCLCPP_INFO_STREAM(
this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name <<
" has a topic name: " << sensorTopicComp->Data());
this->dataPtr->node.Subscribe(
this->dataPtr->ft_sensors_[i]->topicName, &ForceTorqueData::OnForceTorque,
this->dataPtr->ft_sensors_[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) {
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->joints_[i].sim_joint))
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[i].sim_joint,
sim::components::JointForceCmd({0}));
} else {
}
else
{
const auto jointEffortCmd =
this->dataPtr->ecm->Component<sim::components::JointForceCmd>(
this->dataPtr->joints_[i].sim_joint);
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);
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});

6
libraries/gz_quadruped_playground/launch/gazebo.launch.py
View File

@ -120,9 +120,9 @@ def generate_launch_description():
arguments=[
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
"/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
"/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
"/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
"/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
# "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
# "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
# "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
],
output="screen",
parameters=[

2
libraries/gz_quadruped_playground/package.xml
View File

@ -10,8 +10,8 @@
<exec_depend>xacro</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>leg_pd_controller</exec_depend>
<exec_depend>imu_sensor_broadcaster</exec_depend>
<exec_depend>gz_quadruped_hardware</exec_depend>
<export>
<build_type>ament_cmake</build_type>

4
libraries/gz_quadruped_playground/worlds/default.sdf
View File

@ -82,10 +82,6 @@ gz service -s /world/empty/create \
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-contact-system"
name="gz::sim::systems::Contact">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>