search

fsstudio-team / ZeroSimROSUnity

Robotic simulation in Unity with ROS integration.
https://roboticsimulationservices.com/
MIT License
168 stars 21 forks source link

Import URDF not working as expected #22

Open ghost opened 2 years ago

ghost commented 2 years ago

HI,

I'm trying to import the LeoRover description in Unity using ZeroSim. I followed the video tutorial and the comment from @micahpearlman in https://github.com/fsstudio-team/ZeroSimROSUnity/issues/17#issuecomment-995344947. I was able to import the .obj files but it looks like the joints weren't imported properly.

image

Have I missed something or is this the expected behaviour?

Setup info:

Thanks, Alex

micahpearlman commented 2 years ago

Hmmm, no idea? Can you share the scene?

micahpearlman commented 2 years ago

Also, see issue: https://github.com/fsstudio-team/ZeroSimROSUnity/issues/17

ghost commented 2 years ago

Hmmm, no idea? Can you share the scene?

I created a new scene then used ZeroSim -> Import URDF https://github.com/alex-ssom/zerosim_import_leorover_urdf.git

I noticed that the hierarchy tree is different than the one generated when using ZeroSim -> New Robot

micahpearlman commented 2 years ago

@alex-ssom so I was able to import the LEO robot. There is a full description in the README.md.

There was a fix we needed to do so you will want to upgrade ZeroSim to the latest version of 0.1.18.

leo

LEO Robot Example

  1. Clone the Leo robot: git clone --recursive https://github.com/LeoRover/leo_common.git
  2. Checkout Melodic version. cd leo_common && git checkout melodic.
  3. Startup ZeroSim Docker mounting the LEO directory:

# make sure you are in parent directory for `leo_common`
cd .. 

# run docker
docker run -it --rm \
--publish=9090:9090 \
--publish=11311:11311 \
--publish=8083:8083 \
--publish=80:80 \
--publish=5678:5678 \
--name my_zerosim_vnc_docker \
--volume=$(pwd)/leo_common/:/catkin_ws/src/leo_common/ \
zerodog/zerosim_ros_vnc:latest \
bash
  1. In the Docker command prompt build the catkin workspace:
source devel/setup.bash
catkin build
# Make sure to source ROS again to get the new LEO robot
source devel/setup.bash
  1. In the Docker command prompt Run XAcro on the LEO robot to get the URDF:
rosrun xacro xacro src/leo_common/leo_description/urdf/leo_sim.urdf.xacro > /tmp/leo_sim.urdf
  1. Convert LEO robot meshes to .obj for Unity. In the Docker command prompt:
/zerosim_tools/convert_meshes_to_obj.sh ./src/leo_common/leo_description/models
  1. Copy URDF from Docker to host:
# create a directory to store the URDF and meshes
mkdir my_leo_robot

# copy the URDF
docker cp my_zerosim_vnc_docker:/tmp/leo_sim.urdf ./my_leo_robot
  1. Copy Meshes from Docker to host:
# Note we are preserving the path
docker cp my_zerosim_vnc_docker:/catkin_ws/src/leo_common/leo_description/models ./my_leo_robot/leo_description/models
  1. Fix the URDF paths to the meshes:
# First fix up the .DAEs to point to the OBJs
sed -i 's#.dae#.obj#g' my_leo_robot/leo_sim.urdf

# The fix up the .STLs to point to the OBJs
sed -i 's#.stl#.obj#g' my_leo_robot/leo_sim.urdf

# Now remove the `package://` because we are using the filesystem
sed -i 's#package://#./#g' my_leo_robot/leo_sim.urdf
  1. In Unity import URDF by: Right click and select ZeroSim --> Import URDF...
ghost commented 2 years ago

Wow! I didn't expect an answer so complete, I'll try it by the end of the week and I'll let you know how it went :smiley:

micahpearlman commented 2 years ago

@alex-ssom do us a favor, if necessary, fix or add any notes for the instructions above -- this will help a lot of users and be a big help for us.

ghost commented 2 years ago

Testing the provided instructions

The provided instructions worked like a charm except for step 7 and 8. Step 7:

# create a directory and a sub-directory to store the URDF and meshes
mkdir -p my_leo_robot/leo_description

Step 8: The folder should be removed from the second path.

# Note we are preserving the path
docker cp my_zerosim_vnc_docker:/catkin_ws/src/leo_common/leo_description/models ./my_leo_robot/leo_description

Using an extended URDF

It worked so well in fact that I tried using my xacro add on package.

1) I needed to add a second volume:

docker run -it --rm \
--publish=9090:9090 \
--publish=11311:11311 \
--publish=8083:8083 \
--publish=80:80 \
--publish=5678:5678 \
--name my_zerosim_vnc_docker \
--volume=$(pwd)/leo_common/:/catkin_ws/src/leo_common/ \
--volume=$(pwd)/leo_ssom_addons_description/:/catkin_ws/src/leo_ssom_addons_description/ \
zerodog/zerosim_ros_vnc:latest \
bash

2) Install the realsense2_description package

sudo apt update && sudo apt install ros-melodic-realsense2-description

3) Step 4 and 5 as is.

4) Run step 6 for all packages used by my URDF. 5) Create a new directory in my_leo_robot for all additional packages. 6) Run step 8 for all packages used by my URDF. 7) Step 9 as is.

The only issue I encountered is that the add on objects aren't localised and oriented properly. Probably because they haven't been drawn using the same CAD software therefore their original orientations aren't identical. This doesn't happen when displaying my URDF using RViz.

image (1)

I manually placed some of said objects. image

