This is an interesting and highly relevant topic! Any time you have a robot that can interfere with itself, you need a solution to motion planning to ensure your robot doesn’t destroy itself.

For 1 DOF systems, this is pretty straightforward. You define the minimum and maximum position constraints for your arm/elevator/whatever and make sure you always stay between them. To do so, you might use some combination of (1) rejecting, coercing, or clamping setpoints so that they are always within the valid range, (2) soft limits on the speed controller, (3) limit switches, and/or (4) physical hard stops.

For multi-DOF systems, things get fun. In the 2 DOF case (maybe an arm with a shoulder and a wrist, or an elevator and an arm), each DOF has some limits that are independent of the rest of the system (ex. min and max angles), but there are also dependent constraints (ex. if the shoulder is angled down, the wrist must be angled up to avoid hitting the ground). Examples of such constraints could include:

- Min/max angles
- Maximum extension rules
- Robot chassis
- Angles that require an infeasible amount of holding torque

As @Tom_Line alluded, one intuitive way to think about how this all works is to envision a 2D space (for 2 DOFs; generically, it is N-D for N DOFs) where the x and y axes are the positions of the degrees of freedom. A given point (x,y) can either be a valid configuration, or an invalid configuration (meaning it violates one or more constraints). So, your 2D plot may have regions (be they half-planes, rectangles, etc.) representing constraints.

First example: 2 DOFs where all of the constraints are independent (min/max limits on each axis):

```
x's represent invalid states
elevator height
^ invalid: y axis over travel
|xxxxxxxxxxxxxxxxxxxxx
|x x
|x valid states x invalid: x axis over travel
|x x
|x x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
```

Motion planning/control in this example is pretty simple. Just make sure all setpoints are always inside the box of valid states.

Now let’s make it more interesting. Say that if the arm is near the middle of travel (pointing straight down), the elevator must be above a certain height, otherwise the arm intersects the robot chassis.

```
elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x x
|x x
|x xxxxx x
|x xxxxx x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
```

Let’s talk about motion planning using the second example. Say the current state of our system is `s`

and our goal state is `g`

:

```
elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x x
|x x
|x xxxxx x
|x s xxxxx g x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
```

Notice that the elevator height at the current state and the goal is identical. So we just have to move the arm, right…? But moving this way will sweep the system into the constraint we defined for preventing self-intersection with the chassis. Instead, we need to plan a sequence of setpoints to execute that only pass through valid states. Here’s one such example:

```
elevator height
^
|xxxxxxxxxxxxxxxxxxxxx
|x x
|x a______b x
|x / xxxxx \ x
|x s xxxxx g x
|xxxxxxxxxxxxxxxxxxxxx
o--------------------> arm angle
```

`a`

and `b`

are intermediate waypoints we add to ensure that the whole system moves in a safe way. If you think about how this works with our toy example, the elevator goes up, then the arm rotates underneath, then the elevator goes back down. There are many possible ways to generate this path (or other valid paths), including doing some sort of search (discretize the plot into a grid and find a valid path using breadth-first search or A*) or hard-coding rules to deal with different regions of the constraint space. Our approach to the latter in 2018 is here (though largely commented out due to mechanical re-designs eliminating interferences).

Once you have a sequence of waypoints to follow, you have a few options for how to do so:

- Generate trajectories for each DOF and time-synchronize them. This is how you can ensure your mechanism follows the “diagonal lines” in the path above, but currently there is no support for this AFAICT in any existing FRC software library. The upside of this approach is you can yield the fastest (time-optimal) coordinated movements possible. The downside is that the motion planning is “open loop” - if one of your DOFs lags behind its trajectory, it’s possible that the system might still end up in an invalid state. In practice, you always want some buffer around your setpoints to ensure that tracking error doesn’t lead to this!
- Do moves one at a time. Move from
`s`

to `a`

, and only start moving towards `b`

once the path from the current system state to `b`

is a straight line that is collision free. This gives you robustness to control error, but may result in unnecessary stops or jerky movement compared to the optimized, open-loop trajectory.

This whole approach generalizes to an arbitrary number of DOFs, and is basically how industrial 7-DOF arms work.