matthewpeterkelly / optimtraj

A trajectory optimization library for Matlab

OptimTraj - Trajectory Optimization for Matlab

OptimTraj is a matlab library designed for solving continuous-time single-phase trajectory optimization problems. I developed it while working on my PhD at Cornell, studying non-linear controller design for walking robots.

What sort of problems does OptimTraj solve?

Examples:

• Cart-pole swing-up: Find the force profile to apply to the cart to swing-up the pendulum that freely hanges from it.
• Compute the gait (joint angles, rates, and torques) for a walking robot that minimizes the energy used while walking.
• Find a minimum-thrust orbit transfer trajectory for a satellite.

Details:

OptimTraj finds the optimal trajectory for a dynamical system. This trajectory is a sequence of controls (expressed as a function) that moves the dynamical system between two points in state space. The trajectory will minimize some cost function, which is typically an integral along the trajectory. The trajectory will also satisfy a set user-defined constraints.

OptimTraj solves problems with

• continuous dynamics
• boundary constraints
• path constraints
• integral cost function
• boundary cost function

All functions in the problem description can be non-linear, but they must be smooth (C2 continuous).

Features:

• Easy to install - no dependencies outside of Matlab (for base functionality)
• Lots of examples - look at the demo/ directory to see for yourself!
• Readable source code - easy to debug your code and figure out how the software works
• Analytic gradients - most methods support analytic gradients
• Rapidly switch methods - choose from a variety of methods:
  • direct collocation
    • trapezoid
    • Hermite-Simpson (seperated)
  • direct multiple shooting
    • 4th-order Runge-Kutta
  • global (pseudospectral) collocation
    • Chebyshev (Lobatto) -- (requires chebfun)

Installation:

1. Clone or download the repository
2. Add the top level folder to your Matlab path
3. (Optional) Clone or download chebfun (needed for global collocation)
4. Done!

Usage:

• Call the function optimTraj from inside matlab.
• optimTraj takes a single argument: a struct that describes your trajectory optimization problem.
• optimTraj returns a struct that describes the solution. It contains a full description of the problem, the transcription method that was used, and the solution (both as a vector of points and a function handle for interpolation).
• For more details, type help optimTraj at the command line, or check out some of the examples in the demo/ directory.

Documentation:

The best way to learn OptimTraj is by working through a few of the examples in the demo/ directory. There is also a .pdf user guide that can be found in the /docs directory.

For more background on trajectory optimization in general, I wrote a tutorial paper that includes derivations and references for most methods implemented here, along with a variety of practical suggestions and debugging tips. Finally, I have a tutorial webpage for trajectory optimization.

Citing OptimTraj

The best way to cite OptimTraj is using the DOI assigned the the library source code:

@software{Kelly_OptimTraj_Trajectory_Optimization_2022,
  author = {Kelly, Matthew Peter},
  doi = {10.5281/zenodo.7430524},
  month = {12},
  title = {{OptimTraj: Trajectory Optimization for Matlab}},
  url = {https://github.com/MatthewPeterKelly/OptimTraj},
  version = {1.7},
  year = {2022}
}

Alternatively, you can cite the tutorial paper that accompanies OptimTraj:

@article{kelly2017introduction,
  title={An Introduction to Trajectory Optimization: How to do your own Direct Collocation},
  author={Kelly, Matthew},
  journal={SIAM Review},
  volume={59},
  number={4},
  pages={849--904},
  year={2017},
  publisher={SIAM}
}

Contribute:

Contributions are welcome! Feel free to reach out if you're planning a bigger submission, or just go ahead and make a pull request for smaller contributions.

If you are interested in contributing, here are a few possible things to do:

• Create additional demo problems
• Identify holes in the documentation
• Report bugs
• Implement new methods or features
• Look through the open issues

Contributions:

• Will Wehner implemented analytic gradients in the 4th-order Runge-Kutta method.

• Conrad McGreal contributed the minimum-time 3D quadrotor example.

optimtraj's Issues

How to visualize path costs?

1. I want to display the bound and path cost for a solution. The closest thing that I found was soln.info.objVal.

2. I noticed that @(t0,x0,tF,xF) bndCst(soln.time(1), soln.time(end)) + sum(@(t,z,u) pathCst(soln.control)) is not equal to soln.info.objVal. What is going on here?

Best method to add path points

Hi @MatthewPeterKelly, thank you for this awesome project and the description. I'm new to trajectory optimization and want to setup something specific:

Beside the start and end point, I want to reconstruct the trajectory of a vehicle from it's radar reflections. That means, beside the start and end-point, I want to add multiple path-points of the trajectory in between at certain time values. Whats the best way of doing so?

I've started by modifiying the toyCar example, see attachment, in which some points were inserted and assigned to certain trajectory points. In the pathObjective and pathConstraint, the positions and time were assigned. Is this good practice or would you recommend doing it differently?

I defined the path points here:

The first index is the trajectory point where this point is assigned to. The second index is the time in second. Third and fourth index are the x and y positions.

P.s.: The dynamics of the toyCar were adapted to include velocity, steering angle, etc.

========================================================
Edit: Perhaps multiple shooting would be the right approach? Do you have an example on how to implement this?

toyCar.zip

Generate trapezoidal collocation constraints

Hi @MatthewPeterKelly ,

Excuse me fro the naive question. I'm new here
I'd like to know if the user should always define the constraints (eg. non-linear equality constraints)
for solving the trajectory optimization problem.
For example, The system dynamics that I want to provide as a set of constraints is of the form
dS/dt = -AS. (this represents the dynamics of a graph network)

S is a vector and A is a matrix.

I would like to know if it is possible to automatically generate trapezoidal collocation constraints using OptimTraj. I'd like to supply these constraints to fmincon

The complete description of the problem can be found here

Costate Estimates

Do you plan to compute estimates of the costates and provide those with the OptimTraj solution, in addition to the states and controls? The costates can be useful if one wants to use an indirect method. Many optimal control solvers, such as the MATLAB solvers GPOPS-II, DIDO, PROPT, ICLOCS, and FALCON.m and the C++ solver PSOPT, do provide costate estimates in the solution.

GPOPS-II: http://www.gpops2.com/
DIDO: http://www.elissarglobal.com/academic/products/
PROPT: http://tomdyn.com/
ICLOCS: http://www.ee.ic.ac.uk/ICLOCS/
FALCON.m: http://www.fsd.mw.tum.de/software/falcon-m/
PSOPT: http://www.psopt.org/

Hessians

Some of the OptimTraj methods (i.e. trapezoid, hermiteSimpson, rungeKutta) are able to exploit user-supplied gradients of the dynamics, path objective, etc. functions. Are there plans for the OptimTraj methods to also accept Hessians of these functions?

Portability of Interpolation

Part of the solution struct includes a function handle for interpolating the solution and the error estimates. This handle sometimes breaks if the solutions is saved and then opened on a different computer.

There might be a simple fix by moving some of the sub-functions to the top level directory of the code. A slightly more challenging (but robust) fix is to replace the interpolation with a data structure that can be read by the matlab command ppval. Then there is no need to pass around function handles.

Is the Runge-Kutta integrator in the OptimTraj multiple shooting method vectorized for greater speed and efficiency?

Is the Runge-Kutta integrator in the OptimTraj multiple shooting method vectorized for greater speed and efficiency?

Error with running quadRotor2d demo

Hello,

I attempted to run the "MAIN_simulate.m" and "MAIN_minForceTraj.m" files in the quadRotor2d demo, but received the following error in both cases:

Index in position 1 exceeds array bounds (must not exceed 2).

Error in dynamics (line 14)
q = x(1:5,:);

Error in MAIN_simulate>@(t,z)(dynamics(z,ctrlFun(z),p))

Error in odearguments (line 90)
f0 = feval(ode,t0,y0,args{:}); % ODE15I sets args{1} to yp0.

Error in ode45 (line 115)
odearguments(FcnHandlesUsed, solver_name, ode, tspan, y0, options, varargin);

Error in MAIN_simulate (line 29)
soln = ode45(dynFun,tSpan,z0);

I know the OptimTraj library should be compatible with any release of MATLAB, but I encountered this error using both the 2016a version and the 2018b version. Is there a change from earlier versions that might be causing this error?

How to add a terminal cost to the objective function

Hi, I am trying to add a terminal cost to the objective function that is only state dependent, I tried doing this:

function objectiveFunctions()
    R = [0, 0, 0, 0, 0];
    Q = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10]
    tar = [...] % Some Value
    F = u1*R(1)*u1 + u2*R(2)*u2 + u3*R(3)*u3 + u4*R(4)*u4 + u5*R(5)*u5;
    F = F + (q1-tar(1))*Q(1)*(q1-tar(1)) + (q2-tar(2))*Q(2)*(q2-tar(2)) + (q3-tar(3))*Q(3)*(q3-tar(3)) + (q4-tar(4))*Q(4)*(q4-tar(4)) + (q5-tar(5))*Q(5)*(q5-tar(5));
    F = F + (dq1-tar(6))*Q(6)*(dq1-tar(6)) + (dq2-tar(7))*Q(7)*(dq2-tar(7)) + (dq3-tar(8))*Q(8)*(dq3-tar(8)) + (dq4-tar(9))*Q(9)*(dq4-tar(9)) + (dq5-tar(10))*Q(10)*(dq5-tar(10));
    [f, ~, fz, fzi, ~]  = computeGradients(F,z,empty);

    matlabFunction(f,fz,fzi,...
        'file','autoGen_obj_quadraticCost2.m',...
        'vars',{'q1','q2','q3','q4','q5','dq1','dq2','dq3','dq4','dq5','u1','u2','u3','u4','u5'});        