micahpearlman commented 2 years ago

Fantastic! We appreciate your patience in working with us to fix issues.

FYI: There are a lot of Gazebo plugins in the model that ZeroSim doesn't support out of the box -- so if you need them you may have to implement them yourself. Would be tremendous if you did implement them if you could contribute them to ZeroSim.

ghost commented 2 years ago

No problem, that's what open source is for!

We appreciate that you released ZeroSim to the public. I have experience with ROS and linux, but at the moment I have no experience whatsoever with Unity and C#. I have a colleague that's got the opposite set of experiences. It sure is a possibility for us to contribute back to ZeroSim.

ghost commented 2 years ago

For anyone whom might be interested. At the moment, the Import URDF script doesn't handle the link_origin_rpy variable. I've had to manually rewrite my xacro files with every link_origin_rpy set to "0 0 0" to localise the links properly in Unity.

image

micahpearlman commented 2 years ago

@alex-ssom thanks so much! We will take a look at supporting that in the near future.

micahpearlman commented 2 years ago

@alex-ssom searching the googles for a definition of "link_origin_rpy" for URDF and not seeing it? Any references to definition of this in context of URDF? Seems to be an Melodic xacro conversion issue?

ghost commented 2 years ago

I agree with you, the documentation of this variable is very sparse. It's mentionned in the link documentation and in the Building a Visual Robot Model with URDF from Scratch tutorial. In my application, I'm using the link_origin_rpy variable to modify the origin of the CAD file.

xacro output

When using link_origin_rpy my urdf is displayed properly in RViz on both melodic and noetic. image When running 

rosrun xacro xacro src/leo_common/leo_description/urdf/leo_sim.urdf.xacro > leo_ssom_ROSVERSION_xacro.urdf

I get the following output.

