URDF Import Behavior

Omniverse Isaac Sim
,
1

I’m experiencing issues importing the RB-KAIROS holonomic mobile manipulator into Isaac Sim 4.5. After acquiring the URDF from the official GitHub repository, I imported it into Isaac Sim using the following settings: movable base, acceleration drive type, and self-collision enabled.

However, when I press play, the robot immediately becomes unstable and flies off. I’ve attached a video demonstrating this behavior along with the URDF file I’m using.

When I disable self-collision, it is more stable, but then the links pass through themselves. Any suggestions on how to improve this?

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from rbkairos_franka.urdf.xacro     | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="rbkairos">
  <!-- ***************** -->
  <!-- Imported elements -->
  <!-- ***************** -->
  <gazebo reference="base_footprint">
    <material>Gazebo/Green</material>
  </gazebo>
  <!-- in kg-->
  <!-- in kg-->
  <!-- source en.wikipedia.org/wiki/List_of_moments_of_inertia-->
  <!-- This file unifies all sensors that can be used on simulation, defined as xacro:macros -->
  <!-- <xacro:macro name="sensor_ultrasonic_a21_gazebo">
    <gazebo reference="${prefix}_link">
			<xacro:if value="${gpu}">
        <xacro:property name="ray_type" value="gpu_ray" />
        <xacro:property name="plugin_lib" value="libgazebo_ros_gpu_laser.so" />
      </xacro:if>
      <xacro:unless value="${gpu}">
        <xacro:property name="ray_type" value="ray" />
        <xacro:property name="plugin_lib" value="libgazebo_ros_laser.so" />
      </xacro:unless>
			<sensor type="${ray_type}" name="${prefix}_sensor">
				<pose>0 0 0 0 0 0</pose>
				<visualize>false</visualize>
				<update_rate>40</update_rate>
				<ray>
					<scan>
						<horizontal>
							<samples>1081</samples>
							<resolution>1</resolution>
							<min_angle>${min_angle}</min_angle>
							<max_angle>${max_angle}</max_angle>
						</horizontal>
					</scan>
					<range>
						<min>0.06</min>
						<max>20.0</max>
						<! resolution>0.004363323</resolution >
						<resolution>0.004359297</resolution>
						<! resolution>0.001</resolution >
					</range>
					<noise>
						<type>gaussian</type>
						<mean>0.0</mean>
						<stddev>0.01</stddev>
					</noise>
				</ray>
				<plugin name="${prefix}_controller" filename="${plugin_lib}">
					<topicName>${prefix_topic}/scan</topicName>
					<frameName>/${prefix}_link</frameName>
				</plugin>
			</sensor>
	</gazebo>
  </xacro:macro>
  -->
  <!-- Includes -->
  <!-- Includes -->
  <!-- *************** -->
  <!-- Robots Elements -->
  <!-- *************** -->
  <link name="robotbase_footprint">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <joint name="robotbase_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.127"/>
    <!-- ${wheel_radius} property must be defined inside wheel description -->
    <parent link="robotbase_footprint"/>
    <child link="robotbase_link"/>
  </joint>
  <!-- BASE_LINK -->
  <link name="robotbase_link">
    <inertial>
      <mass value="125.0"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.391" ixy="0.004" ixz="0.0" iyy="6.853" iyz="0.0" izz="6.125"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/summit_xlsk_plus.stl"/>
      </geometry>
      <material name="black">
        <color rgba="0.0 0.0 0.0 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0 " xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/summit_xlsk_plus.dae"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="robotbase_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- CHAPAS INOX TAPAS -->
  <link name="robotbase_chapa">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/summit_xls_chapas_inox_tapas.stl"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>
  <joint name="robotbase_tapa_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0 0.56162"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_chapa"/>
  </joint>
  <!-- LOGO ROBOTNIK IZQUIERDO -->
  <link name="robotbase_logo_left">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/robotnik_logo_chasis.stl"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>
  <joint name="robotbase_logo_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.2894 0.35562"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_logo_left"/>
  </joint>
  <!-- LOGO ROBOTNIK DERECHO -->
  <link name="robotbase_logo_right">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/robotnik_logo_chasis.stl"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>
  <joint name="robotbase_logo_right_joint" type="fixed">
    <origin rpy="0 0 3.141592653589793" xyz="0.0 -0.28931 0.35562"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_logo_right"/>
  </joint>
  <!-- LOGO ROBOTNIK FRONTAL -->
  <link name="robotbase_logo_front">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/robotnik_logo_chasis.stl"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>
  <joint name="robotbase_logo_front_joint" type="fixed">
    <origin rpy="0 0 -1.5707963267948966" xyz="0.345 0.0 0.0"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_logo_front"/>
  </joint>
  <!-- LOGO ROBOTNIK TRASERO -->
  <link name="robotbase_logo_rear">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/bases/xls/robotnik_logo_chasis.stl"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>
  <joint name="robotbase_logo_rear_joint" type="fixed">
    <origin rpy="0 0 1.5707963267948966" xyz="-0.345 0.0 0.0"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_logo_rear"/>
  </joint>
  <!-- DOCKING CONTACT -->
  <link name="robotbase_docking_contact">
    </link>
  <joint name="robotbase_docking_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.35 0 0.09"/>
    <parent link="robotbase_link"/>
    <child link="robotbase_docking_contact"/>
  </joint>
  <sick_microscan3_support parent="robotbase_link" prefix="robotfront_laser">
    <origin rpy="0 0 -0.645772" xyz="0.28704 -0.22255 0.20212"/>
  </sick_microscan3_support>
  <sick_microscan3_support parent="robotbase_link" prefix="robotrear_laser">
    <origin rpy="0 0 2.496" xyz="-0.28704 +0.22255 0.20212"/>
  </sick_microscan3_support>
  <joint name="robotfront_right_wheel_joint" type="continuous">
    <parent link="robotbase_link"/>
    <child link="robotfront_right_wheel_link"/>
    <origin rpy="0 0 0" xyz="0.21528 -0.269 0.0"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>
  <link name="robotfront_right_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.2 0.2 0.2 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <!--sphere radius="${omni_wheel_radius}" /-->
        <!--
          <xacro:if value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae" />
          </xacro:if>
          <xacro:unless value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae" />
					</xacro:unless>
