My robot becomes very slow when i use garage world

[ELTechie009]

Dear Team,

I am currently working with a robot equipped with 4 wheels and 2 arms. In the past, I utilized a custom model created with open-source meshes, and there were no issues with robot movement. However, after transitioning to the Garage World environment for navigation tasks, I have observed a significant reduction in the robot’s speed when attempting to reach navigation goals. Could you provide insights or suggestions on potential causes for this decrease in speed?

Best regards,
Kumar.

xacro file goes like this:

<!--arm2 base link-->
    <joint name="arm2_base_joint" type="revolute">
        <axis xyz="0 0 1"/>
        <parent link="mount_link"/>
        <child link="arm2_base_link"/>
        <origin rpy="-${pi/3.5} 0 ${-pi/2}" xyz="0.23 0.08 0.08"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="arm2_base_link">
        <inertial>
        <origin xyz="7.7128E-09 -0.063005 -3.01969999961422E-08" rpy="0 0 0"/>
        <mass value="1.6004"/>
        <inertia ixx="0.00552196561445819" ixy="7.9550614501301E-10" ixz="-1.34378458924839E-09" iyy="0.00352397447953875" iyz="-1.10071809773382E-08" izz="0.00553739792746489"/>
        </inertial>
        <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/arm_base.stl"/>
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/arm_base.stl"/>
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_base_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--arm2 bicep link-->
    <joint name="arm2_shoulder_joint" type="revolute">
        <axis xyz="0 1 0"/>
        <parent link="arm2_base_link"/>
        <child link="arm2_bicep_link"/>
        <origin rpy="0 0 1.5708" xyz="-0.05166 0.0 0.20271"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="arm2_bicep_link">
        <inertial>
        <origin xyz="0.12821 3.5589E-06 0.052492" rpy="0 0 0"/>
        <mass value="1.1198"/>
        <inertia ixx="0.0012474" ixy="-5.4004E-07" ixz="-0.0013148" iyy="0.0072923" iyz="-1.8586E-07" izz="0.0068178"/>
        </inertial>
        <visual>
        <origin xyz="0 0 0" rpy="0 -1.5708 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/bicep.stl"/>
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0 0 0" rpy="0 -1.5708 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/bicep.stl"/>
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_bicep_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--arm2 bottom wrist link-->
    <joint name="arm2_bottom_wrist_joint" type="revolute">
        <axis xyz="0 1 0"/>
        <parent link="arm2_bicep_link"/>
        <child link="arm2_bottom_wrist_link"/>
        <origin rpy="0 0 0" xyz="-0.00001 -0.055 0.27"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="arm2_bottom_wrist_link">
        <inertial>
        <origin xyz="-9.1053E-08 -0.069257 -1.86629999995759E-07" rpy="0 0 0"/>
        <mass value="0.27721"/>
        <inertia ixx="0.00104290750143942" ixy="4.37155302268076E-09" ixz="-2.45049603914627E-09" iyy="0.000380518373895034" iyz="-7.56009835172156E-09" izz="0.00106006525067445"/>
        </inertial>
        <visual>
        <origin xyz="0 0 0.13522" rpy="3.14 0 1.5708"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/wrist.stl"/>
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0 0 0.13522" rpy="3.14 0 1.5708"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/wrist.stl"/>
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_bottom_wrist_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--arm2 elbow link-->
    <joint name="arm2_elbow_joint" type="revolute">
        <axis xyz="0 0 1"/>
        <parent link="arm2_bottom_wrist_link"/>
        <child link="arm2_elbow_link"/>
        <origin rpy="0 0 0" xyz="0.0 0 0.13522"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="arm2_elbow_link">
        <inertial>
        <origin xyz="-0.11109 1.1476E-08 0.046469" rpy="0 0 0"/>
        <mass value="0.84845"/>
        <inertia ixx="0.00079656" ixy="-7.8011E-10" ixz="0.00053616" iyy="0.003576" iyz="4.6326E-10" izz="0.0033698"/>
        </inertial>
        <visual>
        <origin xyz="0.0 0.05163 0.20994" rpy="0 -1.5708 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/elbow.stl"/>
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0.0 0.05163  0.20994" rpy="0 -1.5708 0"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/elbow.stl"/>
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_elbow_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--arm2 top wrist link-->
    <joint name="arm2_top_wrist_joint" type="revolute">
        <axis xyz="0 1 0"/>
        <parent link="arm2_elbow_link"/>
        <child link="arm2_top_wrist_link"/>
        <origin rpy="0 0 0" xyz="0.0 0 0.20994"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="arm2_top_wrist_link">
        <inertial>
        <origin xyz="-9.1053E-08 -0.069257 -1.86629999995759E-07" rpy="0 0 0"/>
        <mass value="0.27721"/>
        <inertia ixx="0.00104290750143942" ixy="4.37155302268076E-09" ixz="-2.45049603914627E-09" iyy="0.000380518373895034" iyz="-7.56009835172156E-09" izz="0.00106006525067445"/>
        </inertial>
        <visual>
        <origin xyz="0 0 0.13522" rpy="3.14 0 1.5708"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/wrist.stl"/>
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0 0 0.13522" rpy="3.14 0 1.5708"/>
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/wrist.stl"/>
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_top_wrist_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--end effector2 base-->
    <joint name="end_effector2_base_joint" type="revolute">
        <axis xyz="0 0 1"/>
        <parent link="arm2_top_wrist_link"/>
        <child link="end_effector2_base_link"/>
        <origin rpy="0 0 0" xyz="0.0 0.0 0.14"/>
        <limit effort="300" lower="${-170 * 3.14 / 180}" upper="${170 * 3.14 / 180}" velocity="10"/>
    </joint>

    <link name="end_effector2_base_link">
        <inertial>
        <origin xyz="-3.9257E-06 -0.00027208 0.037806" rpy="0 0 0" />
        <mass value="0.34365" />
        <inertia ixx="0.00036805" ixy="-7.2868E-08" ixz="-5.0168E-08" iyy="0.0003341" iyz="-2.6462E-06" izz="0.00022294" />
        </inertial>
        <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/end_effector_base.stl" scale="1.0 1.5 1.0" />
        </geometry>
        <material name="orange"/>
        </visual>
        <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/end_effector_base.stl" scale="1.0 1.5 1.0" />
        </geometry>
        </collision>
    </link>

    <gazebo reference="end_effector2_base_link">
        <material>Gazebo/Orange</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
    </gazebo>

