FPD Collision Avoidance
Most Cartesian machines have a rectangular free travel zone. This is nice because you can use a straight line to move between any two points. FirePick Delta (FPD) is different.
FirePick Delta is designed to have a compact footprint. For a desktop PnP robot, that's an awesome feature. However, that compactness also comes with an unusual set of movement constraints. The vertical extrusions obstruct motion.
In general, for any FPD Z-plane, we see that the FPD corner extrusions permit an unobstructed travel zone that resembles a greek cross. With such a topology, linear travel is not guaranteed. In particular, notice that line A is permitted but line B is forbidden by the movement constraints.
For discussing the FPD collision avoidance model, we define the following terms:
- cardinal direction: one of four directions numbered clockwise from 0-3 (or N,E,W,S). Collision avoidance use cases are classified according to starting/ending cardinal directions.
- linear locus: a circle centered on the origin such that any line intersecting the linear locus will be non-colliding. The radius of the linear locus will be a user configurable parameter that controls collision avoidance solutions. In the diagram above, note that line A intersects the linear locus and is therefore permitted and unrestricted. However, line B does not intersect the linear locus and presents a potential collision avoidance problem.
There are several collision avoidance use cases to consider:
- unobstructed: a line that intersects the linear locus is unobstructed
- same cardinal direction: a line with endpoints in the same cardinal direction is unobstructed even though it may not intersect the linear locus
- opposite cardinal direction: a line with endpoints in opposing cardinal directions (i.e., [0,2] or [1,3]) is unobstructed even though it may not intersect the linear locus
- orthogonal cardinal direction: a line such as line B that traverses orthogonal cardinal directions (e.g., [0,1], [0,3]) must avoid collisions
As described previously, the only use case that leads to a need for collision avoidance is a line that traverses orthogonal cardinal directions. For such lines, we may trivially break them up into two separate lines with endpoints in the linear locus. All such two-segment piecewise linear paths will be unobstructed because all segments of the path intersect the linear locus and are therefore both individually unobstructed.
Line B' does solve the collision problem but introduces another problem. The sharp angle between the two line segments of B' impedes smooth motion. To traverse such a path, FPD would need to come to a full stop at the intersection of the two line segments. Ideally, we should use a continuous smooth curve that avoids collisions. Such a continuously smooth curve would not require any "stopping in the middle," and would be the fastest way to avoid collisions.
Considering the upper right quadrant of our collision avoidance model, we introduce an example of a collision avoidance curve for line B.
- N endpoint the north endpoint of the curve is located at (50,90)
- E endpoint the east endpoint of the curve is located at (100,50)
-
linear locus radius is
70
The curve shown is a Pythagorean Hodograph Quintic (PH5) generated by PnPPath.js. As you can see, the curve completely avoids the corner obstacle and is, by definition of the PH5 curve itself, continuous in position, velocity and acceleration. PH5 curves therefore give us a smooth and fast solution for FPD collision avoidance in 2D XY space.
Pick-and-place (PnP) operations are analogous to helicopter package delivery in densely packed urban environments with skyscrapers. This is a useful mental model for extending our XY collision avoidance solution to include the Z axis and account for takeoff and landing. As in the helicopter delivery analogy, we realize that we need to introduce an additional constraint for vertical end point motion:
There should be no XY movement at either end of the PnP path.
Fortunately, PH5 curves work well in this case as well, since we just need specify the two end points and one or two intermediate waypoints that define the path apogee. Here is the PH5 curve for our Z-trajectory:
To handle the Z movement constraint, we need to define new terms:
-
hCruise is the path apogee height above the highest of the two end points of the PnP path. In this example, it is
20. -
tauTakeoff is the starting fraction of the path where XY travel is forbidden and the PnP effector is picking up a part. Given normalized [0,1] time coordinates, we can for example choose a value such as
0.1. - tauLanding is the fraction of the path where XY travel is forbidden and the PnP effector is placing a part. For simplicity, we assume that landing and takeoff are symmetric and therefore tauTakeoff = tauLanding.
Since Z and XY axis motion is orthogonal in standard 3D cartesian coordinates, we can simply combine the two curves to specify 3D motion:
Note that we have to take care and have the XY curve span the sub-interval
for tau over [0.1, 0.9]. In contrast, the Z curve will span
the full interval [0,1].
This linear combination leads to the distinct "kinks in the curve"
that you can see in the chart above at tau values of 0.1 and 0.9.
These kinks correspond to the transition from vertical flight to
horizontal flight.
By starting the XY curve at tauTakeoff, we will rely on the PHFeed of the XY curve to smooth out that kink and provide a graceful transition to horizontal flight. We can also use FireStep to implement this collision avoidance, since FireStep fully supports PH5 curve traversal.
But we are not yet done and need to look a little deeper...
Although we now have a solution to PnP collision avoidance for a cartesian machine, we have not yet accounted for the fact that FirePick Delta is a delta machine, not a cartesian machine. In general, any FPD movement will involve all three stepper motors. Indeed, our assumption of linear independence of Z and XY curves may not apply to delta machines. Superposition requires linear systems and FPD is nonlinear.
The solution that presents itself for FPD is to adapt the cartesian Z and XY collision avoidance curves for use with FPD's delta motor topology. Basically, we sample the cartesian solution presented above and perform the delta conversion into FPD delta space. Since these sample points will be on the desired collision avoidance path, we can use them as the control points for three new individual stepper PH5 curves in FPD delta stepper space. In fact, a careful choice of samples will ensure that the FPD delta PH5 curves completely avoid the takeoff and landing kinks described above (i.e., don't sample the kink). Indeed, this solution actually makes full use of the remarkable PH5 curve property that control points are on the curve.
Here is the collision avoidance curve for an orthogonal-cardinal-direction traverse similar to the one described above. In this example, we traverse between the two points, {(100,50,-60), (50,90,-70)}:
Notice that there are no kinks. All three stepper motor curves have smooth transitions from takeoff to landing and are therefore well-suited to PH5 curve representation and therefore FireStep interpolation.