Noetic

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from src/leo_common/leo_description/urdf/leo_sim.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="leo_sim">
  <!-- LEO GAZEBO -->
  <link name="base_footprint"/>
  <link name="base_link">
    <inertial>
      <mass value="1.584994"/>
      <origin xyz="-0.019662 0.011643 -0.031802"/>
      <inertia ixx="0.01042" ixy="0.001177" ixz="-0.0008871" iyy="0.01045" iyz="0.0002226" izz="0.01817"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_L_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_R_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="antenna_link">
    <inertial>
      <mass value="0.001237"/>
      <origin xyz="0 0 0.028828"/>
      <inertia ixx="2.5529e-7" ixy="0.0" ixz="0.0" iyy="2.5529e-7" iyz="0.0" izz="1.354e-8"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Antenna.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.028"/>
      <geometry>
        <cylinder length="0.056" radius="0.0055"/>
      </geometry>
    </collision>
  </link>
  <link name="camera_frame"/>
  <link name="camera_optical_frame"/>
  <!-- JOINTS -->
  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.19783"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <joint name="rocker_L_joint" type="revolute">
    <origin rpy="0 0 3.141592653589793" xyz="0.00263 0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_L_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
  </joint>
  <joint name="rocker_R_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.00263 -0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_R_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
    <mimic joint="rocker_L_joint"/>
  </joint>
  <joint name="wheel_FL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_FL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_RL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_FR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_FR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_RR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="antenna_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0052 0.056 -0.0065"/>
    <parent link="base_link"/>
    <child link="antenna_link"/>
  </joint>
  <joint name="camera_joint" type="fixed">
    <origin rpy="0 0.2094 0" xyz="0.0971 0 -0.0427"/>
    <parent link="base_link"/>
    <child link="camera_frame"/>
  </joint>
  <joint name="camera_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0.0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="camera_frame"/>
    <child link="camera_optical_frame"/>
  </joint>
  <!-- rocker ODE properties -->
  <gazebo reference="rocker_L_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="rocker_R_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- wheel ODE properties -->
  <gazebo reference="wheel_FL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_FR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- camera -->
  <gazebo reference="camera_frame">
    <sensor name="leo_camera" type="camera">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <visualize>false</visualize>
      <camera name="leo_camera">
        <horizontal_fov>1.9</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
        <distortion>
          <k1>-0.279817</k1>
          <k2>0.060321</k2>
          <k3>0.000487</k3>
          <p1>0.000310</p1>
          <p2>0.000000</p2>
          <center>0.5 0.5</center>
        </distortion>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <robotNamespace></robotNamespace>
        <cameraName>camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_optical_frame</frameName>
        <distortionK1>-0.279817</distortionK1>
        <distortionK2>0.060321</distortionK2>
        <distortionK3>0.000487</distortionK3>
        <distortionT1>0.000310</distortionT1>
        <distortionT2>0.000000</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!-- rocker differential -->
  <gazebo>
    <plugin filename="libleo_gazebo_differential_plugin.so" name="rocker_differential">
      <jointA>rocker_L_joint</jointA>
      <jointB>rocker_R_joint</jointB>
      <forceConstant>100.0</forceConstant>
    </plugin>
  </gazebo>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace></robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
  <transmission name="wheel_FL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_FR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="rocker_L_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_L_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_L_virtual_motor"/>
  </transmission>
  <transmission name="rocker_R_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_R_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_R_virtual_motor"/>
  </transmission>
  <link name="world"/>
  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="estop_guard_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/EStopGuard.dae"/>
      </geometry>
      <origin rpy="0 1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/EStopGuard.stl"/>
      </geometry>
      <origin rpy="0 1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="estop_guard_joint" type="fixed">
    <parent link="base_link"/>
    <child link="estop_guard_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.178 0 -0.02"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="front_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/FrontMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/FrontMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="front_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0.11 0 0.0085"/>
  </joint>
  <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RearMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.155 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RearMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.155 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.045 0 0.0085"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="d435i_holder_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/D435iHolder.dae"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/D435iHolder.stl"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="d435i_holder_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="d435i_holder_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.050 -0.018 0"/>
  </joint>
  <material name="aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
  </material>
  <material name="plastic">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="realsense_joint" type="fixed">
    <origin rpy="0 0.087266 0" xyz="0.024 -0.0145 0.0075"/>
    <parent link="d435i_holder_base_link"/>
    <child link="realsense_bottom_screw_frame"/>
  </joint>
  <link name="realsense_bottom_screw_frame"/>
  <joint name="realsense_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.010600000000000002 0.0175 0.0125"/>
    <parent link="realsense_bottom_screw_frame"/>
    <child link="realsense_link"/>
  </joint>
  <link name="realsense_link">
    <visual>
      <!-- the mesh origin is at front plate in between the two infrared camera axes -->
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0043 -0.0175 0"/>
      <geometry>
        <mesh filename="package://realsense2_description/meshes/d435.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.072"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- camera depth joints and links -->
  <joint name="realsense_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_depth_frame"/>
  </joint>
  <link name="realsense_depth_frame"/>
  <joint name="realsense_depth_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_depth_frame"/>
    <child link="realsense_depth_optical_frame"/>
  </joint>
  <link name="realsense_depth_optical_frame"/>
  <!-- camera left IR joints and links -->
  <joint name="realsense_infra1_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra1_frame"/>
  </joint>
  <link name="realsense_infra1_frame"/>
  <joint name="realsense_infra1_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_infra1_frame"/>
    <child link="realsense_infra1_optical_frame"/>
  </joint>
  <link name="realsense_infra1_optical_frame"/>
  <!-- camera right IR joints and links -->
  <joint name="realsense_infra2_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 -0.05 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra2_frame"/>
  </joint>
  <link name="realsense_infra2_frame"/>
  <joint name="realsense_infra2_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_infra2_frame"/>
    <child link="realsense_infra2_optical_frame"/>
  </joint>
  <link name="realsense_infra2_optical_frame"/>
  <!-- camera color joints and links -->
  <joint name="realsense_color_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.015 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_color_frame"/>
  </joint>
  <link name="realsense_color_frame"/>
  <joint name="realsense_color_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_color_frame"/>
    <child link="realsense_color_optical_frame"/>
  </joint>
  <link name="realsense_color_optical_frame"/>
  <link name="realsense_accel_frame"/>
  <link name="realsense_accel_optical_frame"/>
  <link name="realsense_gyro_frame"/>
  <link name="realsense_gyro_optical_frame"/>
  <joint name="realsense_accel_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_accel_frame"/>
  </joint>
  <joint name="realsense_accel_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_accel_frame"/>
    <child link="realsense_accel_optical_frame"/>
  </joint>
  <joint name="realsense_gyro_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_gyro_frame"/>
  </joint>
  <joint name="realsense_gyro_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="realsense_gyro_frame"/>
    <child link="realsense_gyro_optical_frame"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="jetson_nano_bracket_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoBracket.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.079 0.0225 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoBracket.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0.079 0.0225 -0.0085"/>
    </collision>
  </link>
  <joint name="jetson_nano_bracket_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="jetson_nano_bracket_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.079 -0.0225 0.0085"/>
  </joint>
  <link name="jetson_nano_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoSimplified.dae"/>
      </geometry>
      <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoSimplified.stl"/>
      </geometry>
      <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="jetson_nano_base_joint" type="fixed">
    <parent link="jetson_nano_bracket_link"/>
    <child link="jetson_nano_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="left_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="left_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="left_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 0.054 0"/>
  </joint>
  <link name="right_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 -1.5707963267948966" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="right_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="right_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 -0.054 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="lidar_mount_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/LiDAR_Mount.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/LiDAR_Mount.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="lidar_mount_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="lidar_mount_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 -1.5707963267948966" xyz="-0.095 0 0"/>
  </joint>
  <link name="rplidar_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RPLidar.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RPLidar.stl"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="rplidar_joint" type="fixed">
    <parent link="lidar_mount_link"/>
    <child link="rplidar_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0.0339"/>
  </joint>
  <link name="laser"/>
  <joint name="laser_joint" type="fixed">
    <parent link="rplidar_link"/>
    <child link="laser"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 1.5707963267948966" xyz="0 0 0.031"/>
  </joint>
  <gazebo reference="realsense_depth_frame">
    <sensor name="openni_camera_camera" type="depth">
      <always_on>1</always_on>
      <visualize>true</visualize>
      <camera>
        <horizontal_fov>1.047</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <depth_camera>

        </depth_camera>
        <clip>
          <near>0.1</near>
          <far>10</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="realsense_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>realsense</cameraName>
        <frameName>realsense_depth_frame</frameName>
        <imageTopicName>color/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudCutoff>0.4</pointCloudCutoff>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0.0</CxPrime>
        <Cx>0.0</Cx>
        <Cy>0.0</Cy>
        <focalLength>0.0</focalLength>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo>
    <light name="left_headlight" type="spot">
      <pose>0.659915 -0.501804 1 0 -0 0</pose>
      <diffuse>0.5 0.5 0.5 1</diffuse>
      <specular>0.1 0.1 0.1 1</specular>
      <direction>0 0 -1</direction>
      <attenuation>
        <range>20</range>
        <constant>0.5</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <cast_shadows>0</cast_shadows>
      <spot>
        <inner_angle>0.6</inner_angle>
        <outer_angle>1</outer_angle>
        <falloff>1</falloff>
      </spot>
    </light>
  </gazebo>