<!--end effector2 finger1-->
    <joint name="arm2_finger1_joint" type="prismatic">
        <origin xyz="0.0132 0.0400 0.093" rpy="0 0 -1.5708" /> <!-- <origin xyz="0.0132 0.0265 0.093" rpy="0 0 -1.5708" /> -->
        <parent link="end_effector2_base_link" />
        <child link="arm2_finger1_link" />
        <axis xyz="1 0 0" />
        <limit lower="0" upper="0.037" effort="140" velocity="0.1" /> <!-- upper="0.035" -->
    </joint>

    <link name="arm2_finger1_link">
        <inertial>
        <origin xyz="0.0030491 -0.0066024 0.0064209" rpy="0 0 0" />
        <mass value="0.014278" />
        <inertia ixx="1.7567E-06" ixy="-5.7832E-08" ixz="2.6803E-08" iyy="2.3034E-06" iyz="2.2145E-07" izz="1.3354E-06" />
        </inertial>
        <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/end_effector_finger1.stl" scale="1.5 1.0 1.5" />
        </geometry>
        <material name="yellow"/>
        </visual>
        <collision name="arm2_finger1_collision">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/end_effector_finger1.stl" scale="1.5 1.0 1.5" />
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_finger1_link">
        <material>Gazebo/Yellow</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
        <kp>1000000.0</kp>
        <kd>1.0</kd>
        <minDepth>0.003</minDepth>
        <maxVel>0</maxVel>
        <torsional>
            <ode>
                <coefficient>1000</coefficient>
                <slip>0</slip>
            </ode>
        </torsional>
    <!--tactile sensor finger1-->
        <sensor name="arm2_finger1_contact_sensor" type="contact">
        <selfCollide>true</selfCollide>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <contact>
        <collision>arm2_finger1_link_fixed_joint_lump__arm2_finger1_collision_collision</collision>
        </contact>
        <plugin name="arm2_finger1_bumper_controller" filename="libgazebo_ros_bumper.so">
        <ros>
            <namespace>tactile</namespace>
            <remapping>bumper_states:=arm2_finger1</remapping>
        </ros>
        <frameName>arm2_finger1_tactile</frameName>
        </plugin>
    </sensor>
    </gazebo>
    
