matthewpeterkelly / optimtraj Goto Github PK
View Code? Open in Web Editor NEWA trajectory optimization library for Matlab
License: MIT License
A trajectory optimization library for Matlab
License: MIT License
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);
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?
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.
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.
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/
Is the Runge-Kutta integrator in the OptimTraj multiple shooting method vectorized for greater speed and efficiency?
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.
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!
I want to display the bound and path cost for a solution. The closest thing that I found was soln.info.objVal
.
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?
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.
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?
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.
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/
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
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.
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 ?
Is there a recommended way to cite OptimTraj in academic research? A BibTeX entry would be especially useful.
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?
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.
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?
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.
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?
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.
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!
How can I add a time-varying control bounds to my problem?
Such as the control bounds varies with the state of the system.
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
Make it clear to users how to install this library.
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.
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.
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);
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.
Can I have a constraint on the control input such that '||\mathbf{u}||<=u_{max}'?
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?
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.
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);
错误使用 dynamics
输出参数太多。
出错 MAIN_forceSquared>@(t,x,u)(dynamics(x,u))
出错 directCollocation>myCstGrad (line 627)
[f, fGradRaw] = dynFun(t,x,u);
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).
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.
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.
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.
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.
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.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.