214 lines
6.7 KiB
XML
214 lines
6.7 KiB
XML
<?xml version="1.0"?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
|
|
<ros2_control name="GazeboSystem" type="system">
|
|
<hardware>
|
|
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
|
</hardware>
|
|
|
|
<joint name="FR_abad_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="FR_hip_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="FR_knee_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="FL_abad_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="FL_hip_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="FL_knee_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RR_abad_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RR_hip_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RR_knee_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RL_abad_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RL_hip_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
|
|
<joint name="RL_knee_joint">
|
|
<command_interface name="effort" initial_value="0.0"/>
|
|
<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>
|
|
|
|
<sensor name="LF_foot_end">
|
|
<state_interface name="force.x"/>
|
|
<state_interface name="force.y"/>
|
|
<state_interface name="force.z"/>
|
|
<param name="frame_id">LF_FOOT</param>
|
|
</sensor>
|
|
<sensor name="RF_foot_end">
|
|
<state_interface name="force.x"/>
|
|
<state_interface name="force.y"/>
|
|
<state_interface name="force.z"/>
|
|
<param name="frame_id">RF_FOOT</param>
|
|
</sensor>
|
|
<sensor name="LH_foot_end">
|
|
<state_interface name="force.x"/>
|
|
<state_interface name="force.y"/>
|
|
<state_interface name="force.z"/>
|
|
<param name="frame_id">LH_FOOT</param>
|
|
</sensor>
|
|
<sensor name="RH_foot_end">
|
|
<state_interface name="force.x"/>
|
|
<state_interface name="force.y"/>
|
|
<state_interface name="force.z"/>
|
|
<param name="frame_id">LH_FOOT</param>
|
|
</sensor>
|
|
</ros2_control>
|
|
|
|
<!-- Gazebo's ros2_control plugin -->
|
|
<gazebo>
|
|
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
|
<parameters>$(find cyberdog_description)/config/robot_control.yaml</parameters>
|
|
</plugin>
|
|
<plugin filename="gz-sim-imu-system"
|
|
name="gz::sim::systems::Imu">
|
|
</plugin>
|
|
<plugin filename="gz-sim-contact-system"
|
|
name="gz::sim::systems::Contact">
|
|
</plugin>
|
|
<plugin filename="gz-sim-forcetorque-system"
|
|
name="gz::sim::systems::ForceTorque">
|
|
</plugin>
|
|
</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>
|
|
|
|
</robot> |