Think about replacing time heuristic w/ optimization

[ennerf]

The latest trajectory optimizations (@rLinks234's block approach and a few minor changes) resulted in a >30x (2 waypoints) to >10x (100 waypoints) speedup that makes computing minjerk trajectories extremely cheap for just about all practical purposes.

Benchmarks for specified positions w/ default velocity/accel constraints (all non-end segments free) on my computer look as below

Waypoints	Time
2	0.88 us
3	4 us
5	10 us
10	27 us
20	86 us
100	1 ms
200	5 ms

Since the current time-heuristic for unspecified time vectors (distance / vel-limit * 1.875) doesn't work for non-zero velocity/accel constraints, I'm considering adding an optimization step that makes sure that velocity limits are observed.

Due to the performance improvements this should not impact users at all and also not result in any changes to the API. Observing accelerations could theoretically be done as well, but that would require extensive changes to get knowledge of the kinematics and grav comp.

@daverolli thoughts? We talked about removing the time heuristic in the 2.0 release, but this should be simple enough to add and get some runtime on it to see how well it would work.

[daverolli]

I think something like that might be good. Rather than full on optimization I think the the first step would be just to sample the trajectory check if a segment exceeds velocity limits, and then just scale the timing of any offending section to bring the velocity within limits, possibly with some tunable margin. This would hit most of our need right now. A full-on optimization step could follow later.

If we did this I would prefer to also use it as a chance to make the time heuristic separate from the current one that sits below the API, and start off doing this at the Matlab level, and then move it into Java as it matures.

[ennerf]

The sampling for velocity limits was exactly what I was thinking of. How is that different from an optimization?

I going just implement it in Java directly, but prototyping it in MATLAB works as well. I don't think have the optimization toolbox on my license, so you would have to do the prototyping. The heuristic is still the same as what you used to have in MATLAB, so it'd be easier to just use the existing code rather than me changing the public API just to get to it. (until we know what we want)

[daverolli]

I mean sample the trajectory, and if you're 1.5X over the velocity limit for a segment, increase the time for that segment by 1.5X (or more) and recompute. No gradient descent.

The full optimization was minimizing energy or some other cost while also taking into account time. So it would potentially speed up sections that were slower, and slow down sections that were faster.

[ennerf]

I tried scaling the individual segments, but that doesn't work well. I wish I had tested it in MATLAB before testing it in Java :)

trajGen = HebiTrajectoryGenerator();
positions = [ 1 -1 3 4 ];
time = [ 0 cumsum([1 1 1])];
trajectory = trajGen.newJointMove(positions, 'time', time);
HebiUtils.plotTrajectory(trajectory);

This yields a max velocity for the 2nd segment of just under 6 rad/s

After doubling the time for the 2nd segment (time = [ 0 cumsum([1 2 1])];), the max segment 2 velocity goes down by 30% to 4 rad/s

And after another doubling (time = [ 0 cumsum([1 4 1])];) the max segment 2 velocity goes up again to just under 5 rad/s

[ennerf]

Tested a bit with a naive initial implementation of gradient descent optimizing total duration while not crossing joint velocity limits. At least for a single joint it converges to practically the same solution for a wide range of initial conditions.

% input
positions = [0.2, -0.3, 1, 0.5, -2, 0.37 ];
velocities = [0.1, nan, nan, -0.5, nan, 0];
accelerations = [0, nan, 0, 0, nan, nan];
initialConditions = [100 100 100 100 100];
velLimits = [-1 +1];

% output
time = [0, 1.4594, 3.5364, 5.3019, 8.7835, 12.6355];

I think I'm warming up to the idea of making this a separate function in the API. That way it'd be easier to custom tailor it by passing optional position limits or a kinematics model w/ gravity vector.

[EABN]

First comment for me here :). Not sure if the current trajectory generator considers heuristics for the speed constraints or not. Regardless, I would be glad to bring my opinion. Would you mind sharing with me the actual cost function, optimization variables, and constraints for the trajectory generator? Is the same as in Section 3.1 and 3.2 in this paper? https://dspace.mit.edu/bitstream/handle/1721.1/106840/Roy_Polynomial%20trajectory.pdf?sequence=1&isAllowed=y

The paper seems more complicated than what it should be. If you are interested, I can generate our own optimization program with the specific dynamics and probably solvable within a sample time using interior point methods for solution instead of gradient descent.

[rLinks234]

@EABN , you are correct. Another paper to visit regarding this is "Fast, Dynamic Trajectory Planning for a Dynamically Stable Mobile Robot" by Michael Shomin, Ralph Hollis.

We're essentially just minimizing the jerk, but the "time hueristic" is used to transform the user provided time segments such that the resulting trajectory (still only minimizing jerk) has waypoints with physically feasible velocities.

[EABN]

@rLinks234 and @ennerf, Here is a formulation that allows us to satisfy position, velocity, and acceleration constraints within the optimizer without heuristics. Let me know if we are going in the right track.

To solve it, I suggest we use CVXGEN (depending on licensing costs). This would take advantage that our problem is convex and will produce a relatively fast solution. They will provide us with C code that solves our particular optimization problem. Not sure about the licensing nuances though. We can also try open source solvers like SEDUMI and YALMIP. They are fast too and so far have been pretty reliable (Could be the right initial choice). If HEBI would like to use optimization solvers more often (e.g., optimal control, optimization of inventory, sales strategies, etc), I would recommend Gurobi or Mosek for the long term investment.

[EABN]

Added code to include velocity and acceleration constraints explicitly during the optimization (trajectoryPlanner.m). I also included a function to calculate the approximate minimum time to achieve a task using a bisection method (trajectoryPlannerMinimumTime).

The code is in hebi-hardware-dev/matlab/hardware/Trajectory_Planner, it does not support multiple waypoints yet. The execution speed can be reduced significantly when bypassing cvx. @daverolli @rLinks234 @ennerf any comments on the future directions or the usefulness of the code?

[rLinks234]

Awesome. I'll take a look at the code to see if I can understand the differences from what algorithm we're currently using. I may bug you to explain some things for me...

[EABN]

@rLinks234 I forgot to mention that the functions use cvx; we are using the free-solvers version of it. To install just follow the instructions here. I will be glad to talk about the installation details or the inner side of the code.

[EABN]

Just created a function that uses the newJointMove() method from the HebiTrajectoryGenerator to generate a trajectory that passes through different waypoints, minimizes trajectory time, and satisfies velocity constraints.

Here is an example of use: % Desired waypoints waypoints = [0, -2*pi/6, 0.1, 0, pi]; % Max. velocity (90 RPM is maximum speed of X5-1) maxVelocity = 90*2*pi/60; % Calculate minimum time trajectory trajectory = newJointMoveMinimumTime(waypoints, maxVelocity);

The resulting trajectory looks like this:

As a benchmark, here is a trajectory that calculates time by dividing the delta of distance over the max. speed and scaling the resulting vector of time to satisfy the constraints:

The example code is in hebi-hardware-dev/matlab/hardware/Trajectory_Planner and requires the free open-source optimization library NLopt to compute the solution.

Some related but not important notes:

1. I also tried a similar implementation of the unconstrained QP using cvxgen. Solving for 1 DOF and 2 waypoints took on average 50us on the computer that I have. Conclusion: The solution speed of the current trajectory generator is extremely fast, you guys set the bar really high :1st_place_medal: :)!

2. Using the current version of newJointMove for the delta of positions that are very small leads to unexpected behavior of the solution.

3. The proposed function newJointMoveMinimumTime could be modified to fit multiple DOFs and velocity and acceleration constraints within the waypoints.