Feasibility and constraints

[jbourne15]

I am confused about input feasibility and constraints that you can specify for a 3 dimensional trajectory.

For example I can get infeasibility if I have the following set up:

std::vector<double> segment_times(vertices.size()-1,2);
const double v_max = 5.5;
const double a_max = 1.0*9.81;

nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max);

nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max);

now If I check the input_feasibility for my generated trajectory via:

mav_trajectory_generation::InputConstraints input_constraints;
input_constraints.addConstraint(ICT::kFMax, a_max); 
input_constraints.addConstraint(ICT::kVMax, v_max); 

I get isInfeasibleThrustHigh when I check all my segments.

I think this has something to do with how I setting my segment_times to all be 2 secs. So my question is: is segment_times a constraint that overcomes the velocity and accel constraints via addMaximumMagnitudeConstraint that I set.

FYI, In my application I need to specify the timing so setting segment_times is necessary. Any insight is greatly appreciated.

[rikba]

I think this is a confusing definition of f_max and a_max on our side.
f_max = a_max + 9.81 describes the maximum thrust in [m/s^2] the rotors can supply.

Please check whether input_constraints.addConstraint(ICT::kFMax, a_max+9.81); works for you.

[jbourne15]

Sorry for the delayed response. Yes this seems to work, I don't get isInfeasibleThrustHigh anymore as long as my segment_times are long. Can you explain to me what the segment_times are exactly? Are they an initial guess or constraint? They seem to be more of a constraint when testing the code, but looking at the reference papers in the readme makes it seem like its an initial guess and is optimized. Also the parameter.time_penalty would suggest the latter.

When I run some example code with segment_times large i.e. std::vector<double> segment_times(vertices.size()-1,100); and set parameter.time_penalty to be large, the resulting trajectory has very small velocities (far below the constraints I place) or otherwise a very slow trajectory. This would suggest the time is not being optimized even though the penalty is high.

If I run the same code but set the segment_times using mav_trajectory_generation::estimateSegmentTimes(vertices, v_max, a_max) the resulting trajectory is not feasible with respect to the constraints I place i.e. nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max); and input_constraints.addConstraint(ICT::kVMax, v_max);. For example, if I set v_max=1 and a_max=2.5 I get isInfeasibleVelocity. Shouldn't the addMaximumMagnitudeConstraint ensure the resulting trajectory is feasible wrt to the specific constraint? Why when I check the input_constraints do I get infeasible velocity?

The vertexes for my trajectory: (0,0,2), (2,2,2), (-2,2,2), (-2,-2,-2), (0,0,2).

[jbourne15]

They default to soft constraints. By setting the use_soft_constraints=false nlopt uses hard constraints. Also the time alloc parameter must be set properly to minimize time. However, when I use hard constraints the optimization still converges with infeasible velocities even though I have them constrained. My setup is basically the same as the tutorial. Any ideas?

[jbourne15]

According to the paper by Burri M. et al. 2015.

"Also, it turned out in practical experiments that adding inequality constraints increases the number of necessary iterations significantly and the optimizer does not always respect the constraints."

Also from the paper by Richter C. et al. 2016.

"Due to the non-convexity of the feasible set in flat output space, the optimization
algorithm may encounter an actuator limit and terminate before converging to the optimal ratio of segment times (for example, one of the red or orange lines in Figure 2)."

So there is no guarantee of feasibility regardless of soft or hard constraints. Is that correct?

In my particular application I need trajectories to be feasible. I have tried using the suggestion by Richter by just ignoring the feasibility constraints and adjust the segment times afterwards to become feasible but because my trajectory's first vertex depends on the previous feasible trajectory this constraint needs to be withheld and adjusting the time changes that.

I have also tried various parameter settings by lowering time penalty and increasing the soft constraint weight but can't get anything to be consistently feasible. Also adjusting time allocation parameter doesn't help either (using kRichterTimeAndConstraint and kSquaredTimeAndConstraints). I have only tried a few parameter options, but can anyone comment on how easy it is to tune the optimization parameters for feasibility or comment on a methodology to do so?

One thing I should mention is that my vertexes are fairly spread out. For example it is not uncommon for me to use vertexes like this:
constraints:
type: position value: [14.55, 67.83, 2.262]
type: velocity value: [ 0.2373, 2.01, 0.01177]
type: acceleration value: [ 0.01577, -0.02794, 5.613e-05]
type: jerk value: [0.0006395, -0.009707, -3.51e-05]
type: snap value: [ 7.009e-05, 3.405e-06, -2.599e-07]

constraints:
type: position value: [43.87, 4.859, 1.889]

constraints:
type: position value: [16.72, 66.54, 1.914]

constraints:
type: position value: [25.03, 94.59, 1.969]

constraints:
type: position value: [6.449, 78.12, 1.995]

constraints:
type: position value: [43.32, 81.49, 2.144]

I also use the setup:

parameters.max_iterations = 3000;
parameters.f_rel = 1.0e-6;
parameters.time_penalty = .01;
parameters.initial_stepsize_rel = .1;
parameters.inequality_constraint_tolerance = .01;
parameters.soft_constraint_weight = 10; 

parameters.time_alloc_method = mav_trajectory_generation::NonlinearOptimizationParameters::kRichterTimeAndConstraints;

v_max = 3;
a_max = 10;
j_max = 10;

const int Nnl = 10;
mav_trajectory_generation::PolynomialOptimizationNonLinear<Nnl> nlOpt(dimension, parameters);
nlOpt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::VELOCITY, v_max);
nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, a_max);
nlOpt.addMaximumMagnitudeConstraint(mav_trajectory_generation::derivative_order::JERK, j_max);
nlOpt.optimize();

Generated trajectory:

--- optimization info ---
optimization time: 0.0578275
n_iterations: 132
stopping reason: FTOL_REACHED
cost trajectory: 0.00357025
cost time: 9.7972e+06
cost soft constraints: 1e+12
maxima:
velocity: 7.11072 in segment 4 and segment time 23.8463
acceleration: 0.684812 in segment 4 and segment time 23.8463
jerk: 0.041488 in segment 2 and segment time 9.49892

The segment[0] isInfeasibleVelocity.
The segment[1] isInfeasibleVelocity.
The segment[2] isFeasible.
The segment[3] isFeasible.
The segment[4] isInfeasibleVelocity.

So as you can see even though clearly my soft constraints are not being met my optimization is converging.

Lastly, my questions are:

1. Is there no guarantee of feasibility (regardless of soft or hard constraints)?
2. Can anyone please comment on how easy it is to tune the optimization parameters for feasibility or comment on a methodology to do so?
3. Is it more difficult to generate feasible trajectories with wide spread (in position) vertexes?
4. Do the non zero initial vertex constraints cause issues for the optimization e.g. do high initial velocity constraints lead to infeasibility?
5. Would using a global optimization routine help?