Euler Sequence Confusion Between URDF and MJCF

Discussion in 'Priority support' started by Grant Gibson, Oct 18, 2020.

  1. I am comparing the joint frame origin of a URDF file to a body origin in an MJCF file and I am confused with how the euler angles are interpreted between the two files. I have reduced files to illustrate the problem that I am having. Both files should reproduce the same model.

    Angle_Check.xml
    Code:
    <mujoco model='angle-check'>
        <compiler angle='radian' eulerseq='xyz'/>
        <option gravity='0 0 0'/>
    <worldbody>
        <body name='base' pos='0 0 0' euler='0 0 0'>
            <inertial pos='0 0 0' mass='1' fullinertia='1 1 1 0 0 0'/>
            <joint type='free' limited='false'/>
            <geom type='box' size='0.5 0.05 0.05' rgba="0.8 0.8 0.8 1"/>
            <body name='link1' pos='0 1 0' euler='0.5 -1.5 -0.25'>
                <inertial pos='0 0 0' mass='1' fullinertia='1 1 1 0 0 0'/>
                <joint name='link1' type='hinge' axis='0 0 1'/>
                <geom type='box' size='0.5 0.05 0.05' rgba="0 0.8 0.8 1"/>
            </body>
        </body>
    </worldbody>
    </mujoco>
    upload_2020-10-18_23-3-18.png

    Angle_Check.urdf
    Code:
    <robot name="angle-check">
    
        <!-- Mujoco Options -->
        <mujoco>
            <compiler discardvisual="false" balancedinertia="true"/>
            <option gravity='0 0 0'/>
        </mujoco>
    
        <link name="base">
            <inertial>
                <origin xyz="0 0 0"/>
                <mass value="1"/>
                <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
            </inertial>
            <visual>
                <material>
                    <color rgba="0.8 0.8 0.8 1"/>
                </material>
                <geometry>
                    <box size='0.5 0.05 0.05'/>
                </geometry>
            </visual>
        </link>
    
        <link name="link1">
            <inertial>
                <origin xyz="0 0 0"/>
                <mass value="1"/>
                <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
            </inertial>
            <visual>
                <material>
                    <color rgba="0 0.8 0.8 1"/>
                </material>
                <geometry>
                    <box size="0.5 0.05 0.05"/>
                </geometry>
            </visual>
        </link>
    
        <joint name="link1" type="revolute">
            <origin xyz="0 1 0" rpy="0.5 -1.5 -0.25"/>
            <axis   xyz="0 0 1"/>
            <parent link="base"/>
            <child  link="link1"/>
        </joint>
    
    </robot>
    upload_2020-10-18_23-4-2.png

    To my understanding, both of these code segments should produce the same image (base link shown in gray). These models contain two links connected by a single joint. As noted in the Mujoco documentation, an eulerseq of 'xyz' should correspond to the 'rpy' notation used in URDF's. Therefore i set the 'pos' and 'euler' of the child body in the xml file to be the same as the joint origin in the URDF file.
    Code:
    XML
    <body name='link1' pos='0 1 0' euler='0.5 -1.5 -0.25'>
    
    equivalent to
    
    URDF
    <origin xyz="0 1 0" rpy="0.5 -1.5 -0.25"/>
    I'm guessing that my error is originating from my understanding of the relationship between the joint frames of URDF and body frames of MJCF but I can not seem to figure it out.

    Any help would be greatly appreciated!
     

    Attached Files: