0
$\begingroup$

Rosanswers logo

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:

  1. origin_joint (center of this box is also the global origin)
  2. driven_1
  3. driving_2
  4. driven_2
  5. driving_3
  6. driven_3

image description

<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 rgba="0 0 0.7 1.0"/>
        </material>
    </visual>   
</link>

<link name="motor_4">
    <visual>
        <origin xyz="-5.7 0 13" rpy="0 0 0"/>
        <geometry>
            <cylinder radius=".825" length="3.5"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<link name="rod_2">
    <visual>
        <origin xyz="-5.7 0 3.15" rpy="0 0 0"/>
        <geometry>
            <cylinder radius=".75" length="7.285"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<link name="driving_3">
    <visual>
        <origin xyz="-5.7 0 -2.3" rpy="1.57 0 0"/>
        <geometry>
            <cylinder radius="2" length="3"/>
        </geometry>
        <material name="Cyan1">
            <color rgba="0 0.9 0.9 1.0"/>
        </material>
    </visual>   
</link>

<link name="motor_5">
    <visual>
        <origin xyz="-5.7 2 -2.3" rpy="1.57 0 0"/>
        <geometry>
            <cylinder radius=".825" length="1.161"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<link name="driven_3">
    <visual>
        <origin xyz="-5.7 -2.8 -2.3" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="1.37" length="4.5"/>
        </geometry>
        <material name="Blue2">
            <color rgba="0 0 0.7 1.0"/>
        </material>
    </visual>   
</link>

<link name="motor_6">
    <visual>
        <origin xyz="-5.7 -2.8 1.6" rpy="0 0 0"/>
        <geometry>
            <cylinder radius=".825" length="3.481"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<!-- * * * Joint Definitions * * * -->

    <joint name="joint_1" type="fixed">
    <parent link="motor_1"/>
    <child link="origin_joint"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_2" type="revolute">
    <parent link="origin_joint"/>
    <child link="origin_link"/>
    <origin xyz="0 0 -2.25" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
</joint>

<joint name="joint_3" type="fixed">
    <parent link="origin_link"/>
    <child link="driving_1"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_4" type="fixed">
    <parent link="motor_2"/>
    <child link="driving_1"/>
    <origin xyz="0 0 0"/>
</joint>
    
<joint name="joint_5" type="revolute">
    <parent link="driving_1"/>
    <child link="driven_1"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
</joint>

<joint name="joint_6" type="fixed">
    <parent link="driven_1"/>
    <child link="rod_1"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_7" type="fixed">
    <parent link="rod_1"/>
    <child link="driving_2"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_8" type="fixed">
    <parent link="motor_3"/>
    <child link="driving_2"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_9" type="revolute">
    <parent link="driving_2"/>
    <child link="driven_2"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
</joint>

<joint name="joint_10" type="fixed">
    <parent link="motor_4"/>
    <child link="driven_2"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_11" type="revolute">
    <parent link="driven_2"/>
    <child link="rod_2"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
</joint>

<joint name="joint_12" type="fixed">
    <parent link="rod_2"/>
    <child link="driving_3"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_13" type="fixed">
    <parent link="motor_5"/>
    <child link="driving_3"/>
    <origin xyz="0 0 0"/>
</joint>

<joint name="joint_14" type="revolute">
    <parent link="driving_3"/>
    <child link="driven_3"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
</joint>

<joint name="joint_15" type="fixed">
    <parent link="driven_3"/>
    <child link="motor_6"/>
    <origin xyz="0 0 0"/>
</joint>

</robot>

Originally posted by fusionice on ROS Answers with karma: 16 on 2017-10-18

Post score: 0


Original comments

Comment by gvdhoorn on 2017-10-18:\

the links do not rotate at all how I'm expecting/intending.

If you say this, it would be good to tell us how you think it should rotate.

Comment by gvdhoorn on 2017-10-18:
I'm willing to look at this, but non of the revolute joints specify any limits. That is not allowed, so the urdf you included will not load.


After adding some arbitrary limits:

Failed to find root link: Two root links found: [motor_1] and [motor_2]

Comment by fusionice on 2017-10-18:
1-6 should rotate about their axes (along the center of the cylinder) and all portions of arm should remain connected

Comment by gvdhoorn on 2017-10-18:
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 with origins different than your joints. That can work, but you'll need to give joints the corresponding origins as well.

Comment by gvdhoorn on 2017-10-18:
I would recommend making all geometry origins link-local, as the frames for your joints. That will greatly simplify things.

In essence: make the joints of all links the origins of the links.

Comment by gvdhoorn on 2017-10-18:
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?

Comment by fusionice on 2017-10-18:
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.

Comment by gvdhoorn on 2017-10-18:
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 ..

Comment by fusionice on 2017-10-18:
Thanks for the comments, had to make adjustments and it is now working properly!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

<link name="base_link">
    <visual>
        <geometry>
            <box size="0.0889 0.0762 0.1143"/>
        </geometry>
    </visual>   
</link>


<link name="motor_1">
    <visual>
        <geometry>
            <cylinder radius="0.02794" length="0.1305"/>
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </visual>   
</link>

<joint name="joint_1" type="fixed">
    <parent link="base_link"/>
    <child link="motor_1"/>
    <origin xyz="0 0 0.1224"/>
</joint>

<link name="origin_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.01905" length="0.0242"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_2" type="revolute">
    <parent link="base_link"/>
    <child link="origin_link"/>
    <origin xyz="0 0 -0.0692" rpy="0 0 0"/>
    <limit lower="-3.1416" upper="3.1416" effort="10" velocity="3"/>
    <axis xyz="0 0 1"/>
