search

gazebosim / gazebo-classic

Gazebo classic. For the latest version, see https://github.com/gazebosim/gz-sim
http://classic.gazebosim.org/
Other
1.17k stars 477 forks source link

Issue with libSimpleTrackedVehicalPlugin and urdf model #3391

Open 1456592699 opened 1 month ago

1456592699 commented 1 month ago

Hi, I'm currently working on the simulation of a tracked vehicle using ROS Noetic and Gazebo 11. I want to use a URDF file to load the robot into Gazebo because I also need to use Rviz in my future work. I see the tracked_vehicle_simple.world in Gazebo 11. I tested it by launching the following launch file

    <launch>
      <include file='$(find gazebo_ros)/launch/empty_world.launch'>
        <arg name="world_name" value="$(find gazebo_ros_2d_map_plugin)/worlds/tracked_vehicle_simple.world" />
        <arg name="paused" value="false"/>
        <arg name="use_sim_time" value="true"/>
        <arg name="gui" value="true"/>
        <arg name="headless" value="false"/>
        <arg name="debug" value="false"/>
      </include>
    </launch>

It appears that the plugin libSimpleTrackedVehiclePlugin can work correctly (I can see the topics /gazebo/default/simple_tracked/cmd_vel and /gazebo/default/simple_tracked/cmd_vel_twist through the command gz topic list, and the robot will move when I select the robot model and press the directional keys on my keyboard).

Then, I transformed the SDF file into a URDF file and added the libSimpleTrackedVehiclePlugin into the URDF file like this

    <robot name="/">
        <link name="simple_tracked__base_link">
            ...
        </link>
        ... <!-- other links -->
        <joint name="joint1" type="fixed">
            ...
        </joint>
        ... <!-- other joints -->
        <gazebo reference="simple_tracked__base_link">
            ...
        </gazebo>
        ... <!-- other gazebo settings -->
        <gazebo>
        <plugin filename="libSimpleTrackedVehiclePlugin.so" name="simple_tracked_vehicle">
          <body>simple_tracked__base_link</body>
          <left_track>simple_tracked__left_track</left_track>
          <right_track>simple_tracked__right_track</right_track>
          <left_flipper>simple_tracked__front_left_flipper</left_flipper>
          <right_flipper>simple_tracked__front_right_flipper</right_flipper>
          <right_flipper>simple_tracked__rear_right_flipper</right_flipper> 
          <track_mu>2</track_mu>
          <track_mu2>0.5</track_mu2>
        </plugin>
      </gazebo>
    </robot>

And I wrote the launch file like this:

    <launch>
        <param name="robot_description" textfile="$(find gazebo_ros_2d_map_plugin)/models/simple_tracked/model.urdf" />
        <include file="$(find gazebo_ros)/launch/empty_world.launch" />
        <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model simple_tracked -param robot_description" /> <!-- Corrected node name -->
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gazebo_ros_2d_map_plugin)/rviz/simple_tracked.rviz"/>
    </launch>

When I launch the file, I can't see the two topics subscribed by the libSimpleTrackedVehiclePlugin through gz topic list. It seems that the plugin didn't work properly, but I don't know why. I would appreciate it if you could help me find the reason why the plugin didn't work properly in my URDF file or give me some advice. Thank you very much!

traversaro commented 1 month ago

To debug, you can manually transform the URDF to SDF using gz sdf to understand if something is going wrong in the conversion (that is the one that Gazebo does internally when loading a URDF).

1456592699 commented 1 month ago

Thank you so much for answering my question! I used gz sdf to transform the urdf file into a sdf file,the plugin do not work proprely in the new sdf file. Then I check the difference between the new sdf file. I found that the joint relationship was changed. For example, I wrote joint in the urdf file like this:

<joint name="simple_tracked__front_left_flipper_j" type="revolute">
    <parent link="simple_tracked__left_track"/>
    <child link="simple_tracked__front_left_flipper"/>
    <origin xyz="0.25   0.0735 0.0195" rpy="0  -0.5  0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.2" upper="0.2" effort="-1.0" velocity="-1.0"/>
  </joint>

But in the output sdf file, the joint is:

<joint name='simple_tracked__front_left_flipper_j' type='revolute'>
        <pose relative_to='simple_tracked__base_link'>0.25 0.272 0.0195 0 -0.5 0</pose>
        <parent>simple_tracked__base_link</parent>
        <child>simple_tracked__front_left_flipper</child>
        <axis>
          <xyz>0 1 0</xyz>
          <limit>
            <lower>-0.2</lower>
            <upper>0.2</upper>
            <effort>-1</effort>
            <velocity>-1</velocity>
          </limit>
          <dynamics>
            <spring_reference>0</spring_reference>
            <spring_stiffness>0</spring_stiffness>
          </dynamics>
        </axis>
      </joint>

The parent link is not left_track in the urdf file, but changed to base_link. I don't know why this change happened. Are there something wrong with my urdf file?

Finally, thanks again for your response!

traversaro commented 1 month ago

Unless you share the full URDF/SDF, it is difficult to help you. My guess is that somewhere there is some fixed link somewhere in your model and so the left_track link gets lumped in its parent?

1456592699 commented 1 month ago

Thank you so much for answering my question! You are right, the left_track link is fixed to base_link so in sdf file they are lumped. I tried to use preserveFixedJoint but it didn't work.

  <joint name="simple_tracked__left_track_j" type="fixed">
    <parent link="simple_tracked__base_link"/>
    <child link="simple_tracked__left_track"/>
    <origin xyz="0     0.1985 0" rpy="0 0 0"/>
    <axis xyz="0 0 0"/>
    <limit lower="0" upper="0" effort="0" velocity="0"/>
    <preserveFixedJoint>true</preserveFixedJoint>
  </joint>

model.txt I was wondering if you have time to help me check my urdf file(In order to upload the file, I changed the file's suffix to txt, just modify the suffix to urdf when viewing.). I used pysdf to transform the robot in tracked_vehicle_simple.world into urdf file. And then I found it can not be seen in gazebo, so I add the gazebo materal label into the urdf like 

<gazebo reference="simple_tracked__base_link">
    <material>Gazebo/Red</material>
  </gazebo>

So that it can be seen in Gazebo. Then I add gazebo plugin into the urdf file, it seems that the libKeysToCmdVelPlugin can work properly(I can see the topic it published /gazebo/default/simple_tracked/cmd_vel_twist by using gz topic -l). But the libSimpleTrackedVehiclePlugin didn't work.

Thanks again for your help!

traversaro commented 1 month ago

Can you put the link before the joints in your URDF? Just to exclude that there is a bug related to the order in which the elements are parsed.