</robot>

Melodic

<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from src/leo_common/leo_description/urdf/leo_sim.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="leo_sim">
  <!-- LEO GAZEBO -->
  <link name="base_footprint"/>
  <link name="base_link">
    <inertial>
      <mass value="1.584994"/>
      <origin xyz="-0.019662 0.011643 -0.031802"/>
      <inertia ixx="0.01042" ixy="0.001177" ixz="-0.0008871" iyy="0.01045" iyz="0.0002226" izz="0.01817"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Chassis_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_L_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_R_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Rocker_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelA.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/WheelB.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="package://leo_description/models/Wheel_outline.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="antenna_link">
    <inertial>
      <mass value="0.001237"/>
      <origin xyz="0 0 0.028828"/>
      <inertia ixx="2.5529e-7" ixy="0.0" ixz="0.0" iyy="2.5529e-7" iyz="0.0" izz="1.354e-8"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://leo_description/models/Antenna.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.028"/>
      <geometry>
        <cylinder length="0.056" radius="0.0055"/>
      </geometry>
    </collision>
  </link>
  <link name="camera_frame"/>
  <link name="camera_optical_frame"/>
  <!-- JOINTS -->
  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.19783"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <joint name="rocker_L_joint" type="revolute">
    <origin rpy="0 0 3.14159265359" xyz="0.00263 0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_L_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
  </joint>
  <joint name="rocker_R_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.00263 -0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_R_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
    <mimic joint="rocker_L_joint"/>
  </joint>
  <joint name="wheel_FL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_FL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_RL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_FR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_FR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_RR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="antenna_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0052 0.056 -0.0065"/>
    <parent link="base_link"/>
    <child link="antenna_link"/>
  </joint>
  <joint name="camera_joint" type="fixed">
    <origin rpy="0 0.2094 0" xyz="0.0971 0 -0.0427"/>
    <parent link="base_link"/>
    <child link="camera_frame"/>
  </joint>
  <joint name="camera_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="camera_frame"/>
    <child link="camera_optical_frame"/>
  </joint>
  <!-- rocker ODE properties -->
  <gazebo reference="rocker_L_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="rocker_R_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- wheel ODE properties -->
  <gazebo reference="wheel_FL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_FR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- camera -->
  <gazebo reference="camera_frame">
    <sensor name="leo_camera" type="camera">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <visualize>false</visualize>
      <camera name="leo_camera">
        <horizontal_fov>1.9</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
        <distortion>
          <k1>-0.279817</k1>
          <k2>0.060321</k2>
          <k3>0.000487</k3>
          <p1>0.000310</p1>
          <p2>0.000000</p2>
          <center>0.5 0.5</center>
        </distortion>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <robotNamespace></robotNamespace>
        <cameraName>camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_optical_frame</frameName>
        <distortionK1>-0.279817</distortionK1>
        <distortionK2>0.060321</distortionK2>
        <distortionK3>0.000487</distortionK3>
        <distortionT1>0.000310</distortionT1>
        <distortionT2>0.000000</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!-- rocker differential -->
  <gazebo>
    <plugin filename="libleo_gazebo_differential_plugin.so" name="rocker_differential">
      <jointA>rocker_L_joint</jointA>
      <jointB>rocker_R_joint</jointB>
      <forceConstant>100.0</forceConstant>
    </plugin>
  </gazebo>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace></robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
  <transmission name="wheel_FL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_FR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="rocker_L_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_L_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_L_virtual_motor"/>
  </transmission>
  <transmission name="rocker_R_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_R_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_R_virtual_motor"/>
  </transmission>
  <link name="world"/>
  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="estop_guard_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/EStopGuard.dae"/>
      </geometry>
      <origin rpy="0 1.57079632679 -1.57079632679" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/EStopGuard.stl"/>
      </geometry>
      <origin rpy="0 1.57079632679 -1.57079632679" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="estop_guard_joint" type="fixed">
    <parent link="base_link"/>
    <child link="estop_guard_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.178 0 -0.02"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="front_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/FrontMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/FrontMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="front_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0.11 0 0.0085"/>
  </joint>
  <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RearMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.155 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RearMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.155 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.045 0 0.0085"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="d435i_holder_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/D435iHolder.dae"/>
      </geometry>
      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/D435iHolder.stl"/>
      </geometry>
      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="d435i_holder_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="d435i_holder_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.050 -0.018 0"/>
  </joint>
  <material name="aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
  </material>
  <material name="plastic">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="realsense_joint" type="fixed">
    <origin rpy="0 0.087266 0" xyz="0.024 -0.0145 0.0075"/>
    <parent link="d435i_holder_base_link"/>
    <child link="realsense_bottom_screw_frame"/>
  </joint>
  <link name="realsense_bottom_screw_frame"/>
  <joint name="realsense_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0106 0.0175 0.0125"/>
    <parent link="realsense_bottom_screw_frame"/>
    <child link="realsense_link"/>
  </joint>
  <link name="realsense_link">
    <visual>
      <!-- the mesh origin is at front plate in between the two infrared camera axes -->
      <origin rpy="1.57079632679 0 1.57079632679" xyz="0.0043 -0.0175 0"/>
      <geometry>
        <mesh filename="package://realsense2_description/meshes/d435.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.072"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- camera depth joints and links -->
  <joint name="realsense_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_depth_frame"/>
  </joint>
  <link name="realsense_depth_frame"/>
  <joint name="realsense_depth_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_depth_frame"/>
    <child link="realsense_depth_optical_frame"/>
  </joint>
  <link name="realsense_depth_optical_frame"/>
  <!-- camera left IR joints and links -->
  <joint name="realsense_infra1_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra1_frame"/>
  </joint>
  <link name="realsense_infra1_frame"/>
  <joint name="realsense_infra1_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_infra1_frame"/>
    <child link="realsense_infra1_optical_frame"/>
  </joint>
  <link name="realsense_infra1_optical_frame"/>
  <!-- camera right IR joints and links -->
  <joint name="realsense_infra2_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 -0.05 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra2_frame"/>
  </joint>
  <link name="realsense_infra2_frame"/>
  <joint name="realsense_infra2_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_infra2_frame"/>
    <child link="realsense_infra2_optical_frame"/>
  </joint>
  <link name="realsense_infra2_optical_frame"/>
  <!-- camera color joints and links -->
  <joint name="realsense_color_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.015 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_color_frame"/>
  </joint>
  <link name="realsense_color_frame"/>
  <joint name="realsense_color_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_color_frame"/>
    <child link="realsense_color_optical_frame"/>
  </joint>
  <link name="realsense_color_optical_frame"/>
  <link name="realsense_accel_frame"/>
  <link name="realsense_accel_optical_frame"/>
  <link name="realsense_gyro_frame"/>
  <link name="realsense_gyro_optical_frame"/>
  <joint name="realsense_accel_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_accel_frame"/>
  </joint>
  <joint name="realsense_accel_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_accel_frame"/>
    <child link="realsense_accel_optical_frame"/>
  </joint>
  <joint name="realsense_gyro_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_gyro_frame"/>
  </joint>
  <joint name="realsense_gyro_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_gyro_frame"/>
    <child link="realsense_gyro_optical_frame"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="jetson_nano_bracket_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoBracket.dae"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.079 0.0225 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoBracket.stl"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.079 0.0225 -0.0085"/>
    </collision>
  </link>
  <joint name="jetson_nano_bracket_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="jetson_nano_bracket_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.079 -0.0225 0.0085"/>
  </joint>
  <link name="jetson_nano_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/JetsonNanoSimplified.dae"/>
      </geometry>
      <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/JetsonNanoSimplified.stl"/>
      </geometry>
      <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="jetson_nano_base_joint" type="fixed">
    <parent link="jetson_nano_bracket_link"/>
    <child link="jetson_nano_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="left_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="left_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="left_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 0.054 0"/>
  </joint>
  <link name="right_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/WifiAntenna.dae"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/WifiAntenna.stl"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="right_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="right_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.090 -0.054 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="lidar_mount_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/LiDAR_Mount.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/LiDAR_Mount.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="lidar_mount_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="lidar_mount_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 -1.57079632679" xyz="-0.095 0 0"/>
  </joint>
  <link name="rplidar_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RPLidar.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RPLidar.stl"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="rplidar_joint" type="fixed">
    <parent link="lidar_mount_link"/>
    <child link="rplidar_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.0339"/>
  </joint>
  <link name="laser"/>
  <joint name="laser_joint" type="fixed">
    <parent link="rplidar_link"/>
    <child link="laser"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 1.57079632679" xyz="0 0 0.031"/>
  </joint>
  <gazebo reference="realsense_depth_frame">
    <sensor name="openni_camera_camera" type="depth">
      <always_on>1</always_on>
      <visualize>true</visualize>
      <camera>
        <horizontal_fov>1.047</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <depth_camera>

        </depth_camera>
        <clip>
          <near>0.1</near>
          <far>10</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="realsense_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>realsense</cameraName>
        <frameName>realsense_depth_frame</frameName>
        <imageTopicName>color/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudCutoff>0.4</pointCloudCutoff>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0.0</CxPrime>
        <Cx>0.0</Cx>
        <Cy>0.0</Cy>
        <focalLength>0.0</focalLength>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo>
    <light name="left_headlight" type="spot">
      <pose>0.659915 -0.501804 1 0 -0 0</pose>
      <diffuse>0.5 0.5 0.5 1</diffuse>
      <specular>0.1 0.1 0.1 1</specular>
      <direction>0 0 -1</direction>
      <attenuation>
        <range>20</range>
        <constant>0.5</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <cast_shadows>0</cast_shadows>
      <spot>
        <inner_angle>0.6</inner_angle>
        <outer_angle>1</outer_angle>
        <falloff>1</falloff>
      </spot>
    </light>
  </gazebo>
