Introduce Planner Coordinate Frame

[yck011522]

Feature Request

To introduce a Planner Coordinate Frame. This reduced ambiguity when defining planning targets, specifying planning group, and how tools are attached to robots.

Details

After multiple sessions of brainstorm with @gonzalocasas, we realized there is an ambiguity in compas_fab regarding the behavior of how planning groups are handled. Planning groups, as an essential concept for robot motion planning, defines which joints can be moved by emotion planner.

Typically the planning groups are defined and extracted as a read-only attribute from an SRDF file. At the moment, compas_fab do not provide support for users to edit SRDF or the definition of planning groups. The rationale of this is because in the current workflow of using the ROS Planning backend, the SRDF is already embedded in the Docker image file. Many of the embedded SRDF are provided by open repos (such as by ROS-Industrial) are not maintained by us.

more coming...

[yck011522]

However, in many existing SRDFs we packaged in the ROS Docker file (which is managed here: https://github.com/gramaziokohler/ros_docker) the planning group have variations that either plan towards the flange or the tool0 link. According to (REP 199 Coordinate Frames for Serial Industrial Manipulators) the two frames have different meaning and in some cases are not identical (e.g. the case of UR robots). This variation causes an rather confusing behavior when trying to understand how tools are oriented when attached to the robot and how targets are defined.

Famously this causes the problem where users experience tools that are flipped when attached or planning to targets that are upside down.

The source of this confusion lies in the fact that different robot manufacturer have different convention when defining their base frames and flange frames. For example, in general:

• ABB robot base frame X forward, Z up.
• UR5 robot base frame Y into robot cable, (X backward), Z up. (https://forum.universal-robots.com/t/base-coordinate-system-documentation/140/2)
• UR5 flange frame Y is away from the Tool connector, and Z is normal to the tool flange.
• ABB flange frame robot tool0 : Z away from the tool connector.

REP199 proposed a robot link definition scheme to improve the consistency from a robot programmer's perspective. It uses two virtual frames (base_link and flange) to maintain consistent behavior while two physical frames (base and tool0) are matched with the brand-dependent robot controller. A typical robot link relationship under such scheme appear as follows:

world 
├ (Cartesian planning targets are defined relative to world)
└ base_link (comply to REP103 and REP199, Positive X forward, Positive Y left, Positive Z up)
 ├ base (coincident with robot origin on brand-dependent controller)
 └ link_1
  └ ..
   └ link_N
   └ flange (comply to REP103 and REP199, positive X must always point away from the last link)
    └ tool0 (coincident with default TCP on brand-dependent controller)
    └ gripper (user defined, application specific tool frames) 

[yck011522]

At the moment, custom user specified tools are managed by compas_fab. The rationale is that users can easily define and change tools without editing the URDF / SRDF and the docker files. The current behavior is that compas_fab will attach the tool to the last link of the planning group.

In many of the SRDF we have seen, the main planning group is defined to from base_link to tool_0 (the brand-dependent definition of the end-effector frame).

    <group name="manipulator">
        <chain base_link="base_link" tip_link="tool0"/>
    </group>

This explains why a tool that is modeled for one specific robot maybe flipped when attaching to another robot vendor. Which also makes the preparation of generic toolmodels for demonstration purpose impossible.

In an ideal case, another planning group with flange as the tip_link should also exist, this would allow compas_fab to attach tools to the more stable flange link to maximize interoperability. Unfortunately the SRDFs we packaged are sourced from external repos and we intend to keep it this way. And none of the SRDFs have flange as tip_link.

Solutions we have considered to address this problem include:

• Modify the SRDFs after sourcing and before packaging the ROS Docker image. This is likely achieved by an GitHub Build action.
• Change the SRDFs within docker before ROS is launched. This is likely a script executed within Docker during compose up and before ROS is launched.
• Keep track of the transformation between flange and tool0, have compas_fab transform tools and targets from flange to which ever tip_link is defined.

---

Example SRDFs:
https://github.com/gramaziokohler/abb_crb15000_5_95_moveit_config/blob/8f9810e37eed8e649bc59fb64dd3d5d2bbd7418a/config/abb_crb15000_5_95.srdf#L12-L14

https://github.com/gramaziokohler/abb_irb1600_8_145_moveit_config/blob/25a591f4f0134e11b4e197cd3a6063d76c401d24/config/abb_irb1600_8_145.srdf#L12-L14

[yck011522]

While I personally prefer the last solution, @gonzalocasas prefers the second option. Anyhow, all of these require substantial development effort that we cannot afford at the moment.

Therefore, we decided not to address this problem now and opt for defining a Planner Coordinate Frame (PCF). The definition of this frame is determined by the SRDF, hence determined by which robot the user loads and which planning group the user select to use. The PCF is formally defined to be identical to the frame of the tip_link in the selected planning group of the selected robot.

This frame will be used as the base frame for which tools are attached and planning targets are defined. This is also the frame that forward and inverse kinematics use.

[yck011522]

Now the associated PR is mostly a change of terminology in all the relevant places as there are no code change. A new section will be added to docs dedicated to explaining this complication.

For example FrameTarget will explicitly mention:

    Given a FrameTarget, the planner aims to find a path moving
    the robot's Planner Coordinate Frame (PCF) to the specified `FrameTarget.target_frame`.
    In many case, the PCF corresponds to the tool0 coordinate frame (T0CF) that matches with the robot controller.
    See :ref:`coordinate_frames` for more details about the PCF,
    or if the planning result does not match the pose displayed on the robot controller.

    If the user wants to plan with a tool, such that the user-defined tool frames / tool tip point (TCP) / Tool Coordinate Frame (TCF) is matched with the target),
    the `tool_coordinate_frame` parameter should be provided to define the TCF relative to the PCF of the robot. 
    Note that the tool model is always attached to the PCF.

[yck011522]

While explaining the new coordinate frame in the documentation, it will be good to explain also the difference between base and base link.
This is in response to the common confusion where robot base frame is different from that on the robot pendant (
ros-industrial/universal_robot#676) .