end

and in the objective function I gave to the problem I did this

dObj_end = autoGen_obj_quadraticCost2(x(1,end),x(2,end),x(3,end),x(4,end),x(5,end),x(6,end),x(7,end),x(8,end),x(9,end),x(10,end),u(1,end),u(2,end),u(3,end),u(4,end),u(5,end));
dObj_pre = autoGen_obj_quadraticCost(x(1,1:end-1),x(2,1:end-1),x(3,1:end-1),x(4,1:end-1),x(5,1:end-1),x(6,1:end-1),x(7,1:end-1),x(8,1:end-1),x(9,1:end-1),x(10,1:end-1),u(1,1:end-1),u(2,1:end-1),u(3,1:end-1),u(4,1:end-1),u(5,1:end-1));
dObj = [dObj_pre, dObj_end];

Where autoGen_obj_quadraticCost is the similar to autoGen_obj_quadraticCost2 with the only difference being the R matrix is not zeros. I tried this but at the end the control just blows up since I suppose the cost function defined in autoGen_obj_quadraticCost2 also is used to calculate the cost on states other than the last state.

Can anyone help me on this matter?

Thanks in advance!

GPOPS-II Interpolation Function Handle returned by OptimTraj

In gpopsWrapper.m, OptimTraj provides an interpolation function handle for the GPOPS-II solution using pchip, as per the code below. Is pchip the most accurate way to interpolate the GPOPS-II solution?