</robot>

When running diff on the two generated files, the only difference is the precision of the angles.

Expected output (from manually adjusted xacro)

<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from src/leo_common/leo_description/urdf/leo_sim.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="leo_sim">
  <!-- LEO GAZEBO -->
  <link name="base_footprint"/>
  <link name="base_link">
    <inertial>
      <mass value="1.584994"/>
      <origin xyz="-0.019662 0.011643 -0.031802"/>
      <inertia ixx="0.01042" ixy="0.001177" ixz="-0.0008871" iyy="0.01045" iyz="0.0002226" izz="0.01817"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/Chassis.obj"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Chassis_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_L_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/Rocker.obj"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Rocker_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="rocker_R_link">
    <inertial>
      <mass value="1.387336"/>
      <origin xyz="0 0.01346 -0.06506"/>
      <inertia ixx="0.002956" ixy="-0.000001489324" ixz="-0.000008103407" iyy="0.02924" iyz="0.00007112" izz="0.02832"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/Rocker.obj"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Rocker_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/WheelA.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Wheel_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RL_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/WheelA.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Wheel_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_FR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/WheelB.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Wheel_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="wheel_RR_link">
    <inertial>
      <mass value="0.283642"/>
      <origin xyz="0 0.030026 0"/>
      <inertia ixx="0.000391" ixy="0.00000123962" ixz="5.52582e-7" iyy="0.0004716" iyz="-0.000002082042" izz="0.000391"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/WheelB.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0.04485 0"/>
      <geometry>
        <cylinder length="0.04" radius="0.0625"/>
      </geometry>
    </collision>
    <collision>
      <geometry>
        <mesh filename="./leo_description/models/Wheel_outline.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="antenna_link">
    <inertial>
      <mass value="0.001237"/>
      <origin xyz="0 0 0.028828"/>
      <inertia ixx="2.5529e-7" ixy="0.0" ixz="0.0" iyy="2.5529e-7" iyz="0.0" izz="1.354e-8"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="./leo_description/models/Antenna.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.028"/>
      <geometry>
        <cylinder length="0.056" radius="0.0055"/>
      </geometry>
    </collision>
  </link>
  <link name="camera_frame"/>
  <link name="camera_optical_frame"/>
  <!-- JOINTS -->
  <joint name="base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.19783"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <joint name="rocker_L_joint" type="revolute">
    <origin rpy="0 0 3.14159265359" xyz="0.00263 0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_L_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
  </joint>
  <joint name="rocker_R_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.00263 -0.14167 -0.04731"/>
    <parent link="base_link"/>
    <child link="rocker_R_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="100.0" lower="-0.24" upper="0.24" velocity="100.0"/>
    <dynamics damping="0.1" friction="1.0"/>
    <mimic joint="rocker_L_joint"/>
  </joint>
  <joint name="wheel_FL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_FL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RL_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_L_link"/>
    <child link="wheel_RL_link"/>
    <axis xyz="0 -1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_FR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_FR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="wheel_RR_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.15256 -0.08214 -0.08802"/>
    <parent link="rocker_R_link"/>
    <child link="wheel_RR_link"/>
    <axis xyz="0 1 0"/>
    <limit effort="2.0" velocity="6.0"/>
    <dynamics damping="0.1" friction="0.3125"/>
  </joint>
  <joint name="antenna_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0052 0.056 -0.0065"/>
    <parent link="base_link"/>
    <child link="antenna_link"/>
  </joint>
  <joint name="camera_joint" type="fixed">
    <origin rpy="0 0.2094 0" xyz="0.0971 0 -0.0427"/>
    <parent link="base_link"/>
    <child link="camera_frame"/>
  </joint>
  <joint name="camera_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="camera_frame"/>
    <child link="camera_optical_frame"/>
  </joint>
  <!-- rocker ODE properties -->
  <gazebo reference="rocker_L_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="rocker_R_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- wheel ODE properties -->
  <gazebo reference="wheel_FL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_FR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RL_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="wheel_RR_link">
    <kp>1e6</kp>
    <kd>1.0</kd>
    <mu1>3.0</mu1>
    <mu2>0.5</mu2>
    <fdir1>1 0 0</fdir1>
    <minDepth>0.003</minDepth>
  </gazebo>
  <!-- camera -->
  <gazebo reference="camera_frame">
    <sensor name="leo_camera" type="camera">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <visualize>false</visualize>
      <camera name="leo_camera">
        <horizontal_fov>1.9</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
        <distortion>
          <k1>-0.279817</k1>
          <k2>0.060321</k2>
          <k3>0.000487</k3>
          <p1>0.000310</p1>
          <p2>0.000000</p2>
          <center>0.5 0.5</center>
        </distortion>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <robotNamespace></robotNamespace>
        <cameraName>camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_optical_frame</frameName>
        <distortionK1>-0.279817</distortionK1>
        <distortionK2>0.060321</distortionK2>
        <distortionK3>0.000487</distortionK3>
        <distortionT1>0.000310</distortionT1>
        <distortionT2>0.000000</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!-- rocker differential -->
  <gazebo>
    <plugin filename="libleo_gazebo_differential_plugin.so" name="rocker_differential">
      <jointA>rocker_L_joint</jointA>
      <jointB>rocker_R_joint</jointB>
      <forceConstant>100.0</forceConstant>
    </plugin>
  </gazebo>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace></robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
  <transmission name="wheel_FL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RL_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RL_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RL_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_FR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_FR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_FR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="wheel_RR_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wheel_RR_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="wheel_RR_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="rocker_L_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_L_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_L_virtual_motor"/>
  </transmission>
  <transmission name="rocker_R_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rocker_R_joint">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rocker_R_virtual_motor"/>
  </transmission>
  <link name="world"/>
  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="estop_guard_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/EStopGuard.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/EStopGuard.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="estop_guard_joint" type="fixed">
    <parent link="base_link"/>
    <child link="estop_guard_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 1.57079632679 -1.57079632679" xyz="-0.178 0 -0.02"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="front_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/FrontMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/FrontMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="front_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 -1.57079632679" xyz="0.11 0 0.0085"/>
  </joint>
  <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/RearMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/RearMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="d435i_holder_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/D435iHolder.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/D435iHolder.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="d435i_holder_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="d435i_holder_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 3.14159265359" xyz="0.018 -0.050 0"/>
  </joint>
  <material name="aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
  </material>
  <material name="plastic">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="realsense_joint" type="fixed">
    <origin rpy="0 0.087266 -1.57079632679" xyz="-0.0145 -0.024 0.0075"/>
    <parent link="d435i_holder_base_link"/>
    <child link="realsense_bottom_screw_frame"/>
  </joint>
  <link name="realsense_bottom_screw_frame"/>
  <joint name="realsense_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0106 0.0175 0.0125"/>
    <parent link="realsense_bottom_screw_frame"/>
    <child link="realsense_link"/>
  </joint>
  <link name="realsense_link">
    <visual>
      <!-- the mesh origin is at front plate in between the two infrared camera axes -->
      <origin rpy="1.57079632679 0 1.57079632679" xyz="0.0043 -0.0175 0"/>
      <geometry>
        <mesh filename="./realsense2_description/meshes/d435.obj"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.072"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- camera depth joints and links -->
  <joint name="realsense_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_depth_frame"/>
  </joint>
  <link name="realsense_depth_frame"/>
  <joint name="realsense_depth_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_depth_frame"/>
    <child link="realsense_depth_optical_frame"/>
  </joint>
  <link name="realsense_depth_optical_frame"/>
  <!-- camera left IR joints and links -->
  <joint name="realsense_infra1_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra1_frame"/>
  </joint>
  <link name="realsense_infra1_frame"/>
  <joint name="realsense_infra1_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_infra1_frame"/>
    <child link="realsense_infra1_optical_frame"/>
  </joint>
  <link name="realsense_infra1_optical_frame"/>
  <!-- camera right IR joints and links -->
  <joint name="realsense_infra2_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 -0.05 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_infra2_frame"/>
  </joint>
  <link name="realsense_infra2_frame"/>
  <joint name="realsense_infra2_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_infra2_frame"/>
    <child link="realsense_infra2_optical_frame"/>
  </joint>
  <link name="realsense_infra2_optical_frame"/>
  <!-- camera color joints and links -->
  <joint name="realsense_color_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.015 0"/>
    <parent link="realsense_link"/>
    <child link="realsense_color_frame"/>
  </joint>
  <link name="realsense_color_frame"/>
  <joint name="realsense_color_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_color_frame"/>
    <child link="realsense_color_optical_frame"/>
  </joint>
  <link name="realsense_color_optical_frame"/>
  <link name="realsense_accel_frame"/>
  <link name="realsense_accel_optical_frame"/>
  <link name="realsense_gyro_frame"/>
  <link name="realsense_gyro_optical_frame"/>
  <joint name="realsense_accel_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_accel_frame"/>
  </joint>
  <joint name="realsense_accel_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_accel_frame"/>
    <child link="realsense_accel_optical_frame"/>
  </joint>
  <joint name="realsense_gyro_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.01174 -0.00552 0.0051"/>
    <parent link="realsense_link"/>
    <child link="realsense_gyro_frame"/>
  </joint>
  <joint name="realsense_gyro_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
    <parent link="realsense_gyro_frame"/>
    <child link="realsense_gyro_optical_frame"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="jetson_nano_bracket_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/JetsonNanoBracket.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/JetsonNanoBracket.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="jetson_nano_bracket_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="jetson_nano_bracket_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.054 -0.155 0.0085"/>
  </joint>
  <link name="jetson_nano_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/JetsonNanoSimplified.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/JetsonNanoSimplified.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="jetson_nano_base_joint" type="fixed">
    <parent link="jetson_nano_bracket_link"/>
    <child link="jetson_nano_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="-1.57079632679 0 1.57079632679" xyz="0.0765 -0.079 0"/>
  </joint>
  <link name="left_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/WifiAntenna.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/WifiAntenna.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="left_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="left_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 3.14159265359" xyz="-0.054 -0.245 0"/>
  </joint>
  <link name="right_antenna_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/WifiAntenna.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/WifiAntenna.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="right_antenna_joint" type="fixed">
    <parent link="rear_mounting_plate_base_link"/>
    <child link="right_antenna_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0.054 -0.245 0"/>
  </joint>
  <!-- Adapted from the kinova-ros pkg -->
  <link name="lidar_mount_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/LiDAR_Mount.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/LiDAR_Mount.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="lidar_mount_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="lidar_mount_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 -0.095 0"/>
  </joint>
  <link name="rplidar_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/RPLidar.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/RPLidar.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="rplidar_joint" type="fixed">
    <parent link="lidar_mount_link"/>
    <child link="rplidar_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.0339"/>
  </joint>
  <link name="laser"/>
  <joint name="laser_joint" type="fixed">
    <parent link="rplidar_link"/>
    <child link="laser"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 1.57079632679" xyz="0 0 0.031"/>
  </joint>
  <!-- RpLidar A2 using GPU -->
  <gazebo reference="laser">
    <sensor name="rplidar_sensor" type="gpu_ray">
      <pose>0 0 0.0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>30</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159265</min_angle>
            <max_angle>3.14159265</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.125</min>
          <max>12.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.05</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_gpu_laser.so" name="gazebo_ros_rplidar_controller">
        <topicName>scan</topicName>
        <frameName>laser</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="realsense_depth_frame">
    <sensor name="openni_camera_camera" type="depth">
      <always_on>1</always_on>
      <visualize>true</visualize>
      <camera>
        <horizontal_fov>1.047</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <depth_camera>

        </depth_camera>
        <clip>
          <near>0.1</near>
          <far>10</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="realsense_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>realsense</cameraName>
        <frameName>realsense_depth_frame</frameName>
        <imageTopicName>color/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>color/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudCutoff>0.4</pointCloudCutoff>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0.0</CxPrime>
        <Cx>0.0</Cx>
        <Cy>0.0</Cy>
        <focalLength>0.0</focalLength>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo>
    <light name="left_headlight" type="spot">
      <pose>0.659915 -0.501804 1 0 -0 0</pose>
      <diffuse>0.5 0.5 0.5 1</diffuse>
      <specular>0.1 0.1 0.1 1</specular>
      <direction>0 0 -1</direction>
      <attenuation>
        <range>20</range>
        <constant>0.5</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <cast_shadows>0</cast_shadows>
      <spot>
        <inner_angle>0.6</inner_angle>
        <outer_angle>1</outer_angle>
        <falloff>1</falloff>
      </spot>
    </light>
  </gazebo>