-->
        <cylinder length="0.087" radius="0.127"/>
        <!-- sphere radius="${omni_wheel_radius}"/-->
      </geometry>
    </collision>
    <inertial>
      <mass value="6.5"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0303095" ixy="0" ixz="0" iyy="0.05241925" iyz="0" izz="0.0303095"/>
    </inertial>
  </link>
  <!-- Transmission is important to link the joints and the controller (see summit_xl_control/config/summit_xl_control.yaml)-->
  <transmission name="robotfront_right_wheel_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="robotfront_right_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="robotfront_right_wheel_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="robotfront_right_wheel_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="robotfront_left_wheel_joint" type="continuous">
    <parent link="robotbase_link"/>
    <child link="robotfront_left_wheel_link"/>
    <origin rpy="0 0 0" xyz="0.21528 0.269 0.0"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>
  <link name="robotfront_left_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.2 0.2 0.2 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <!--sphere radius="${omni_wheel_radius}" /-->
        <!--
          <xacro:if value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae" />
          </xacro:if>
          <xacro:unless value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae" />
					</xacro:unless>
-->
        <cylinder length="0.087" radius="0.127"/>
        <!-- sphere radius="${omni_wheel_radius}"/-->
      </geometry>
    </collision>
    <inertial>
      <mass value="6.5"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0303095" ixy="0" ixz="0" iyy="0.05241925" iyz="0" izz="0.0303095"/>
    </inertial>
  </link>
  <!-- Transmission is important to link the joints and the controller (see summit_xl_control/config/summit_xl_control.yaml)-->
  <transmission name="robotfront_left_wheel_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="robotfront_left_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="robotfront_left_wheel_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="robotfront_left_wheel_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="robotback_left_wheel_joint" type="continuous">
    <parent link="robotbase_link"/>
    <child link="robotback_left_wheel_link"/>
    <origin rpy="0 0 0" xyz="-0.21528 0.269 0.0"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>
  <link name="robotback_left_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.2 0.2 0.2 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <!--sphere radius="${omni_wheel_radius}" /-->
        <!--
          <xacro:if value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae" />
          </xacro:if>
          <xacro:unless value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae" />
					</xacro:unless>
