Refactor the robot description, and include mecanum control.
This commit is contained in:
Matt Spencer
@@ -11,26 +11,21 @@ controller_manager:
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# Mecanum drive controller
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
# NOTE: The URDF joint names do not match the physical wheel positions due to a
|
||||
# naming inconsistency in agv_pro.urdf. The mapping between the controller's
|
||||
# logical positions and the URDF joint names is as follows:
|
||||
#
|
||||
# Physical position │ URDF joint name
|
||||
# ──────────────────┼────────────────────────────
|
||||
# front-left (FL) │ left_front_wheel_joint ✓
|
||||
# front-right (FR) │ left_rear_wheel_joint ← physically front-right
|
||||
# rear-left (RL) │ right_front_wheel_joint ← physically rear-left
|
||||
# rear-right (RR) │ right_rear_wheel_joint ✓
|
||||
# Wheel link names match their physical corners:
|
||||
# front-left (FL) -> left_front_wheel_joint
|
||||
# front-right (FR) -> right_front_wheel_joint
|
||||
# rear-left (RL) -> left_rear_wheel_joint
|
||||
# rear-right (RR) -> right_rear_wheel_joint
|
||||
#
|
||||
# The same mapping is used by the hardware interface (front_left_joint /
|
||||
# front_right_joint / rear_left_joint / rear_right_joint params in the URDF
|
||||
# <ros2_control> block).
|
||||
mecanum_drive_controller:
|
||||
ros__parameters:
|
||||
front_left_wheel_command_joint_name: left_front_wheel_joint
|
||||
front_right_wheel_command_joint_name: left_rear_wheel_joint
|
||||
rear_left_wheel_command_joint_name: right_front_wheel_joint
|
||||
rear_right_wheel_command_joint_name: right_rear_wheel_joint
|
||||
front_left_wheel_command_joint_name: front_left_wheel_joint
|
||||
front_right_wheel_command_joint_name: front_right_wheel_joint
|
||||
rear_left_wheel_command_joint_name: rear_left_wheel_joint
|
||||
rear_right_wheel_command_joint_name: rear_right_wheel_joint
|
||||
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_footprint
|
||||
|
||||
@@ -2,35 +2,13 @@
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!--
|
||||
Gazebo (gz-sim Harmonic) specific tags. Only included when sim=true.
|
||||
Provides wheel surface friction/material and loads the gz_ros2_control
|
||||
system plugin that hosts the controller_manager inside gz-sim.
|
||||
Gazebo (gz-sim Harmonic) integration. Only two responsibilities:
|
||||
1. Load the gz_ros2_control system plugin, which hosts the
|
||||
controller_manager inside gz-sim and reads the controller yaml.
|
||||
Per-wheel mecanum friction is emitted by the wheel macro when
|
||||
gazebo_ignition (sim) is true, so it is NOT repeated here.
|
||||
-->
|
||||
<xacro:macro name="agv_pro_gazebo" params="namespace controllers_file">
|
||||
|
||||
<!-- Per-wheel surface friction + dark material -->
|
||||
<xacro:macro name="wheel_gz" params="link">
|
||||
<gazebo reference="${link}">
|
||||
<mu1>0.8</mu1>
|
||||
<mu2>0.8</mu2>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
<visual>
|
||||
<material>
|
||||
<ambient>0.1 0.1 0.1 1</ambient>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:wheel_gz link="${namespace}left_front_wheel_link"/>
|
||||
<xacro:wheel_gz link="${namespace}right_front_wheel_link"/>
|
||||
<xacro:wheel_gz link="${namespace}left_rear_wheel_link"/>
|
||||
<xacro:wheel_gz link="${namespace}right_rear_wheel_link"/>
|
||||
|
||||
<!-- gz_ros2_control system plugin: hosts the controller_manager in gz-sim -->
|
||||
<xacro:macro name="agv_pro_gazebo" params="prefix controllers_file">
|
||||
<gazebo>
|
||||
<plugin filename="gz_ros2_control-system"
|
||||
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||
|
||||
@@ -1,59 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!--
|
||||
ros2_control block shared by the physical robot and the Gazebo simulation.
|
||||
|
||||
The only difference between the two is the <hardware> plugin:
|
||||
sim=false -> agv_pro_hardware/AgvProHardwareInterface (real serial base)
|
||||
sim=true -> gz_ros2_control/GazeboSimSystem (Gazebo Harmonic)
|
||||
|
||||
Joint-name note: the URDF joint origins show two wheels are physically at the
|
||||
'wrong' position relative to their names (left_rear is physically front-right,
|
||||
right_front is physically rear-left). The hardware plugin (real) and the
|
||||
mecanum_drive_controller (both) resolve this via the front/rear/left/right
|
||||
parameter mapping.
|
||||
-->
|
||||
<xacro:macro name="agv_pro_ros2_control" params="sim namespace port_name">
|
||||
<ros2_control name="AgvProSystem" type="system">
|
||||
<hardware>
|
||||
<xacro:if value="${sim}">
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${sim}">
|
||||
<plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
|
||||
<param name="port_name">${port_name}</param>
|
||||
<param name="wheel_radius">0.072</param>
|
||||
<param name="lx">0.172</param>
|
||||
<param name="ly">0.180</param>
|
||||
<!-- Physical wheel position -> URDF joint name mapping -->
|
||||
<param name="front_left_joint">${namespace}left_front_wheel_joint</param>
|
||||
<param name="front_right_joint">${namespace}left_rear_wheel_joint</param>
|
||||
<param name="rear_left_joint">${namespace}right_front_wheel_joint</param>
|
||||
<param name="rear_right_joint">${namespace}right_rear_wheel_joint</param>
|
||||
</xacro:unless>
|
||||
</hardware>
|
||||
|
||||
<joint name="${namespace}left_front_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${namespace}left_rear_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${namespace}right_front_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${namespace}right_rear_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -1,33 +1,34 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||
xmlns:gz="http://gazebosim.org/schema">
|
||||
|
||||
<!-- Optional namespace prefix for all link/joint names -->
|
||||
<xacro:arg name="namespace" default=""/>
|
||||
<xacro:property name="namespace" value="$(arg namespace)"/>
|
||||
<xacro:arg name="prefix" default="" />
|
||||
<xacro:property name="prefix" value="$(arg prefix)" />
|
||||
|
||||
<!-- Hardware selection: false -> real serial base, true -> Gazebo (gz-sim) -->
|
||||
<xacro:arg name="sim" default="false"/>
|
||||
<xacro:property name="sim" value="$(arg sim)"/>
|
||||
<xacro:arg name="sim" default="false" />
|
||||
<xacro:property name="sim" value="$(arg sim)" />
|
||||
|
||||
<!-- Real-hardware serial port (ignored when sim=true) -->
|
||||
<xacro:arg name="port_name" default="/dev/agvpro_controller"/>
|
||||
<xacro:property name="port_name" value="$(arg port_name)"/>
|
||||
<xacro:arg name="port_name" default="/dev/agvpro_controller" />
|
||||
<xacro:property name="port_name" value="$(arg port_name)" />
|
||||
|
||||
<!-- Controller params file consumed by gz_ros2_control (sim only) -->
|
||||
<xacro:arg name="controllers_file" default=""/>
|
||||
<xacro:property name="controllers_file" value="$(arg controllers_file)"/>
|
||||
<xacro:arg name="controllers_file" default="" />
|
||||
<xacro:property name="controllers_file" value="$(arg controllers_file)" />
|
||||
|
||||
<link name="${namespace}base_footprint"/>
|
||||
<link name="${prefix}base_footprint" />
|
||||
|
||||
<joint name="${namespace}base_joint" type="fixed">
|
||||
<parent link="${namespace}base_footprint"/>
|
||||
<child link="${namespace}base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
||||
<joint name="${prefix}base_joint" type="fixed">
|
||||
<parent link="${prefix}base_footprint" />
|
||||
<child link="${prefix}base_link" />
|
||||
<origin xyz="0 0 0.020" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}base_link">
|
||||
<link name="${prefix}base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
||||
<origin xyz="-0.0076254 0.0 0.06693" rpy="0 0 0" />
|
||||
<mass value="19.236" />
|
||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
|
||||
iyy="0.24191" iyz="0.0044629"
|
||||
@@ -53,147 +54,43 @@
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<link name="${namespace}right_rear_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.21659" />
|
||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07"
|
||||
izz="0.00051178" />
|
||||
</inertial>
|
||||
<!-- Wheels (built from the shared wheel macro) -->
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro" />
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<xacro:agv_pro_wheel
|
||||
prefix="${prefix}front_right_"
|
||||
parent="${prefix}base_link"
|
||||
reflect="false"
|
||||
mesh="wheel_lb_link.stl"
|
||||
origin_xyz="0.17128 -0.1799 0.052"
|
||||
gazebo_ignition="${sim}" />
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<xacro:agv_pro_wheel
|
||||
prefix="${prefix}front_left_"
|
||||
parent="${prefix}base_link"
|
||||
reflect="true"
|
||||
mesh="wheel_lf_link.stl"
|
||||
origin_xyz="0.17128 0.1799 0.052"
|
||||
gazebo_ignition="${sim}" />
|
||||
|
||||
<joint name="${namespace}right_rear_wheel_joint" type="continuous">
|
||||
<origin xyz="-0.171806101587598 -0.179900399999999 0.0518836514526621" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}right_rear_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
<xacro:agv_pro_wheel
|
||||
prefix="${prefix}rear_left_"
|
||||
parent="${prefix}base_link"
|
||||
reflect="false"
|
||||
mesh="wheel_rf_link.stl"
|
||||
origin_xyz="-0.17181 0.1799 0.052"
|
||||
gazebo_ignition="${sim}" />
|
||||
|
||||
<link name="${namespace}right_front_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="6.6563E-05 -0.019725 8.3836E-05" rpy="0 0 0" />
|
||||
<mass value="0.21659122149244" />
|
||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
||||
iyy="0.00097519" iyz="-2.3635E-07"
|
||||
izz="0.00051178" />
|
||||
</inertial>
|
||||
<xacro:agv_pro_wheel
|
||||
prefix="${prefix}rear_right_"
|
||||
parent="${prefix}base_link"
|
||||
reflect="true"
|
||||
mesh="wheel_rb_link.stl"
|
||||
origin_xyz="-0.171806101587598 -0.179900399999999 0.052"
|
||||
gazebo_ignition="${sim}" />
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}right_front_wheel_joint" type="continuous">
|
||||
<origin xyz="-0.17181 0.1799 0.051884"
|
||||
rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}right_front_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}left_front_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="1.4671E-06 -0.019803 4.3218E-06" rpy="0 0 0" />
|
||||
<mass value="0.3015" />
|
||||
<inertia ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
|
||||
iyy="0.00099948" iyz="7.5332E-08"
|
||||
izz="0.00052362" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}left_front_wheel_joint" type="continuous">
|
||||
<origin xyz="0.17128 0.1799 0.052"
|
||||
rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}left_front_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}left_rear_wheel_link">
|
||||
<inertial>
|
||||
<origin xyz="-2.4454E-06 0.019725 -4.3121E-06" rpy="0 0 0" />
|
||||
<mass value="0.29613" />
|
||||
<inertia ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
|
||||
iyy="0.0009752" iyz="1.173E-07"
|
||||
izz="0.00051131" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}left_rear_wheel_joint" type="continuous">
|
||||
<origin xyz="0.17128 -0.1799 0.052" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}left_rear_wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}laser_link">
|
||||
<link name="${prefix}laser_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||
<mass value="0.049095" />
|
||||
@@ -220,36 +117,36 @@
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${namespace}lidar_joint" type="fixed">
|
||||
<joint name="${prefix}lidar_joint" type="fixed">
|
||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}laser_link" />
|
||||
<parent link="${prefix}base_link" />
|
||||
<child link="${prefix}laser_link" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}camera_link"/>
|
||||
<link name="${prefix}camera_link" />
|
||||
|
||||
<joint name="${namespace}camera_joint" type="fixed">
|
||||
<joint name="${prefix}camera_joint" type="fixed">
|
||||
<origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}camera_link" />
|
||||
<parent link="${prefix}base_link" />
|
||||
<child link="${prefix}camera_link" />
|
||||
</joint>
|
||||
|
||||
<link name="${namespace}imu_link"/>
|
||||
<link name="${prefix}imu_link" />
|
||||
|
||||
<joint name="${namespace}imu_joint" type="fixed">
|
||||
<joint name="${prefix}imu_joint" type="fixed">
|
||||
<origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
|
||||
<parent link="${namespace}base_link" />
|
||||
<child link="${namespace}imu_link" />
|
||||
<parent link="${prefix}base_link" />
|
||||
<child link="${prefix}imu_link" />
|
||||
</joint>
|
||||
|
||||
<!-- ros2_control: real hardware or gz_ros2_control depending on 'sim' -->
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.ros2_control.xacro"/>
|
||||
<xacro:agv_pro_ros2_control sim="${sim}" namespace="${namespace}" port_name="${port_name}"/>
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/control/agv_pro.ros2_control.xacro" />
|
||||
<xacro:agv_pro_ros2_control sim="${sim}" prefix="${prefix}" port_name="${port_name}" />
|
||||
|
||||
<!-- Gazebo-only tags (friction, material, gz_ros2_control system plugin) -->
|
||||
<!-- Gazebo system plugin (hosts controller_manager); wheel friction is in the wheel macro -->
|
||||
<xacro:if value="${sim}">
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro"/>
|
||||
<xacro:agv_pro_gazebo namespace="${namespace}" controllers_file="${controllers_file}"/>
|
||||
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro" />
|
||||
<xacro:agv_pro_gazebo prefix="${prefix}" controllers_file="${controllers_file}" />
|
||||
</xacro:if>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,55 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!--
|
||||
Single ros2_control block, shared by the real robot and the Gazebo sim.
|
||||
Only the <hardware> plugin differs:
|
||||
sim=false -> agv_pro_hardware/AgvProHardwareInterface (real serial base)
|
||||
sim=true -> gz_ros2_control/GazeboSimSystem (Gazebo Harmonic)
|
||||
|
||||
Joint names match the wheel macro:
|
||||
<prefix>{front_left,front_right,rear_left,rear_right}_wheel_joint
|
||||
Each wheel sits at its true corner, so the mapping is 1:1 (no swaps).
|
||||
-->
|
||||
<xacro:macro name="agv_pro_ros2_control" params="sim prefix port_name">
|
||||
<ros2_control name="AgvProSystem" type="system">
|
||||
<hardware>
|
||||
<xacro:if value="${sim}">
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${sim}">
|
||||
<plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
|
||||
<param name="port_name">${port_name}</param>
|
||||
<param name="wheel_radius">0.072</param>
|
||||
<param name="lx">0.172</param>
|
||||
<param name="ly">0.180</param>
|
||||
<param name="front_left_joint">${prefix}front_left_wheel_joint</param>
|
||||
<param name="front_right_joint">${prefix}front_right_wheel_joint</param>
|
||||
<param name="rear_left_joint">${prefix}rear_left_wheel_joint</param>
|
||||
<param name="rear_right_joint">${prefix}rear_right_wheel_joint</param>
|
||||
</xacro:unless>
|
||||
</hardware>
|
||||
|
||||
<joint name="${prefix}front_left_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}front_right_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}rear_left_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}rear_right_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,59 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot xmlns:xacro="http://wiki.ros.org/xacro">
|
||||
<!-- code from https://github.com/RobotnikAutomation/robotnik_description/blob/jazzy-devel/urdf/inertia.urdf.xacro -->
|
||||
<!-- source en.wikipedia.org/wiki/List_of_moments_of_inertia-->
|
||||
|
||||
<!-- TODO Solid sphere of radius r and mass m-->
|
||||
<xacro:macro
|
||||
name="solid_sphere_inertia"
|
||||
params="m r">
|
||||
<inertia
|
||||
ixx="${(2*r*r*m)/5}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="${(2*r*r*m)/5}"
|
||||
iyz="0"
|
||||
izz="${(2*r*r*m)/5}" />
|
||||
</xacro:macro>
|
||||
|
||||
<!-- TODO Hollow sphere of radius r and mass m-->
|
||||
<xacro:macro
|
||||
name="hollow_sphere_inertia"
|
||||
params="m r">
|
||||
<inertia
|
||||
ixx="${(2*r*r*m)/3}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="${(2*r*r*m)/3}"
|
||||
iyz="0"
|
||||
izz="${(2*r*r*m)/3}" />
|
||||
</xacro:macro>
|
||||
|
||||
<!-- TODO Solid cuboid of width w, height h, depth d, and mass m -->
|
||||
<!-- yes, axis in solid_cuboid_inertia are changed, w relates to X axis, d to Z axis, and h to Y axis -->
|
||||
<!-- Keep this comment -->
|
||||
<xacro:macro
|
||||
name="solid_cuboid_inertia"
|
||||
params="m h d w">
|
||||
<inertia
|
||||
ixx="${(m*(h*h+d*d))/12}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="${(m*(w*w+d*d))/12}"
|
||||
iyz="0"
|
||||
izz="${(m*(w*w+h*h))/12}" />
|
||||
</xacro:macro>
|
||||
|
||||
<!-- TODO Solid cylinder of radius r, height h and mass m -->
|
||||
<xacro:macro
|
||||
name="solid_cylinder_inertia"
|
||||
params="m r h">
|
||||
<inertia
|
||||
ixx="${m*(3*r*r+h*h)/12}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="${m*(3*r*r+h*h)/12}"
|
||||
iyz="0"
|
||||
izz="${m*r*r/2}" />
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -0,0 +1,113 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot
|
||||
name="wheel_macro"
|
||||
xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||
xmlns:gz="http://gazebosim.org/schema">
|
||||
<!--
|
||||
AGV Pro wheel macro.
|
||||
|
||||
Builds a continuous-rotation wheel (link + joint) from a mesh, mass,
|
||||
inertia tensor and mounting origin.
|
||||
|
||||
Params:
|
||||
prefix base name, e.g. 'left_front'
|
||||
mesh STL filename in agv_pro_description/meshes
|
||||
reflect reflect the model and the physics
|
||||
wheel_radius
|
||||
wheel_height
|
||||
wheel_mass link mass (kg)
|
||||
origin_xyz joint origin relative to base_link
|
||||
-->
|
||||
<xacro:macro
|
||||
name="agv_pro_wheel"
|
||||
params="prefix
|
||||
parent
|
||||
origin_xyz
|
||||
reflect
|
||||
mesh
|
||||
wheel_radius:=0.072
|
||||
wheel_height:=0.04
|
||||
wheel_mass:=0.25
|
||||
gazebo_ignition:=false">
|
||||
|
||||
<xacro:macro name="cylinder_inertia" params="m r h">
|
||||
<inertia
|
||||
ixx="${m*(3*r*r+h*h)/12}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="${m*r*r/2}"
|
||||
iyz="0"
|
||||
izz="${m*(3*r*r+h*h)/12}" />
|
||||
</xacro:macro>
|
||||
|
||||
<joint name="${prefix}wheel_joint" type="continuous">
|
||||
<origin xyz="${origin_xyz}" rpy="0 0 0" />
|
||||
<parent link="${parent}" />
|
||||
<child link="${prefix}wheel_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
<limit effort="100000" velocity="100" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="${prefix}wheel_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://agv_pro_description/meshes/wheels/${mesh}" />
|
||||
</geometry>
|
||||
<material name="darkgrey">
|
||||
<color rgba="0.2 0.2 0.2 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<sphere radius="${wheel_radius}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="${wheel_mass}" />
|
||||
<origin xyz="0 0 0" />
|
||||
<xacro:cylinder_inertia
|
||||
m="${wheel_mass}"
|
||||
r="${wheel_radius}"
|
||||
h="${wheel_height}" />
|
||||
</inertial>
|
||||
|
||||
</link>
|
||||
|
||||
|
||||
<xacro:if value="${gazebo_ignition}">
|
||||
<gazebo reference="${prefix}wheel_link">
|
||||
<collision>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>0.0</mu2>
|
||||
<xacro:if value="${reflect}">
|
||||
<fdir1
|
||||
gz:expressed_in="$(arg prefix)base_footprint">1 -1 0</fdir1>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${reflect}">
|
||||
<fdir1
|
||||
gz:expressed_in="$(arg prefix)base_footprint">1 1 0</fdir1>
|
||||
</xacro:unless>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>100000.0</kp>
|
||||
<kd>10.0</kd>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@@ -17,23 +17,13 @@ controller_manager:
|
||||
# (gz_ros2_control/GazeboSimSystem here vs agv_pro_hardware/AgvProHardwareInterface
|
||||
# on the real robot); the controller parameters are identical.
|
||||
#
|
||||
# NOTE: The URDF joint names do not match the physical wheel positions due to a
|
||||
# naming inconsistency in the shared robot model. The controller compensates via
|
||||
# the front/rear/left/right command-joint mapping below:
|
||||
#
|
||||
# Physical position │ URDF joint name
|
||||
# ──────────────────┼────────────────────────────
|
||||
# front-left (FL) │ left_front_wheel_joint ✓
|
||||
# front-right (FR) │ left_rear_wheel_joint ← physically front-right
|
||||
# rear-left (RL) │ right_front_wheel_joint ← physically rear-left
|
||||
# rear-right (RR) │ right_rear_wheel_joint ✓
|
||||
# ─────────────────────────────────────────────────────────────────────────────
|
||||
mecanum_drive_controller:
|
||||
ros__parameters:
|
||||
front_left_wheel_command_joint_name: left_front_wheel_joint
|
||||
front_right_wheel_command_joint_name: left_rear_wheel_joint
|
||||
rear_left_wheel_command_joint_name: right_front_wheel_joint
|
||||
rear_right_wheel_command_joint_name: right_rear_wheel_joint
|
||||
front_left_wheel_command_joint_name: front_left_wheel_joint
|
||||
front_right_wheel_command_joint_name: front_right_wheel_joint
|
||||
rear_left_wheel_command_joint_name: rear_left_wheel_joint
|
||||
rear_right_wheel_command_joint_name: rear_right_wheel_joint
|
||||
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_footprint
|
||||
|
||||
Reference in New Issue
Block a user