</robot>

Let me know if more info is required.

micahpearlman commented 2 years ago

Apologies, you will have to walk me through this -- too much of a data dump to sort through. Not sure what link_origin_rpy is, not mentioned explicitly in the URDF link documentation? Seems like an XAcro thing? And what is the part of the URDF that is broken and what is expected (please avoid a fulll dump if you can)?

ghost commented 2 years ago

Sorry for the data dump and the confusion, I'm so used to my tools that I forgot to mention that link_origin_rpy is a xacro macro to define the link origin rpy as per the urdf link documentation at: https://wiki.ros.org/urdf/XML/link Here's how it's used inside my macro.

<origin xyz="${link_origin_xyz}" rpy="${link_origin_rpy}"/>

I'll try to be as clear as possible, using the rear_mounting_plate as an example. The CAD origin of the rear_mounting_plate is the same as the origin of the front_mounting_plate but in order to simplify the positioning of accessories on the mounting plate, I wanted to relocate its origin on the center hole at the front of the rear_mounting_plate. Then rotate it's origin so that it's properly oriented. image For some reason, while this works when displaying the urdf in RViz, it doesn't work when imported in Unity.

  <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/visual/RearMountingPlate.dae"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.155 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://leo_ssom_addons_description/meshes/collision/RearMountingPlate.stl"/>
      </geometry>
      <origin rpy="0 0 -1.57079632679" xyz="0.155 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="-0.045 0 0.0085"/>
  </joint>

