Problem with initial guess P.x0

[kurosh-z]

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 ?

[MatthewPeterKelly]

I wouldn't be too worried about the shifting initial guess. You can see what was changed to figure out which variable was causing the trouble. This sometimes is triggered by equality constraints on the boundary states.

The badly conditioned matrix is going to cause trouble. See if you can track that down using the debugging tool and breakpoints. My guess is that there is some problem in your formulation, although if you track down an issue with OptimTraj definitely let me know.

Good luck!

[kurosh-z]

Thank you for your reply . That was a Problem with my Dyanamics function. I've also modeled inverted triple pendulum and it works really well. (let me know if you want to add these to your examples)
I have encounterd some other issues. Im gonna open a new topic for them.

screenshots of the results:

[MatthewPeterKelly]

Hi Kurosh,

Nice work with the triple pendulum. That would make for a great addition to the examples for OptimTraj. If you're up for it, please make a pull request that contains the triple pendulum code. We'll do a code review and then if all goes well I will merge it in and add you to the contributors list for the project.

Matt