Reactive Trajectory Adjustment for Motion Execution Using Chekhov

This project was completed to satisfy MIT's Thesis requirement for a Masters of Engineering degree. The goal of my thesis was to develop an algorithm that could make small adjustments to a robot's planned trajectory in real time when dealing with unstructured dynamic environments. When in a dynamic environment, such as working with humans, a robot's planned path may become invalid due to a collision. A robot may need to generate several new paths to complete one task which may be infeasible due to time constraints. The reactive trajectory adjustment algorithm solves this problem by making small adjustments to the original path (if possible), which will be much faster than always generating new paths.

Methods and Results

The reactive trajectory algorithm uses an incremental adjustment approach that can make minor adjustments in response to collisions or goal changes. The two biggest contributions from this algorithm are the reaction vector computation, and the pose adjustment algorithm.
The general algorithm is as follows:

  1. Start with a planned trajectory, which is essentially a sequence of poses
    • A pose is simply a set of joint angles for the robot
  2. Then loop through all poses and check for a collision or if the goal region has changed
  3. If there is no collision or goal region change, execute the next pose, and repeat step 2
  4. If there is a collision or goal change make an adjustment to try and resolve the collision
    • Either the robot can resolve the collision and you’ll continue from step 2. Or the robot can’t resolve the collision, and you will abort back to the planning in step 1

To make a collision adjustment, the following algorithm is used:

  1. Find all poses in collision
  2. Compute a reaction vector for the first, middle and end pose of the collision
    • A reaction vector is a vector of the direction the robot should move in to avoid the collision
  3. Interpolate reaction vectors for the rest of the poses in collision
  4. For each collision pose, adjust based on the reaction vector
    • To do this, a quadratic program is used that will move the robot in the direction of the reaction vector based on joint angles while minimizing the distance the end-effector moves from it’s original position
    • This will make several very minor adjustments until the pose is no longer in collision
  5. Continue execution with the new poses that are no longer in collision

The goal adjustment is very similar to the collision adjustment, but rather than adjusting multiple pose, only the goal pose is adjusted and the rest are interpolated from the new pose and previous poses. This gives a general idea of how the robot will execute a plan and make adjustments.

The reactive trajectory algorithm was developed and tested in C++ with the OpenRAVE robotic simulation environment. The videos to the right were recorded from OpenRAVE and show two of the sample scenarios. The goal of the algorithm was to not only make correct adjustments, but also be significantly faster than traditional planning. The speed of the algorithm was compared with Bidirectional RRT (BiRRT), a commonly used planning algorithm. While BiRRT usually took 1.5-3 seconds, the reactive trajectory adjustment algorithm always took about a quarter of a second. This is significant not only because it is much faster, but is also far more consistent. Most of the scenarios tested were rather simple, whereas a complex scenario may take minutes to create a plan with BiRRT, and each re-plan will take the same. Whereas with the reactive trajectory adjustment algorithm, each re-plan should still be on the order of a quarter of a second assuming the adjustments needed are relatively small.

For a more detailed explanation of the algorithm, and results, please refer to my thesis linked above through MIT.

Pole Collision Adjustment: This video shows the WAM robot simulated in OpenRAVE. The original trajectory shows the robot colliding with a pole-like object. The robot uses the reactive trajectory adjustment algorithm to avoid the collision with the new trajectory.

Goal Adjustment: This video shows the WAM robot simulated in OpenRAVE. In this scenario the robot is tracking an individual cup until it goes out of range, and then selects a new cup to track. This shows another use of the reactive trajectory adjustment algorithm rather than just collision adjustment.