SDF Import: handling joint pose?

[Gregwar]

Looks like the joint pose is not read when importing SDF files (just like the origin is read during URDF), in this example:

<sdf version="1.6">
    <model name="onshape">

        <link name="a">
            <collision>
                <geometry><box><size>0.5 0.5 0.3</size></box></geometry>
            </collision>
        </link>

        <link name="b">
            <pose>1.5 0 0 0 0 0</pose>
            <collision>
                <geometry><box><size>0.5 0.5 0.3</size></box></geometry>
            </collision>
        </link>

        <joint name="revolute_joint" type="revolute">
            <pose>2 0 0 0 0 0</pose>
            <axis>
                <xyz>0 0 1</xyz>
                <limit>
                    <lower>-2</lower>
                    <upper>2</upper>
                    <effort>1000</effort>
                    <velocity>12</velocity>
                </limit>
            </axis>
            <parent>a</parent>
            <child>b</child>
        </joint>
    </model>
</sdf>

The child object (b) revolutes around its own frame, and the pose in the joint has not effect

See: http://sdformat.org/spec?ver=1.6&elem=joint#joint_pose

[erwincoumans]

We only support a small subset of SDF to load the kinematics and dynamics parameters. There are APIs to set the joint poses. If someone wants to extend the SDF parser etc, please contribute and re-open.

[Gregwar]

Ok, I think that could be an interresting patch to do indeed, since this is handled for URDFs files (with the origin) and not for SDFs