Path -> Trajectory with constraints

[TrentWeiss]

Hello!

Super cool library, looks pretty useful.

I am currently working on a classical 3D motion execution problem on a racecar. I.e. I have a pre-prescribed path that I need a racecar to follow, i.e. a list of time-free waypoints in SE3 (although for relatively flat regions of SE3, the SE2 approximation in the plane of the car's axles is pretty good). Technically, these waypoints are parameterized by arclength along the path, but that doesn't help with forming a trajectory.

This racecar is quite zippy, but it's dynamics are (obviously) not instantaneous. Does Klampt have any ready-made code for producing a trajectory from this path that follows the prescribed path(as closely as math will allow) without violating the various dynamic constraints of the car? E.g. don't exceed a provided maximum velocity, linear acceleration, and (most critically) centripetal acceleration.

This problem is nothing super complicated and I could take a stab at rolling my own solution, but if Klampt already has it done, I may as well use a library and avoid the headache and risk that I'll screw it up (which is quite high).

Thanks! --Trent

[krishauser]

No, there is no out-of-the box code for this yet. There are some C++ tools for solving the time-scaling problem, which cast the problem as a linear program, but they currently don’t support centripetal constraints unless you are able to formulate the math so they can be represented as linear constraints in (path acceleration, path velocity squared). How are your constraints formulated?

[TrentWeiss]

I model the centripetal acceleration as (locally) circular motion, so that the centripetal acceleration is just where v is the path velocity and r is the local radius of curvature (which I compute by fitting a piece-wise Bezier Curve to the path and computing the radius of curvature in closed form at each point). This fitting method also enables computing arc-length's along the path very accurately with Simpson's Rule.

So the centripetal constraint would be . Where is path speed.

My original plan was to use either sequential quadratic programming (or perhaps a linearized version with linear programming) and cast the optimization problem as as:

min

s.t.

I.e. go as fast as possible, except where that would exceed limits imposed by the car's physics (tack on a minus sign to flip the standard formulation of a minimization into a maximization). and would be part of the same vector space that expands in dimension with the number of sampled points on the path, but acceleration just doesn't show up in the functional, only in the constraint. The . The factors for would be fixed constants from the Bezier Curve fits.

Note the one-side inequality for the acceleration constraint. My intuition is that if the resulting trajectory is one where the car physically can't accelerate fast enough, not a big deal. But if it results in one where it can't brake fast enough, different problem entirely (this would probably result in crash). in this formulation would be the fastest braking rate the car can achieve (negative acceleration).

Once velocities and accelerations were computed, times could be assigned as a solution to the resultant taylor series at each point (because arc lengths are available). Constraints on higher derivatives are also possible in this formulation, but 2nd order is probably good enough.

That said, I like your approach of computing timescales directly as a linear program.