URDF Revolute Joints?
0 down vote favorite
I'm having a hard time defining simple revolute joints in an arm. The arm is modeled in a stowed configuration, and 1-6 0 down vote favorite
I'm having a hard time defining simple revolute joints in an arm. The arm is modeled in a stowed configuration, and 1-6 should rotate about their axes (along the center of the cylinder). When I try to control these joints in Rviz, the links do not rotate at all how I'm expecting/intending. This is my first time making a URDF file so your patience and help is appreciated. The links in the image are:
- origin_joint (center of this box is also the global origin)
- driven_1
- driving_2
- driven_2
- driving_3
- driven_3
<robot name="bca_arm">
<!-- * * * Link Definitions * * * -->
<link name="origin_joint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="3.5 3 4.5"/>
</geometry>
<material name="Cyan1">
<color rgba="0 0.9 0.9 1.0"/>
</material>
</visual>
</link>
<link name="motor_1">
<visual>
<origin xyz="0 0 4.819" rpy="0 0 0"/>
<geometry>
<cylinder radius="1.1" length="5.138"/>
</geometry>
<material name="Yellow2">
<color rgba="0.8 0.8 0 1.0"/>
</material>
</visual>
</link>
<link name="origin_link">
<visual>
<origin xyz="0 0 -2.727" rpy="0 0 0"/>
<geometry>
<cylinder radius=".75" length=".954"/>
</geometry>
<material name="Yellow2">
<color rgba="0.8 0.8 0 1.0"/>
</material>
</visual>
</link>
<link name="driving_1">
<visual>
<origin xyz="0 0 -5.354" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="2.15" length="3"/>
</geometry>
<material name="Cyan1">
<color rgba="0 0.9 0.9 1.0"/>
</material>
</visual>
</link>
<link name="motor_2">
<visual>
<origin xyz="2.9 0 -5.354" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="1.1" length="2.814"/>
</geometry>
<material name="Yellow2">
<color rgba="0.8 0.8 0 1.0"/>
</material>
</visual>
</link>
<link name="driven_1">
<visual>
<origin xyz="-2.836 0 -5.354" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="2.106" length="2.672"/>
</geometry>
<material name="Blue2">
<color rgba="0 0 0.7 1.0"/>
</material>
</visual>
</link>
<link name="rod_1">
<visual>
<origin xyz="-2.836 0 1.9" rpy="0 0 0"/>
<geometry>
<cylinder radius=".75" length="10.3"/>
</geometry>
<material name="Yellow2">
<color rgba="0.8 0.8 0 1.0"/>
</material>
</visual>
</link>
<link name="driving_2">
<visual>
<origin xyz="-2.836 0 9.054" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="2.15" length="3"/>
</geometry>
<material name="Cyan1">
<color rgba="0 0.9 0.9 1.0"/>
</material>
</visual>
</link>
<link name="motor_3">
<visual>
<origin xyz="0 0 9.054" rpy="0 1.57 0"/>
<geometry>
<cylinder radius="1.1" length="2.814"/>
</geometry>
<material name="Yellow2">
<color rgba="0.8 0.8 0 1.0"/>
</material>
</visual>
</link>
<link name="driven_2">
<visual>
<origin xyz="-5.7 0 9.054" rpy="0 0 0"/>
<geometry>
<cylinder radius="1.37" length="4.5"/>
</geometry>
<material name="Blue2">
<color ...
If you say this, it would be good to tell us how you think it should rotate.
I'm willing to look at this, but non of the
revolute
joints specify anylimit
s. That is not allowed, so the urdf you included will not load.After adding some arbitrary limits:
1-6 should rotate about their axes (along the center of the cylinder) and all portions of arm should remain connected
I have a suspicion you write "and all portions of the arm should remain connected" because they aren't right now. I think that is because you are specifying
geometry
withorigin
s different than yourjoint
s. That can work, but you'll need to givejoint
s the correspondingorigin
s as well.I would recommend making all
geometry
origin
s link-local, as the frames for yourjoint
s. That will greatly simplify things.In essence: make the
joint
s of alllink
s theorigin
s of thelink
s.Suggestion: check wiki/urdf/xml for the required elements and attributes.
Is the URDF you include in your question text the one you tested? Because I cannot get it to load at all without fixing it up. How did you determine that things don't work as you expect?
Sorry all I was using this to work on and visualize what I was doing: https://mymodelrobot.appspot.com and realized Rviz wouldn't load it at all. I will try to redefine using joints/origins.
Please also keep in mind that we use metres for everything in ROS, not millimetres. Right now your arm is several metres long and wide ..