</joint>

<link name="driving_1">
    <visual>
        <origin xyz="0 0 0" rpy="0 1.57 0"/>
        <geometry>
            <cylinder radius="0.05461" length="0.0762"/>
        </geometry>
        <material name="Cyan1">
            <color rgba="0 0.9 0.9 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_3" type="fixed">
    <parent link="origin_link"/>
    <child link="driving_1"/>
    <origin xyz="0 0 -0.067"/>
</joint>

<link name="motor_2">
    <visual>
        <origin xyz="0 0 0" rpy="0 1.57 0"/>
        <geometry>
            <cylinder radius="0.02794" length="0.0715"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_4" type="fixed">
    <parent link="driving_1"/>
    <child link="motor_2"/>
    <origin xyz="0.074 0 0"/>
</joint>

<link name="driven_1">
    <visual>
        <origin xyz="0 0 0" rpy="0 1.57 0"/>
        <geometry>
            <cylinder radius="0.0535" length="0.068"/>
        </geometry>
        <material name="Blue2">
            <color rgba="0 0 0.7 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_5" type="revolute">
    <parent link="driving_1"/>
    <child link="driven_1"/>
    <origin xyz="-0.0720 0 0" rpy="0 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="3"/>
    <axis xyz="1 0 0"/>
</joint>

<link name="rod_1">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.019" length="0.262"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_6" type="fixed">
    <parent link="driven_1"/>
    <child link="rod_1"/>
    <origin xyz="0 0 0.18"/>
</joint>

<link name="driving_2">
    <visual>
        <origin xyz="0 0 0" rpy="0 1.57 0"/>
        <geometry>
            <cylinder radius="0.0546" length="0.0762"/>
        </geometry>
        <material name="Cyan1">
            <color rgba="0 0.9 0.9 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_7" type="fixed">
    <parent link="rod_1"/>
    <child link="driving_2"/>
    <origin xyz="0 0 0.185"/>
</joint>

<link name="motor_3">
    <visual>
        <origin xyz="0 0 0" rpy="0 1.57 0"/>
        <geometry>
            <cylinder radius="0.02794" length="0.0715"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_8" type="fixed">
    <parent link="driving_2"/>
    <child link="motor_3"/>
    <origin xyz="0.074 0 0"/>
</joint>

<link name="driven_2">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.035" length="0.1143"/>
        </geometry>
        <material name="Blue2">
            <color rgba="0 0 0.7 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_9" type="revolute">
    <parent link="driving_2"/>
    <child link="driven_2"/>
    <origin xyz="-0.073 0 0" rpy="0 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="3"/>
    <axis xyz="1 0 0"/>
</joint>

<link name="motor_4">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.021" length="0.0889"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_10" type="fixed">
    <parent link="driven_2"/>
    <child link="motor_4"/>
    <origin xyz="0 0 0.1016"/>
</joint>

<link name="rod_2">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.019" length="0.185"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_11" type="revolute">
    <parent link="driven_2"/>
    <child link="rod_2"/>
    <origin xyz="0 0 -0.1496" rpy="0 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="3"/>
    <axis xyz="0 0 1"/>
</joint>

<link name="driving_3">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0"/>
        <geometry>
            <cylinder radius="0.0508" length="0.0762"/>
        </geometry>
        <material name="Cyan1">
            <color rgba="0 0.9 0.9 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_12" type="fixed">
    <parent link="rod_2"/>
    <child link="driving_3"/>
    <origin xyz="0 0 -0.1396"/>
</joint>

<link name="motor_5">
    <visual>
        <origin xyz="0 0 0" rpy="1.57 0 0"/>
        <geometry>
            <cylinder radius="0.0209" length="0.0294"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_13" type="fixed">
    <parent link="driving_3"/>
    <child link="motor_5"/>
    <origin xyz="0 0.0528 0"/>
</joint>

<link name="driven_3">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.0347" length="0.1143"/>
        </geometry>
        <material name="Blue2">
            <color rgba="0 0 0.7 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_14" type="revolute">
    <parent link="driving_3"/>
    <child link="driven_3"/>
    <origin xyz="0 -0.0729 0" rpy="0 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="3"/>
    <axis xyz="0 1 0"/>
</joint>

<link name="motor_6">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <cylinder radius="0.0209" length="0.0884"/>
        </geometry>
        <material name="Yellow2">
            <color rgba="0.8 0.8 0 1.0"/>
        </material>
    </visual>   
</link>

<joint name="joint_15" type="fixed">
    <parent link="driven_3"/>
    <child link="motor_6"/>
    <origin xyz="0 0 0.1016"/>
</joint>

Originally posted by fusionice with karma: 16 on 2017-10-18

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by atom on 2022-10-13:
Can you please tell me, what changes you made for this i have a similar problem, my robot's revolute joint isn't turning about its axis my question is here https://answers.ros.org/question/407840/help-on-revolute-joint-placement-in-gazebo/

Comment by fusionice on 2022-10-14:
Hi @atom. I haven't worked on this in quite some time but remember this online tool being very helpful in properly creating my URDF model. http://www.mymodelrobot.appspot.com/5629499534213120

Comment by atom on 2022-10-14:
I've been using this website for designing the robot, but I think I'm not having a conceptual understanding of how to make a robot's revolute joining move around itself. Thank you for taking time to respond though, very much appreciated.. I've been stuck with this problem for more than 4 days now haha..

$\endgroup$

Your Answer

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