soln.interp.state = @(t)( interp1(tSoln',xSoln',t','pchip',nan)' );
soln.interp.control = @(t)( interp1(tSoln',uSoln',t','pchip',nan)' );

The formula for the GPOPS-II state is given in equation (25) of the GPOPS-II paper:

Patterson, Michael A., and Anil V. Rao. "GPOPS-II: A MATLAB software for solving multiple-phase optimal control problems using hp-adaptive Gaussian quadrature collocation methods and sparse nonlinear programming." ACM Transactions on Mathematical Software (TOMS) 41.1 (2014): 1.

http://dl.acm.org/citation.cfm?id=2558904

solved

Specifying dynamics on a lower dimension

Hi Matt, thank you for this software package. I was wondering if there is a way to specify partial dynamics that are projected on a lower-dimensional space. For example, instead of xdot = f(t,x,u), we specify E*xdot = f(t,x,u) where E is a matrix of size (m,n) where m<n so f will be [m,1].

Normally, E is eye(n,n) and f is [n,1] but if we know dynamics only for some part of the state, then we can use an E to do the trick.

ADiGator for OptimTraj

ADiGator (https://sourceforge.net/projects/adigator/) is free, open-source MATLAB software that computes vectorized gradients (and even Hessians) via automatic differentiation. It was developed by the author of GPOPS-II to make it easier to supply vectorized gradients and Hessians to GPOPS-II. Perhaps ADiGator could be integrated into OptimTraj so that OptimTraj can optionally use ADiGator to generate vectorized derivatives of the optimal control functions (e.g. dynamics, path objective, etc.). This would be very useful, as it is often very difficult to manually derive and implement vectorized gradients (and Hessians).

Why does OptimTraj print out the following message, even for the demo examples?

When I run OptimTraj, it prints out the message below. Perhaps OptimTraj could be modified to prevent this message from being printed.

Your initial point x0 is not between bounds lb and ub; FMINCON
shifted x0 to strictly satisfy the bounds.

Are the OptimTraj chebyshev and multiCheb methods able to exploit user-supplied gradients of the pathObjective, dynamics, etc...?

The trapezoid, hermiteSimpson, and rungeKutta methods in OptimTraj are able to exploit user-supplied gradients of the pathObjective, dynamics, etc... Are the OptimTraj chebyshev and multiCheb methods also able to exploit user-supplied gradients?

Vectorized Gradients for OptimTraj Example Problems

Which symbolic solver was used to generate the gradients for the OptimTraj example problems? Does that symbolic solver directly generate vectorized gradients, because it looks like the analytic gradients implemented in the OptimTraj examples have all been vectorized.

Improve input validation

Right now the input validation will not warn you if there is an unused field in the options struct. This results in a problem when you make a typo in a field name, causing the solver to use the default options quietly.

How to cite OptimTraj?

Is there a recommended way to cite OptimTraj in academic research? A BibTeX entry would be especially useful.

Constraints on the Control Inputs

Can I have a constraint on the control input such that '||\mathbf{u}||<=u_{max}'?

Mesh analysis for Chebyshev method

So far, only the trapezoid and hermiteSimpson methods support mesh analysis. The key idea here is that you can compute an estimate of the error in the dynamics along the trajectory and then use this to construct a new (more accurate) mesh for the next iteration. Look up hp-adaptive meshing if your curious, or send me an email.

More NLP Solver Options for OptimTraj

The MATLAB code fminsdp: https://www.mathworks.com/matlabcentral/fileexchange/43643-fminsdp would make it really easy for OptimTraj to easily switch between several different 3rd party NLP solvers like IPOPT, SNOPT, KNITRO, and fmincon, assuming the user already has them installed. The documentation says that fminsdp is designed for small-scale optimization problems, but as long as there are no matrix inequality constraints (which is the novelty of fminsdp), fminsdp should be able to solve large-scale NLP problems (like those formulated from an optimal control problem by OptimTraj) via the third-party solvers. The fminsdp and fmincon interfaces are almost the same, so you would basically just have to replace the fmincon call in the OptimTraj code to fminsdp.

when run MAIN_forceSquared.m ,it shows err use dynamics ,the output parameters is too much ,ERR: MAIN_forceSquared>@(t,x,u)(dynamics(x,u))

错误使用 dynamics
输出参数太多。

出错 MAIN_forceSquared>@(t,x,u)(dynamics(x,u))

出错 directCollocation>myCstGrad (line 627)
[f, fGradRaw] = dynFun(t,x,u);

New Demo: balancing an inverted pendulum in space

This is a thing that neat quad-copter demos do all of the time. Move a balanced and inverted pendulum between two points in 3D space, by applying a force to the base. The force can push on the pendulum, but not pull.

Problem with initial guess P.x0

Hi,
I wrote the matlab function which calculates dynamics for an inverted pendulum and I would like to use OtimTraj to find a Swingup Trajectory however as I run the simulation I see this warning from FMINCON :

Your initial point x0 is not between bounds lb and ub; FMINCON
shifted x0 to strictly satisfy the bounds.

Hier is how P.x0 looks like :

and after that this warning :
Warning: Matrix is close to singular or badly scaled. Results may
be inaccurate. RCOND = 3.066313e-18

Hier is how I set the initial conditions and boundries :
( as an Input I use the Cart's acceleration ! )

`clc; clear;
addpath OptimTraj

dist = 0 ;
maxForce = 40;
duration = 2;

problem.func.dynamics = @(t,x,u)( invPendDynamics(x,u) );
problem.func.pathObj = @(t,x,u)( u.^2 ); %Force-squared cost function

problem.boundslow = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = duration;
problem.bounds.finalTime.upp = duration;

problem.bounds.initialState.low = [0;pi;0;0];
problem.bounds.initialState.upp = [0;pi;0;0];
problem.bounds.finalState.low = [0;0;0;0];
problem.bounds.finalState.upp = [0;0;0;0];

problem.bounds.state.low = [-2;-2pi;-inf;-inf];
problem.bounds.state.upp = [2;2pi;inf;inf];

problem.bounds.control.low = -maxForce;
problem.bounds.control.upp = maxForce;

problem.guess.time = [0,duration];
problem.guess.state = [problem.bounds.initialState.low, problem.bounds.finalState.low];
problem.guess.control = [0,0];
`
Do you know what might be causing the error ?

How to add time-varying control bounds

How can I add a time-varying control bounds to my problem?
Such as the control bounds varies with the state of the system.

Add support for Backwards Euler integration method

One key feature in OptimTraj is the ability to compare different collocation methods. There is currently support for Hermite--Simpson collocation and trapezoidal collocation, as well as 4th-Order Runge--Kutta multiple shooting. It would be nice to add support for additional methods, and an good next step would be Backwards Euler collocation.

This feature could be implemented by copy-pasting the trapezoid.m file and switching it over to use backwards Euler](https://en.wikipedia.org/wiki/Backward_Euler_method) integration. The might be a few other details to work out, such as adding support to the top-level interfaces for the new method and also adding an example to show how to use the new method.

OptimTraj User Documentation error: Path and Boundary Constraints

The documentation PDF states (page 3, section 2.2) that for the path and boundary constraint functions, the first output should be the equality constraint and the second output should be the inequality constraint.

However, the comments in the OptimTraj function itself (optimTraj.m) state that the first output is the _in_equality constraint and the second is the equality constraint. My experimentation with the function has revealed that this is correct and that the documentation is mistaken.

Direct Collocation on an arbitrary grid

Right now, the direct collocation methods assume that the time grid in the trajectory is uniform. It would be much better if the user could supply an arbitrary grid on which to solve the problem.

DIDO and PROPT Integration with OptimTraj

The commercial MATLAB optimal control solver GPOPS-II has been integrated into OptimTraj. It may be useful to also integrate the commercial MATLAB optimal control solvers DIDO and PROPT into OptimTraj.

DIDO: http://www.elissarglobal.com/academic/products/
PROPT: http://tomdyn.com/

Naming Discussion: OptimTraj, or something better?

It seems that the name TrajOpt is already taken by a software package from Pieter Abeel's group at Berkeley.

This library should be renamed. So far the best that I've come up with is OptimTraj, but I'm open to suggestions.

fmincon has 4 algorithm options: 'interior-point', 'trust-region-reflective', 'sqp', 'active-set'. Is OptimTraj capable of using any of these 4 NLP algorithms? Does OptimTraj default to use the default fmincon algorithm 'interior-point'?

I have been able to use the fmincon algorithms: 'interior-point', 'sqp', 'active-set' . But the 'trust-region-reflective' algorithm produces this error:

Running OptimTraj, iteration 1
-> Transcription via Hermite-Simpson method, nSegment = 6

Error using fmincon (line 446)
Option Algorithm = 'trust-region-reflective' but this algorithm does not solve problems with the
constraints you have specified. Run a different algorithm by setting the Algorithm option. For information
on applicable algorithms, see Choosing the Algorithm in the documentation.

Error in directCollocation (line 95)
[zSoln, objVal,exitFlag,output] = fmincon(P);

Error in hermiteSimpson (line 77)
soln = directCollocation(problem);

Error in optimTraj (line 182)
soln(iter) = hermiteSimpson(P);

Error in invoke_OptimTraj (line 170)
soln = optimTraj(problem);

Clean up installation

Make it clear to users how to install this library.

• Add a INSTALL.md file
• Move source code into a src/ directory
• Add a simple installation script to add the library
  • include a flag to optionally install examples

Error using trapezoid/rungeKutta methods but none with chebyshev

I get the following error while using trapezoid/rungeKutta methods only. My problem is of a reentry vehicle optimization with 8 states, 2 controls, and an objective to minimize time. I can use the Chebyshev method though it never converges.

Acrobot demo throws an error:

I added the main folder to my matlab path. Then went to the folder for the acrobot, openend the main file, ran it and got:

Running OptimTraj, iteration 1
  -> Transcription via trapezoid method, nGrid = 20

Unrecognized function or variable 'getIpOptions'.

Error in fmincon (line 860)
    options = getIpOptions(options,sizes.nVar,mEq,flags.constr,defaultopt,10,0.01);

Handle multi-phase problems

Many optimal control problems can be more easily solved by splitting the trajectory into multiple phases, joint by some interface (e.g., continuity/discontinuity) conditions.

Would it be possible to add this feature?

Unrecognized field name

I've just cloned the repository and tried running a few of the demos, but I keep getting the following error:

WARNING: options.nlpOpt.GradConstr is not a valid option
WARNING: options.nlpOpt.GradObj is not a valid option
WARNING: options.nlpOpt.DerivativeCheck is not a valid option
WARNING: options.nlpOpt.GradConstr is not a valid option
WARNING: options.nlpOpt.GradObj is not a valid option
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Running OptimTraj, iteration 1
  -> Transcription via trapezoid method, nGrid = 10
Unrecognized field name "GradObj".

Error in directCollocation (line 26)
flagGradObj = strcmp(Opt.nlpOpt.GradObj,'on');

Error in trapezoid (line 74)
    soln = directCollocation(problem);

Error in optimTraj (line 180)
            soln(iter) = trapezoid(P);

Error in MAIN (line 167)
soln = optimTraj(problem);

That's specifically the printout from the gradientsTutorial.
I've checked that problem.options(1).nlpOpt.GradObj is set to on before being passed from Main.m > optimTraj.m > trapezoid.m > directCollocation.m. I also haven't modified the files in any way. I just opened Main.m in the demo folder and hit run. Is anyone else experiencing this or am I missing something?

Can this be used for time parameterization?

I love the work you've done with both this project and Kinematic_Constrained_Trajectory. I was wondering if any of your work can solve the time parametrization problem? I ran your Matlab code and read through some of it, and it seems you always specify the time interval of your waypoints or max time allowed. Is this a constraint that must be pre-calculated? I'm trying to find a solution for this problem.

Thanks!

What is the deal with the undocumented MultiCheb method in OptimiTraj?

Is there any issue with the 'multiCheb' method? It doesn't seem to be documented in the users' guide, but it seems to give good answers for the demo examples and for my particular optimal control problem.

Bug (?) in OptimTraj wrapper for GPOPS?

I'm having some trouble getting the OptimTraj gpops method to work on my problem in certain cases. Do you know what might be causing the error below? I think this error may be related to how OptimTraj wraps GPOPS-II, because whenever I use GPOPS-II independently from OptimTraj on my optimal control problem, I never get this error.

| |
| Finding Solution on Mesh 2 |
|___________________________________________________________________|
Computing Radau Points, Integration Weights, and Integration Matrix
Creating Bounds For Nonlinear Program
Creating Guess For Nonlinear Program
Error using griddedInterpolant
Interpolation requires at least two sample points in each dimension.

Error in interp1 (line 161)
F = griddedInterpolant(X,V,method);

Error in gpopsGuessRPMI

Error in gpopsSolveRPMI

Error in gpopsMeshShell

Error in gpops2

Error in gpopsWrapper (line 84)
output = gpops2(setup);

Error in optimTraj (line 190)
soln(iter) = gpopsWrapper(P);

Error in invoke_OptimTraj (line 176)
soln = optimTraj(problem);

Error when running toyCar in octave

Hi, I am running toyCar's MAIN.m file, and get this error

error: pivot is zero
error: called from
    gjp at line 42 column 5
    cpiv_bard at line 76 column 4
    __lm_feasible__ at line 302 column 31
    fmincon at line 417 column 24
    directCollocation at line 95 column 33
    trapezoid at line 74 column 10
    optimTraj at line 180 column 24

I am wondering if this is due to me using octave, or if there is a fundamental bug happening.
Another "fix" I've had to do is to add

'GradObj', 'off',...
'GradConstr', 'off'

options in optimset in GetDefaultOptions.m. Otherwise directCollocation.m's line 26
flagGradObj = strcmp(Opt.nlpOpt.GradObj,'on');
was raising an error.

Not sure if this is also an issue due to octave or if it's actually an issue with the code.

long run time

Hi,
I'm solving a optimal control problem with your great solver,
but some times it took so so long time to show the results, I attached a sample picture,
Is there any trick like changing the stopping criteria to resolve this problem?

Support for multiple-phase problems

As of now, TrajOpt can only solve single-phase trajectory optimization problems. The extension to multiple-phase problems isn't too hard, but it does require a bit of restructuring of the code base. Look-up GPOPS-II if you're curious about multiple-phase problems.

Problem with z state in quadRotor3D

Hi,
Excuse my question but I have tried to run MAIN.m in the quadRotor3D demo and after analysing the result of the optimizer I found that the position z shows strange behavior at least for me.

Basically when I look at the velocity in z and at the position in z they dont make sense because the variation of z is a lot higher than its velocity. In the repository example, the z position goes from 0 to 25 m in less than a second but the velocity stays between -2 and 2 m/s.

Another problem I found is when my initial z position set in inicial state is different than zero, the initial z position of the solution is completly wrong. For example, I tried setting my initial z position to 3 m and after solving the initial z position of the solution was 170 m.

I have tried to analyse the system dynamics but nothing looks wrong to me so if you could help me with this I would be thankful to you.

Add support for other NLP solvers

There are many NLP solvers that can interface with Matlab, potentially allowing users to try to solve problems using a core solver other than FMINCON. Two possible interfaces are with Knitro and SNOPT, links below:

Knitro:
https://www.artelys.com/tools/knitro_doc/3_referenceManual/knitromatlabReference.html
https://www.artelys.com/tools/knitro_doc/1_introduction/installation.html

SNOPT
https://github.com/snopt/snopt-matlab
https://ccom.ucsd.edu/~optimizers/downloads/index.php