SRDF conventions and parsing

[cmastalli]

Hi guys,

I want to raise a discussion and potential actions to be tackled regarding SRDF convention. My motivation is to have relevant semantic information that is used for motion planners, controllers, etc. Specifically I have these two points in mind:

1. Defining set of foot and hand end-effectors
2. Describing unimportant joints (i.e. joints that are not needed to be considered for planning, control, etc; e.g. neck joints).

I would say that everyone agree that this information should be described in the SRDF. However it is not very clear for me how to properly do it. In any case I believe we can attach to this standard, otherwise we should discuss how to modify the SRDF standard.

---

Point 1: set of foot and hand end-effector
I think the best way to do this is through the end_effector tag. With this tag we should also define the groups in order to semantically describe a leg or arm. Let me write an example for the HyQ robot:

    <!-- left front leg -->
    <group name="lf_leg">
        <joint name="lf_haa_joint" />
        <joint name="lf_hfe_joint" />
        <joint name="lf_kfe_joint" />
        <chain base_link="trunk" tip_link="lf_foot" />
    </group>
    <!-- right front leg -->
    <group name="rf_leg">
        <joint name="rf_haa_joint" />
        <joint name="rf_hfe_joint" />
        <joint name="rf_kfe_joint" />
        <chain base_link="trunk" tip_link="rf_foot" />
    </group>
    <!-- left hind leg -->
    <group name="lh_leg">
        <joint name="lh_haa_joint" />
        <joint name="lh_hfe_joint" />
        <joint name="lh_kfe_joint" />
        <chain base_link="trunk" tip_link="lh_foot" />
    </group>
    <!-- right hind leg -->
    <group name="rh_leg">
        <joint name="rh_haa_joint" />
        <joint name="rh_hfe_joint" />
        <joint name="rh_kfe_joint" />
        <chain base_link="trunk" tip_link="lh_foot" />
    </group>

    <end_effector name="lf_foot" parent_link="lf_foot" group="lf_leg" />
    <end_effector name="rf_foot" parent_link="rf_foot" group="rf_leg" />
    <end_effector name="lh_foot" parent_link="lh_foot" group="lh_leg" />
    <end_effector name="rh_foot" parent_link="rh_foot" group="rh_leg" />

With this example, you can parser:

1. The frame of the end-effector.
2. The number of joints of that chain, then, for instance, you can build an appropriate Jacobian.

Please note that it's missing the foot dimension. For that we will have to improve the SRDF. This is an example of how it could look:

    <foot name="lf_foot" type="point" /?>
    <!-- for a humanoid case -->
    <foot name="l_foot" type="planar" dim_x="0.1" dim_y="0.1" />

---

Point 2: Unrelevant joints*
There are sometime joints that you don't want to consider, these are examples:

1. Neck joints are not needed for walking planning.
2. Gripper joints are not controlled in torque (e.g. TSID)

For doing so, I suggest to extract this information from the groups tag, and the developer decides which group to use. In this case, it's important to define a naming convention for groups.

---

Final note
This might be long way discussion, and it's possible that we cannot make a decision any-time soon. I would invite everyone to this discussion, especially I would appreciate the point of view of main developers (@jcarpent @jmirabel @gabrielebndn @nim65s @olivier-stasse @nmansard).

[wxmerkt]

However it missing the foot dimension. For that we will have to improve the SRDF.

We do this by defining the corners/contact points as relative offset frames of the foot frame. This allows us to handle the points for contact wrenches explicitly (and also add intermediate points instead of just the four corners, if desired). Perhaps something like this would be better than specifying the dimension and assuming implicitly that the foot is a rectangle.

[jmirabel]

For contacts, I implemented the following convention (https://gepettoweb.laas.fr/hpp/hpp-manipulation-urdf/doxygen-html/index.html):

<contact name="name">
  <!-- Exactly 1 of -->
  <!-- Set the link used to build the reference frame
       In the first case, the position of the points below are expressed in the
       link frame.

       In the second case, the position of the points below are expressed in the
       frame of the "index_collision_object"th collision body in the link. If
       you have only one body, then set it to zero. This is a convenient way of
       of defining surfaces on a particular body.
       -->
    <link name="link_name" />
    <link name="link_name" index="index_collision_object" />

  <!-- Exactly 1 -->
    <!-- Sequence of 3D points expressed in the frame defined above -->
    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025 </point>

    <!-- Sequence of planar shape.
         Shapes are defined by a sequence of points to be connected (consecutively).
         For a shape defined by N points, N+1 values must be provided in this tag:
         N iPoint_1 ... iPoint_N

         WARNING: Shapes 3 0 1 2 and 3 0 2 1 are different because their respective orientations are opposites.
         firstSegment ^ secondSegment must points outside the object -->
    <shape> 3 0 2 1 3 2 1 3</shape>
</contact>