Question about setting the safety margin with Python

[DrawZeroPoint]

Dear all,

I wonder what could be the canonical way to set safety margins in HPP with the Python binding. My use case involves planning paths for the manipulator to pick and place an object on top of the table. Right now the implemented framework using HPP as the core works fluently, yet sometimes the trajectories could get the gripper/arm pretty close to surroundings (e.g., the table), which could be risky if that were to be transferred to real-world usage.

AFAIK, hpp-fcl enables setting a safety margin between objects, and particularly, I suppose setSecurityMarginForEdge from hpp.corbaserver.manipulation may be relevant to the function, so I have implemented it like this (a simplified version according to the actual working code, where some of the variables are replaced with CAPITAL VARS for succinctness. Please kindly refer to the original code if the context is not clear):

margined_edge = EDGE
for edge in graph.edges.keys():
    if edge == margined_edge:
        for joint in JOINTS:
            graph.setSecurityMarginForEdge(edge, JOINTS, 'universe', MARGIN_VALUE)

This segment of code somehow pushed the gripper away from the table in experiments, but I am not 100% sure if the code is taking effect or if I just observed several successful plannings produced by the random sampler (M-RRT).

So my questions are as follows:

1. Is the aforementioned code on the right track (or do we need to manage the margin differently)?
2. If so, how to choose which EDGE(s) to manipulate if we want the margin to be kept for the whole pick-and-place process or at least in a best-effort way?

FYI, my constraint graph is like this:

constraintgraph.pdf

where curi is the robot name, cube_30 is a 30mm cube to grasp.

Thank you for any suggestions in advance.

[florent-lamiraux]

You can use the following helper python class:

from hpp.corbaserver.manipulation import SecurityMargins

sm = SecurityMargins(ps, factory, [robotName, objectName])
sm.setSecurityMarginBetween (robotName, objectName, 0.02)
sm.apply()

where robotName is the name of the robot, objectName is the name of the object. Note the robotName is the prefix of the robot joint names, while objectName is the prefix of the object freeflying joint: objectName + '/root_joint'.

The 3 last lines should be inserted between

factory.generate()

and

graph.initialize()

These instructions set the security margin to 0.02 between the robot links and the object on all edges except between the gripper and object on edges where the object is about to be grasped. Up to now, security margins between robots / objects and static obstacles is not taken into account, but it should not be difficult to modify class SecurityMargin in order to fix that. Please feel free to make a pull request.

You can find an example here: https://github.com/agimus/agimus-demos/blob/master/talos/manipulate_boxes/common_hpp.py line 203.

[florent-lamiraux]

Do you still have questions about this issue or can I close it?

[DrawZeroPoint]

Dear @florent-lamiraux, I have implemented the code according to the instructions. However, as you said, the code could only prevent the collision between robot parts and the object (in my current case, the object is just a 30mm x 30mm cube, hence this feature produces no significant difference), whereas the robot can still collide with the surrounding environment (e.g., the table holding the cube) with these additions.

After going through the SecurityMargins code, I tried explicitly adding universe to the robotsAndObjects list to enable setting the margin between universe and the robot. I did so because the document indicates that such an operation is permissible. However, this does not lead to the expected margin behavior.

I wonder whether the margin is properly set and functional when we let joint1 be one of the robot joints, and joint2 be the 'universe' when using the graph.setSecurityMarginForEdge function, which seems to play a key role in the SecurityMargins class.

[florent-lamiraux]

I will investigate later in the week.

[florent-lamiraux]

@DrawZeroPoint , I solved the problem and made a pull request: https://github.com/humanoid-path-planner/hpp-manipulation-corba/pull/126. If you compile from source, you can take the devel branch of my repository until the pull request is merged.

[DrawZeroPoint]

That is great! Thank you and looking forward to the official release.