-->
        <cylinder length="0.087" radius="0.127"/>
        <!-- sphere radius="${omni_wheel_radius}"/-->
      </geometry>
    </collision>
    <inertial>
      <mass value="6.5"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0303095" ixy="0" ixz="0" iyy="0.05241925" iyz="0" izz="0.0303095"/>
    </inertial>
  </link>
  <!-- Transmission is important to link the joints and the controller (see summit_xl_control/config/summit_xl_control.yaml)-->
  <transmission name="robotback_left_wheel_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="robotback_left_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="robotback_left_wheel_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="robotback_left_wheel_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="robotback_right_wheel_joint" type="continuous">
    <parent link="robotbase_link"/>
    <child link="robotback_right_wheel_link"/>
    <origin rpy="0 0 0" xyz="-0.21528 -0.269 0.0"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="100" velocity="100"/>
    <joint_properties damping="0.0" friction="0.0"/>
  </joint>
  <link name="robotback_right_wheel_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae"/>
      </geometry>
      <material name="darkgrey">
        <color rgba="0.2 0.2 0.2 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
      <geometry>
        <!--sphere radius="${omni_wheel_radius}" /-->
        <!--
          <xacro:if value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_1.dae" />
          </xacro:if>
          <xacro:unless value="${reflect}">
              <mesh filename="/home/sesem/rbkairos_common/summit_xl_description//meshes/wheels/omni_wheel_2.dae" />
					</xacro:unless>
