2
$\begingroup$

I was able to launch my robot model in rviz2 - however I used the Solidworks to URDF exporter to generate my urdf file which I have found to be very unstable. For this reason I just let it use automatically generated coordinate system for the robot. However, now my model is facing up and needs to be rotated 90 degrees about the x axis relative to the base links coordinate frame. I know its possible to manually try and edit each links rpy origins within the urdf file but this would take an intense amount of trial and error and headache. Every link seems to have a different coordinate frame (xyz) axis relative to one another.

So is there anyway to globally rotate the entire robot model instead of trying to trial and error one link at a time from within the URDF - or am I thinking about this totally wrong? I basically want to get the bottom of the quadruped parallel to the ground plane in rviz and then try and adjust the xyz origins globally to raise the entire robot so its not in the ground plane (if possible)

If anyone could help or point me in the right direction I would greatly appreciate it! I will attach screenshots for more reference.

Base link xyz axis enter image description here

Showing additional link's xyz axis relative to base link enter image description here

$\endgroup$

1 Answer 1

1
$\begingroup$

I don't have that much experience with URDF but I think a feasible solution would be to create another link called base_link as a parent of the main frame of your current model (I guess base_link_chassis).

In the joint between the two frames, you could define the rotation needed to get your model parallel to the floor plane. The X axis (red) of base_link should point forward in the robot preferred direction of movement, the Y axis to the left (green) and the Z axis (blue) upwards.

Additionally, make sure that base_link is centered according to the robot model (or in a position that makes things easier for your specific application). Usually, a centered base_link makes navigation tasks easier, as you can assume a robot radius instead of the robot footprint.

However, if you find the time in a future, try to clean the frame tree and give to each of the links a meaning for its position and orientation.

For more specific conventions related to coordinate frames, you can refer to this REP.

Besides the physical meaning of links when it refers to robot movement, you can also think about links more broadly as change in coordinate system. That is what you want to achieve in your application, you want to change how the model looks like according to a given reference system. Instead of changing all the links orientation, we could add another reference system where you include this rotation.

I'll use as a base example some of the urdf of the URDF tutorials, specifically the Origins example. You can also take a look to the rest of the URDFs used here.

The urdf looks like this:

<?xml version="1.0"?>
<robot name="origins">
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>

</robot>

It consists of 2 links (base_link and right_leg) with visual information and 1 joint (base_to_right_leg) that joins both of them with a given distance (as defined in <origin xyz="0 -0.22 0.25"/>).

We can add an additional link (added_base) to become the root link as in the following example:

<?xml version="1.0"?>
<robot name="origins">
  
  <!-- New link with no visual information -->
  <link name="added_base">  
  </link>

  <!-- New joint that defines rotation and positioning -->
  <joint name="added_base_to_base_link" type="fixed"> 
    <parent link="added_base"/>
    <child link="base_link"/>
    <!-- Define rotation and positioning here -->
    <origin  rpy="0 1.57075 0" xyz="0 -0.22 0.25"/>
  </joint>

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </visual>
  </link>

  <link name="right_leg">
    <visual>
      <geometry>
        <box size="0.6 0.1 0.2"/>
      </geometry>
      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
    </visual>
  </link>

  <joint name="base_to_right_leg" type="fixed">
    <parent link="base_link"/>
    <child link="right_leg"/>
    <origin  xyz="0 -0.22 0.25"/>
  </joint>

</robot>

We establish a relationship between the previous root base_link and the new added_base in the joint added_base_to_base_link that defines the rotation (rpy) and the positioning (xyz).

We can launch the model in rviz as:

ros2 launch urdf_tutorial display.launch.py model:=path/to/modified.urdf

and you'll get something similar to this, enabling the TFs:

rviz_fixed_base_link

The new link appear! But the point of view did not changed, as base_link is still the fixed frame for rviz. Change the fixed frame to the new added_base and you'll get something like this:

enter image description here

In your specific example, you'll have to add a base_link as the parent of base_link_chassis.

$\endgroup$
4
  • $\begingroup$ Thank you for your input, but I am a little confused how this would work. If I made a new 'base_link' as a parent to the 'base_link_chassis', wouldn't I need to generate an STL mesh which is basically a link of the entire robot model? And, would this cause any issues if the other links share the same physical mesh link as this new 'base_link' - or would it be unaffected? Also, how exactly would the hierarchy of adding this new 'base_link' work? Also where in the joint definition could this "correction rotation" be defined? Would all of this be done in the urdf file? Thanks for your help! $\endgroup$ Commented Jun 19 at 12:56
  • $\begingroup$ You don't necessarily need a mesh to create a link. Although a link usually represents physical objects, like legs or base of sensor, they are used to help locate in space specific points for different coordinate references. For example, again in 2D navigation, some models include another child link base_footprint that is the projection of base_link to the floor. It does not represent a physical object but it is convenient to use this other coordinate system as it does not include information about height. $\endgroup$
    – ivrolan
    Commented Jun 21 at 10:29
  • $\begingroup$ I'll edit my answer to clarify this and include an example $\endgroup$
    – ivrolan
    Commented Jun 21 at 10:30
  • $\begingroup$ Thanks so much for your help man - it worked great $\endgroup$ Commented Jul 15 at 11:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.