Script question

[Iko-Git]

Hello! Kindly, I want to as if I can, with Flashlight, impose inequality constraints on the first and second derivatives of the piecewise polynomial trajectory. What I am aiming to do is to generate a snap optimized piecewise polynomial trajectory (based on a number of 3D waypoints), in which the zeroth (the minimum snap piecewise polynomial), the first (speed), and the second (acceleration) derivatives won't exceed a certain value. Any example/guidance would be highly appreciated.

[mikeroberts3000]

Hi! Great questions. There are several subtle issues that affect the structure of the resulting optimization problem. Regardless of your answers to these questions, what you want to do is possible. But your answers to these questions will determine: (a) if what you want to do is already implemented in Flashlight; and (b) how well-behaved the resulting optimization problem is.

• In your application, are you considering the polynomial parameter t to be time? In other words, do you want to impose constraints on velocity and acceleration of a body moving through space? Or are you considering t to be an arbitrary parameter that is used to control the shape of a path, with a separate progress curve or something to determine the evolution of the body over time.

• Do you want to impose constraints for a few specific values of t, e.g., at the beginning and end of a trajectory? Or do you want to impose your constraints everywhere along the trajectory. If you want to impose the constraints everywhere, could you live with imposing constraints on a dense sampling of points along the trajectory? Or do you really need to impose the constraints everywhere, e.g., because you want to avoid very thin obstacles?

[Iko-Git]

@mikeroberts3000 Thanks for your reply. Let me reply to your comments one by one.

• Yes, within my application I am considering the polynomial parameter t to be time, and I want to impose constraints on the velocity and the acceleration of a body moving through space (3D). More in detail, I am trying to end up with a piecewise polynomial that is constructed based on a number of waypoints in 3D space (let's refer to it by NR_W), and in my case, the number of the segments in the piecewise poly is (NR_W - 1).
• Regarding the constraints, I want to impose the following: --Velocity and acceleration are zero at the first and last waypoint. --Continuity constraint, up to the 4th derivative (the snap), at every waypoint in between the first and the last waypoints. --Limit the maximum value of the segments in the piecewise, its first, and second derivatives. In other words, limit the maximum value which the polynomial and its first two derivatives (speed and acceleration) can take in every segment in the piecewise.

Or do you really need to impose the constraints everywhere, e.g., because you want to avoid very thin obstacles?

Avoiding VERY thin obstacles, NO! What I am aiming to do is to avoid crashing with other agents in the space, by setting the last-mentioned constraint above. In other words, setting the polynomial maximum value of every segment such that the agent can't go out of a corridor constructed between two waypoints (prohibit it to enter another agent's corridor)

Thanks again for your support

[mikeroberts3000]

Based on what you're saying, I think your best bet is to derive a new optimization problem and solve it using an off-the-shelf QP solver. You can use our code as a reference, but I don't think everything you want to do is implemented in Flashlight. Some of the constraints you want are implemented in Flashlight, but not in a clean way, and how to invoke them isn't well-documented. As an optional pro tip, I recommend getting comfortable with SymPy to grind through the calculus for you.

Yes, within my application I am considering the polynomial parameter t to be time.

Ah perfect, this makes things easier. If the position is represented as a polynomial and t is time, then velocity, acceleration, etc will also be polynomials, and this will lead to an especially convenient mathematical formulation. I recommend re-deriving our original optimization problem (see our SIGGRAPH Asia 2015 paper for a gentle walkthrough), but in a way that includes your specific constraints. You will find that all the constraints you would like to apply at specific times can be formulated as linear (with respect to the polynomial coefficients that represent the trajectory) equality and inequality constraints, thereby preserving the convexity of the problem. If you have really long trajectories (i.e., really large t values), you might need to worry about numerical conditioning, but I recommend not worrying about this at first.

Avoiding VERY thin obstacles, NO!

Even better. This also makes things easier. You can approximate your maintain condition X at all times constraints with a sampling approach, i.e., with a finite collection of constraints at specific times. If you end up with a trajectory that violates condition X at any in-between times, you can simply add more constraints at the in-between times and re-solve. Either way, you'll end up with a tiny well-behaved QP that can be solved in a few milliseconds, so iteratively adding constraints and re-solving it isn't a big deal.

What I am aiming to do is to avoid crashing with other agents in the space.

In general, enforcing mutual collision avoidance in a multi-agent setting is more difficult to encode. But if you can restrict each agent to their own fixed convex region ahead of time, then you can solve a separate convex optimization problem for each agent. If you need to restrict each agent to a non-convex region, there are also methods for that, but they destroy the convex structure in the problem.

Anyway, I hope that helps, feel free to post here if you're stuck.