-->
        <cylinder length="0.087" radius="0.127"/>
        <!-- sphere radius="${omni_wheel_radius}"/-->
      </geometry>
    </collision>
    <inertial>
      <mass value="6.5"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0303095" ixy="0" ixz="0" iyy="0.05241925" iyz="0" izz="0.0303095"/>
    </inertial>
  </link>
  <!-- Transmission is important to link the joints and the controller (see summit_xl_control/config/summit_xl_control.yaml)-->
  <transmission name="robotback_right_wheel_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="robotback_right_wheel_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="robotback_right_wheel_joint_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="robotback_right_wheel_link">
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
    <fdir1 value="1 0 0"/>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <joint name="robotimu_joint" type="fixed">
    <axis xyz="1 0 0"/>
    <!--origin xyz="0 0 0.2825"/-->
    <origin rpy="0 0 0" xyz="0.127 -0.129 0.212"/>
    <parent link="robotbase_link"/>
    <child link="robotimu_link"/>
  </joint>
  <link name="robotimu_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0 0.01"/>
      <mass value="0.05"/>
      <inertia ixx="3.3333333333333337e-06" ixy="0" ixz="0" iyy="3.3333333333333337e-06" iyz="0" izz="3.3333333333333337e-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size=".001 .001 .001"/>
      </geometry>
    </collision>
  </link>
  <gazebo>
    <plugin filename="libhector_gazebo_ros_imu.so" name="robotimu_controller">
      <alwaysOn>true</alwaysOn>
      <topicName>imu/data</topicName>
      <serviceName>robotimuimu/service</serviceName>
      <bodyName>robotimu_link</bodyName>
      <frameId>robotimu_link</frameId>
      <updateRate>50.0</updateRate>
      <gaussianNoise>0.00</gaussianNoise>
      <yawOffset>0.0</yawOffset>
      <yawDrift>0.0</yawDrift>
      <yawGaussianNoise>0.0</yawGaussianNoise>
      <!-- angular velocities parameters -->
      <rateOffset>0.0 0.0 0.0</rateOffset>
      <rateDrift>0.0 0.0 0.0</rateDrift>
      <rateGaussianNoise>0.0 0.0 0.0</rateGaussianNoise>
      <!-- linear accelerations parameters -->
      <accelOffset>0.0 0.0 0.0</accelOffset>
      <accelDrift>0.0 0.0 0.0</accelDrift>
      <accelGaussianNoise>0.0 0.0 0.0</accelGaussianNoise>
    </plugin>
  </gazebo>
  <joint name="robotfront_laser_base_joint" type="fixed">
    <origin rpy="0 -3.141592653589793 2.356194490192345" xyz="0.29316 -0.22809 0.23162"/>
    <parent link="robotbase_link"/>
    <child link="robotfront_laser_base_link"/>
  </joint>
  <link name="robotfront_laser_base_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.010 0.010 0.010"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/robotnik_sensors//meshes/hokuyo_ust_20lx.dae"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0 0.035"/>
      <mass value="0.130"/>
      <inertia ixx="8.016666666666669e-05" ixy="0" ixz="0" iyy="8.016666666666669e-05" iyz="0" izz="5.4166666666666685e-05"/>
    </inertial>
  </link>
  <link name="robotfront_laser_link">
	</link>
  <joint name="robotfront_laser_joint" type="fixed">
    <parent link="robotfront_laser_base_link"/>
    <child link="robotfront_laser_link"/>
    <origin rpy="0 0 0" xyz="0.00 0.0 0.0474"/>
  </joint>
  <gazebo reference="robotfront_laser_link">
    <sensor name="robotfront_laser_sensor" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1081</samples>
            <resolution>1</resolution>
            <min_angle>-2.3562</min_angle>
            <max_angle>2.3562</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.06</min>
          <max>20.0</max>
          <!-- resolution>0.004363323</resolution -->
          <resolution>0.004359297</resolution>
          <!-- resolution>0.001</resolution -->
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="robotfront_laser_controller">
        <topicName>front_laser/scan</topicName>
        <frameName>/robotfront_laser_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="robotrear_laser_base_joint" type="fixed">
    <origin rpy="0 -3.141592653589793 -0.7853981633974483" xyz="-0.29316 0.22809 0.23162"/>
    <parent link="robotbase_link"/>
    <child link="robotrear_laser_base_link"/>
  </joint>
  <link name="robotrear_laser_base_link">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <box size="0.010 0.010 0.010"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/robotnik_sensors//meshes/hokuyo_ust_20lx.dae"/>
      </geometry>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0.0 0 0.035"/>
      <mass value="0.130"/>
      <inertia ixx="8.016666666666669e-05" ixy="0" ixz="0" iyy="8.016666666666669e-05" iyz="0" izz="5.4166666666666685e-05"/>
    </inertial>
  </link>
  <link name="robotrear_laser_link">
	</link>
  <joint name="robotrear_laser_joint" type="fixed">
    <parent link="robotrear_laser_base_link"/>
    <child link="robotrear_laser_link"/>
    <origin rpy="0 0 0" xyz="0.00 0.0 0.0474"/>
  </joint>
  <gazebo reference="robotrear_laser_link">
    <sensor name="robotrear_laser_sensor" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1081</samples>
            <resolution>1</resolution>
            <min_angle>-2.3562</min_angle>
            <max_angle>2.3562</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.06</min>
          <max>20.0</max>
          <!-- resolution>0.004363323</resolution -->
          <resolution>0.004359297</resolution>
          <!-- resolution>0.001</resolution -->
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="robotrear_laser_controller">
        <topicName>rear_laser/scan</topicName>
        <frameName>/robotrear_laser_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="robotfront_rgbd_camera_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.3585 0.0 0.25062"/>
    <parent link="robotbase_link"/>
    <child link="robotfront_rgbd_camera_link"/>
  </joint>
  <link name="robotfront_rgbd_camera_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/robotnik_sensors//meshes/orbbec_astra.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/robotnik_sensors//meshes/orbbec_astra.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="2.3437500000000006e-06" ixy="0" ixz="0" iyy="2.0833333333333336e-07" iyz="0" izz="2.4020833333333336e-06"/>
    </inertial>
  </link>
  <joint name="robotfront_rgbd_camera_rgb_joint" type="fixed">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0125 0.0150"/>
    <parent link="robotfront_rgbd_camera_link"/>
    <child link="robotfront_rgbd_camera_rgb_frame"/>
  </joint>
  <link name="robotfront_rgbd_camera_rgb_frame">
    <inertial>
      <mass value="0.297"/>
      <origin xyz="-0.02 -0.0125 0.0"/>
      <inertia ixx="0.0006960937500000001" ixy="0" ixz="0" iyy="6.1875e-05" iyz="0" izz="0.00071341875"/>
    </inertial>
  </link>
  <joint name="robotfront_rgbd_camera_rgb_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0.0 0.0 0.0"/>
    <parent link="robotfront_rgbd_camera_rgb_frame"/>
    <child link="robotfront_rgbd_camera_rgb_optical_frame"/>
  </joint>
  <link name="robotfront_rgbd_camera_rgb_optical_frame">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
      <inertia ixx="2.3437500000000006e-06" ixy="0" ixz="0" iyy="2.0833333333333336e-07" iyz="0" izz="2.4020833333333336e-06"/>
    </inertial>
  </link>
  <joint name="robotfront_rgbd_camera_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0375 0.0150"/>
    <parent link="robotfront_rgbd_camera_link"/>
    <child link="robotfront_rgbd_camera_depth_frame"/>
  </joint>
  <link name="robotfront_rgbd_camera_depth_frame">
    <inertial>
      <mass value="0.001"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="2.3437500000000006e-06" ixy="0" ixz="0" iyy="2.0833333333333336e-07" iyz="0" izz="2.4020833333333336e-06"/>
    </inertial>
  </link>
  <joint name="robotfront_rgbd_camera_depth_optical_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0"/>
    <parent link="robotfront_rgbd_camera_depth_frame"/>
    <child link="robotfront_rgbd_camera_depth_optical_frame"/>
  </joint>
  <link name="robotfront_rgbd_camera_depth_optical_frame">
    <inertial>
      <mass value="0.001"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="2.3437500000000006e-06" ixy="0" ixz="0" iyy="2.0833333333333336e-07" iyz="0" izz="2.4020833333333336e-06"/>
    </inertial>
  </link>
  <gazebo reference="robotfront_rgbd_camera_link">
    <sensor name="robotfront_rgbd_camera_depth_sensor" type="depth">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <camera>
        <horizontal_fov>1.0471975511965976</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>3.5</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="robotfront_rgbd_camera_controller">
        <cameraName>front_rgbd_camera</cameraName>
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
        <frameName>/robotfront_rgbd_camera_depth_optical_frame</frameName>
        <baseline>0.1</baseline>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
        <pointCloudCutoff>0.4</pointCloudCutoff>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="fr3_joint_robotbase_link" type="fixed">
    <parent link="robotbase_link"/>
    <child link="fr3_link0"/>
    <origin rpy="0 0 -3.141592653589793" xyz="-0.2731 0 0.56162"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link0">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link0.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link0_sc">
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="-0.075 0 0.06"/>
      <geometry>
        <cylinder length="0.03" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.06 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.09 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link0"/>
    <child link="fr3_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link1">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link1.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link1_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.1915"/>
      <geometry>
        <cylinder length="0.283" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.05000000000000002"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.33299999999999996"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link1"/>
    <child link="fr3_link1_sc"/>
  </joint>
  <joint name="fr3_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="fr3_link0"/>
    <child link="fr3_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.3093" upper="2.3093" velocity="2.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.3093" soft_upper_limit="2.3093"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link2">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link2.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link2_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link2"/>
    <child link="fr3_link2_sc"/>
  </joint>
  <joint name="fr3_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="fr3_link1"/>
    <child link="fr3_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.5133" upper="1.5133" velocity="1.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.5133" soft_upper_limit="1.5133"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link3">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link3.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link3_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.145"/>
      <geometry>
        <cylinder length="0.15" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06999999999999999"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.21999999999999997"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link3"/>
    <child link="fr3_link3_sc"/>
  </joint>
  <joint name="fr3_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="fr3_link2"/>
    <child link="fr3_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.4937" upper="2.4937" velocity="1.5"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.4937" soft_upper_limit="2.4937"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link4">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link4.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link4_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.06"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link4"/>
    <child link="fr3_link4_sc"/>
  </joint>
  <joint name="fr3_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="fr3_link3"/>
    <child link="fr3_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.7478" upper="-0.4461" velocity="1.25"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7478" soft_upper_limit="-0.4461"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link5">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link5.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link5_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.26"/>
      <geometry>
        <cylinder length="0.1" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.21000000000000002"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.31"/>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0.08" xyz="0 0.08 -0.13"/>
      <geometry>
        <cylinder length="0.14" radius="0.055"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.08 -0.06"/>
      <geometry>
        <sphere radius="0.055"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.08 -0.2"/>
      <geometry>
        <sphere radius="0.055"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link5"/>
    <child link="fr3_link5_sc"/>
  </joint>
  <joint name="fr3_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="fr3_link4"/>
    <child link="fr3_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.48" upper="2.48" velocity="3.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.48" soft_upper_limit="2.48"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link6">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link6.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link6_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.03"/>
      <geometry>
        <cylinder length="0.08" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.010000000000000002"/>
      <geometry>
        <sphere radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.07"/>
      <geometry>
        <sphere radius="0.08"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_link6"/>
    <child link="fr3_link6_sc"/>
  </joint>
  <joint name="fr3_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="fr3_link5"/>
    <child link="fr3_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="0.8521" upper="4.2094" velocity="1.5"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="0.8521" soft_upper_limit="4.2094"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_link7">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/link7.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_link7_sc">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.01"/>
      <geometry>
        <cylinder length="0.14" radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 0.08"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 -0.060000000000000005"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267948966 0" xyz="0.06 0 0.082"/>
      <geometry>
        <cylinder length="0.01" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.065 0 0.082"/>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.055 0 0.082"/>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="fr3_link7"/>
    <child link="fr3_link7_sc"/>
  </joint>
  <joint name="fr3_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="fr3_link6"/>
    <child link="fr3_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.6895" upper="2.6895" velocity="3.0"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.6895" soft_upper_limit="2.6895"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="fr3_link8"/>
  <joint name="fr3_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="fr3_link7"/>
    <child link="fr3_link8"/>
  </joint>
  <joint name="fr3_hand_joint" type="fixed">
    <parent link="fr3_link8"/>
    <child link="fr3_hand"/>
    <origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="fr3_hand">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/hand.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/collision/hand.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="fr3_hand_sc">
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.04"/>
      <geometry>
        <cylinder length="0.1" radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.05 0.04"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 -0.05 0.04"/>
      <geometry>
        <sphere radius="0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.1"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0.05 0.1"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 -0.05 0.1"/>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="fr3_hand_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="fr3_hand"/>
    <child link="fr3_hand_sc"/>
  </joint>
  <!-- Define the hand_tcp frame -->
  <link name="fr3_hand_tcp"/>
  <joint name="fr3_hand_tcp_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.1034"/>
    <parent link="fr3_hand"/>
    <child link="fr3_hand_tcp"/>
  </joint>
  <link name="fr3_leftfinger">
    <visual>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/finger.dae"/>
      </geometry>
    </visual>
    <!-- screw mount -->
    <collision>
      <origin rpy="0 0 0" xyz="0 18.5e-3 11e-3"/>
      <geometry>
        <box size="22e-3 15e-3 20e-3"/>
      </geometry>
    </collision>
    <!-- cartriage sledge -->
    <collision>
      <origin rpy="0 0 0" xyz="0 6.8e-3 2.2e-3"/>
      <geometry>
        <box size="22e-3 8.8e-3 3.8e-3"/>
      </geometry>
    </collision>
    <!-- diagonal finger -->
    <collision>
      <origin rpy="0.5235987755982988 0 0" xyz="0 15.9e-3 28.35e-3"/>
      <geometry>
        <box size="17.5e-3 7e-3 23.5e-3"/>
      </geometry>
    </collision>
    <!-- rubber tip with which to grasp -->
    <collision>
      <origin rpy="0 0 0" xyz="0 7.58e-3 45.25e-3"/>
      <geometry>
        <box size="17.5e-3 15.2e-3 18.5e-3"/>
      </geometry>
    </collision>
  </link>
  <link name="fr3_rightfinger">
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/sesem/rbkairos_common/franka_description//meshes/visual/finger.dae"/>
      </geometry>
    </visual>
    <!-- screw mount -->
    <collision>
      <origin rpy="0 0 0" xyz="0 -18.5e-3 11e-3"/>
      <geometry>
        <box size="22e-3 15e-3 20e-3"/>
      </geometry>
    </collision>
    <!-- cartriage sledge -->
    <collision>
      <origin rpy="0 0 0" xyz="0 -6.8e-3 2.2e-3"/>
      <geometry>
        <box size="22e-3 8.8e-3 3.8e-3"/>
      </geometry>
    </collision>
    <!-- diagonal finger -->
    <collision>
      <origin rpy="-0.5235987755982988 0 0" xyz="0 -15.9e-3 28.35e-3"/>
      <geometry>
        <box size="17.5e-3 7e-3 23.5e-3"/>
      </geometry>
    </collision>
    <!-- rubber tip with which to grasp -->
    <collision>
      <origin rpy="0 0 0" xyz="0 -7.58e-3 45.25e-3"/>
      <geometry>
        <box size="17.5e-3 15.2e-3 18.5e-3"/>
      </geometry>
    </collision>
  </link>
  <joint name="fr3_finger_joint1" type="prismatic">
    <parent link="fr3_hand"/>
    <child link="fr3_leftfinger"/>
    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
    <axis xyz="0 1 0"/>
    <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
    <dynamics damping="0.3"/>
  </joint>
  <joint name="fr3_finger_joint2" type="prismatic">
    <parent link="fr3_hand"/>
    <child link="fr3_rightfinger"/>
    <origin rpy="0 0 0" xyz="0 0 0.0584"/>
    <axis xyz="0 -1 0"/>
    <limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
    <mimic joint="fr3_finger_joint1"/>
    <dynamics damping="0.3"/>
  </joint>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="ros_control">
      <robotParam>robot_description</robotParam>
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_planar_move.so" name="omni_steering">
      <commandTopic>robotnik_base_control/cmd_vel</commandTopic>
      <odometryTopic>robotnik_base_control/odom</odometryTopic>
      <odometryFrame>robotodom</odometryFrame>
      <odometryRate>50.0</odometryRate>
      <robotBaseFrame>robotbase_footprint</robotBaseFrame>
      <publishTF>True</publishTF>
      <enableYAxis>true</enableYAxis>
    </plugin>
  </gazebo>