<!--end effector2 finger2-->
    <joint name="arm2_finger2_joint" type="prismatic">
        <origin xyz="-0.0132 -0.0400 0.093" rpy="0 0 1.5708" /> <!-- <origin xyz="-0.0132 -0.0265 0.093" rpy="0 0 1.5708" /> -->
        <parent link="end_effector2_base_link" />
        <child link="arm2_finger2_link" />
        <axis xyz="1 0 0" />
        <limit lower="0" upper="0.037" effort="140" velocity="0.1" /> <!-- upper="0.035" -->
        <!-- <mimic joint="arm2_finger1_joint" multiplier="1" offset="0"/> -->    <!-- not require for now -->
    </joint>

    <link name="arm2_finger2_link">
        <inertial>
        <origin xyz="0.0030491 -0.0066024 0.0064209" rpy="0 0 0" />
        <mass value="0.014278" />
        <inertia ixx="1.7567E-06" ixy="-5.7832E-08" ixz="2.6803E-08" iyy="2.3034E-06" iyz="2.2145E-07" izz="1.3354E-06" />
        </inertial>
        <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/visual/end_effector_finger2.stl" scale="1.5 1.0 1.5" />
        </geometry>
        <material name="yellow"/>
        </visual>
        <collision name="arm2_finger2_collision">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <mesh filename="package://mobdualarm/meshes/collision/end_effector_finger2.stl" scale="1.5 1.0 1.5" />
        </geometry>
        </collision>
    </link>

    <gazebo reference="arm2_finger2_link">
        <material>Gazebo/Yellow</material>
        <mu1>0.2</mu1>
        <mu2>0.2</mu2>
        <turnGravityOff>true</turnGravityOff>
        <implicitSpringDamper>1</implicitSpringDamper>
        <kp>1000000.0</kp>
        <kd>1.0</kd>
        <minDepth>0.003</minDepth>
        <maxVel>0</maxVel>
        <torsional>
            <ode>
                <coefficient>1000</coefficient>
                <slip>0</slip>
            </ode>
        </torsional>
    <!--tactile sensor finger2-->
        <sensor name="arm2_finger2_contact_sensor" type="contact">
        <selfCollide>true</selfCollide>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <contact>
        <collision>arm2_finger2_link_fixed_joint_lump__arm2_finger2_collision_collision</collision>
        </contact>
        <plugin name="arm2_finger2_bumper_controller" filename="libgazebo_ros_bumper.so">
        <ros>
            <namespace>tactile</namespace>
            <remapping>bumper_states:=arm2_finger2</remapping>
        </ros>
        <frameName>arm2_finger2_tactile</frameName>
        </plugin>
        </sensor>
    </gazebo>
    
    <!--grasp plugin arm2 -->
    <gazebo>
	<plugin name="arm2_gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
    <arm>
       <arm_name>arm2</arm_name>
       <palm_link> end_effector2_base_link  </palm_link>
       <gripper_link> arm2_finger1_link </gripper_link>
       <gripper_link> arm2_finger2_link </gripper_link>
    </arm>
    <forces_angle_tolerance>100</forces_angle_tolerance>
    <update_rate>4</update_rate>
    <grip_count_threshold>1</grip_count_threshold>
    <max_grip_count>8</max_grip_count>
    <release_tolerance>0.05</release_tolerance>
    <disable_collisions_on_attach>false</disable_collisions_on_attach>
    <contact_topic>__default_topic__</contact_topic>
    </plugin>
</gazebo>

<!-- fix moveit mimc joint not found in robot model error -->
<!-- <joint name="arm2_finger2_joint_mimic" type="fixed">
    <parent link="end_effector2_base_link" />
    <child link="arm2_dummy_mimic_fix" />
</joint>
<link name="arm2_dummy_mimic_fix"/>
 -->