Open jbourne15 opened 5 years ago
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.
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).
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?
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:
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:
now If I check the input_feasibility for my generated trajectory via:
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: issegment_times
a constraint that overcomes the velocity and accel constraints viaaddMaximumMagnitudeConstraint
that I set.FYI, In my application I need to specify the timing so setting
segment_times
is necessary. Any insight is greatly appreciated.