When modifying the xacro definition to remove the use of <origin rpy="X Y Z"/> I get a properly imported urdf, but the link origins aren't where i'd like them to be. image

 <link name="rear_mounting_plate_base_link">
    <visual>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/visual/RearMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./leo_ssom_addons_description/meshes/collision/RearMountingPlate.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0085"/>
    </collision>
  </link>
  <joint name="rear_mounting_plate_joint" type="fixed">
    <parent link="front_mounting_plate_base_link"/>
    <child link="rear_mounting_plate_base_link"/>
    <axis xyz="0 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

I'll formulate it this way. As a user, I'd like to only keep a single urdf definition of my robot that can use the <origin rpy="X Y Z"/> argument and that can be imported in Unity without needing modifications.

Hope I made the issue clear enough..

micahpearlman commented 2 years ago

Again, apologies, totally lost -- maybe because it's Monday -- super appreciate your patience. I don't understand what the "good" URDF is and the "bad" URDF is above and what the difference are exactly? Is it like a 90 degree rotation about an axis? Or something more? Do you think it would be possible to replicate this with a super simple XAcro file with simple geo? (FYI: I know nearly nothing about XAcro other then how to convert to URDF)

BTW: If it is an issue with everything is rotated 90 degrees about an axis I suspect it may be an issue in the convert_meshes_to_obj.sh (https://github.com/fsstudio-team/zerosim_docker/blob/master/tools/convert_meshes_to_obj.sh) + the dae_to_obj.py script (https://github.com/fsstudio-team/zerosim_docker/blob/master/tools/dae_to_obj.py). It makes some assumptions about the models up and forward axis, specifically the line: bpy.ops.export_scene.obj(filepath=fobj, axis_forward='Y', axis_up='Z') that may be incorrect for your 3D models perhaps?

ghost commented 2 years ago

Thanks for you patience as well, I'm quite fluent, but English isn't my first language so there might be some stuff that's lost in translation.

FYI: xacro is just an xml macro language on top of the urdf xml. It makes life easier when you want to add links and joints in a modular way. As an example, this is the file that i use to add parts to the LeoRover original URDF:

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/e_stop.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/mounting_plates.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/realsense.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/jetson_nano.xacro" />
    <xacro:include filename="$(find leo_ssom_addons_description)/urdf/rplidar.xacro" />
</robot>

Link origin issue

I believe you found the root cause of the issue. To my knowledge, the link origin rotations and translations values aren't used when importing the URDF. I think that it could be handled in the conversion script probably with bpy.ops.transform.rotate(rpy) and bpy.ops.transform.translate(xyz). I guess it might also be handled here: https://github.com/fsstudio-team/ZeroSimROSUnity/blob/9adff2a83fb4cb9ec4207ca7e933ca56ae5ad01f/Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs#L113 and there: https://github.com/fsstudio-team/ZeroSimROSUnity/blob/9adff2a83fb4cb9ec4207ca7e933ca56ae5ad01f/Runtime/Scripts/Util/ImportExport/ZOImportURDF.cs#L213

I investigated by opening my .dae file in Blender.

image

I then manually applied the <origin rpy="0 0 -1.57079632679" xyz="0.155 0 -0.0085"/> rotations then translations.

image

The origin is now where the URDF is expecting it.

Validation in Blender with joint origin applied

After import image

After joint origin is applied the part is properly located and the joint is where it's expected to be. <origin rpy="0 0 0" xyz="-0.045 0 0.0085"/> image