</robot>
3

Hi @sesem738, thank you for posting your question. It is likely that when importing the URDF, the bodies end up in collision because of the way Isaac Sim automatically generates collision. It jumps around because the constraints of the joints cause it to be continuously in collision. A couple of suggestions to fix this:

  1. Import with Convex Decomposition option so that the collision boxes are smaller and less chance for them to overlap.
  2. Figure out which links (likely the wheels is my guess) are in collision and deleted their colliders if not needed.
  3. If the wheel collisions are needed then add them to a collision filter so they don’t collider with each other (the pins of the mecanum wheel that is) but still can collide with the ground: Colliders — Omni Physics

Hope that helps

Michael

4

Hi @michalin, this is what the colliders look like.

Screenshot from 2025-04-09 21-04-10
Screenshot from 2025-04-09 21-04-10408×613 144 KB

Screenshot from 2025-04-09 21-03-40
Screenshot from 2025-04-09 21-03-40570×782 282 KB

5

Could you share the USD for this setup? It would speed up debugging. Thanks!

6

Hi @michalin, here it is.

rbk_fr3.zip (6.9 MB)

7

Hi @sesem738, I have tried the asset that you sent me and it seems like the behavior you showed is no longer happening. I am guessing that the original problem was that the bodies were in self-collision all the time. My suggestion would be to remove the collider corresponding to the cart chassis. I think for you that is located in the “rbk_fr3_physics.usd” file. The mesh is located in the Prim path /colliders/robotbase_footprint/summit_xlsk_plus/mesh. Just uncheck the Collision Enabled box in the Collider property.

Sorry for the delay in my response!