Reactive DMP implementation for obstacle avoidance

[af-a]

Problem description

The current DMP-based implementation of the move arm action uses open-loop, naive trajectory generation, which does not take obstacles into account. For example, if the robot attempts to pick an object off of a table, it will simply reproduce the given trajectory, regardless of whether that would cause the end-effector to collide with other obstacles on that table. This is not ideal in such cases and may lead to task failures.

Desired solution

The DMP formulation enables an effective obstacle avoidance strategy, discussed in detail in this publication, by adding an appropriate term in the differential equation governing the evolution of the variable (end-effector position). Implementing this could be enhance the move arm action with real-time obstacle avoidance, assuming that the robot can perceive the positions of obstacles with reasonable accuracy.

Suggested courses of action:

• [x] Changing the way in which the trajectory is stepped though in the DMPExecutor. Instead of rolling out the whole trajectory, storing the end-effector coordinates, and then sending velocity commands in the control loop, we could generate individual steps of the trajectory within the control loop. We can call this online trajectory generation.
• [ ] Adding 3D obstacle avoidance to the trajectory generation logic (following the referenced paper).
  • [ ] Implementing single object avoidance, by deriving a force profile from the estimated pose of the perceived object
  • [ ] Integrating multiple perceived objects in the avoidance strategy

Future

• Implement fully reactive obstacle avoidance through continuous perception and motion adaptation, during trajectory generation. This will also enable avoiding dynamic obstacles.

[njanirudh]

How do we expect the robot to find the obstacle? Is it manually set?

[alex-mitrevski]

For now, we assume that the scene has been perceived, so the positions and bounding boxes of obstacles are known. In the long run, we would like to react to dynamic obstacles as well.

[af-a]

The first point was addressed in #231.