search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.24k stars 1.25k forks source link

trajectory optimization fmincon error in testMassSpringDamperTrajOpt #487

Closed RussTedrake closed 9 years ago

RussTedrake commented 9 years ago

Site: drake002 Build Name: minimal-RobotLocomotion-master Build Server Directory: /tmp/drake-minimal-13467/drake CDash: http://kobol.csail.mit.edu/cd/index.php?project=Drake&date=20141014

Most recent commits to drake included in this build:

fc15ebe Merge pull request #482 from RussTedrake/traj_opt_notes
5ca23e1 Merge pull request #474 from rdeits/quadrotor-3d-demo
2b5d4ab take geval back out of direct traj opt methods
d6c2152 Merge pull request #476 from kuindersma/rm-financial-toolbox-dep
1dda5f0 revert output argument order change in solveTraj

systems/plants/test/testMassSpringDamperTrajOpt (unknown issue)

error_id = optim:barrier:DerivEqUndefAtX0
error_message = Nonlinear equality constraint derivatives at initial point
contain Inf, NaN, or complex values. Fmincon cannot continue.
is_segfault = 0

                           < M A T L A B (R) >
                 Copyright 1984-2014 The MathWorks, Inc.
                   R2014a (8.3.0.532) 64-bit (glnxa64)
                            February 11, 2014

To get started, type one of these: helpwin, helpdesk, or demo.
For product information, visit www.mathworks.com.

To reproduce this test use rng(1505729508,'twister')

[Warning: Disabling autosave for simulink blocks (to avoid generating a lot
of
*.mdl.autosave files in your directory.  If you aren't a regular Simulink
user
and don't want this disabled, comment out this section in addpath_drake] 
[> In addpath_drake at 74
 In addpath_drake at 4] 
Cannot find required pod snopt
[Warning: system has constraints, but they aren't enforced in the simulink
model
yet.] 
[> In WarningManager>WarningManager.warnOnce at 21
 In DrakeSystem.DrakeSystem>DrakeSystem.getModel at 251
 In DynamicalSystem.simulate at 23
 In DrakeSystem.DrakeSystem>DrakeSystem.simulate at 481
 In
DirectTrajectoryOptimization>DirectTrajectoryOptimization.getInitialVars at
215
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
175
 In testMassSpringDamperTrajOpt at 23] 
Error using barrier (line 37)
Nonlinear equality constraint derivatives at initial point contain Inf,
NaN, or complex values. Fmincon cannot continue.

Error in fmincon (line 818)
   [X,FVAL,EXITFLAG,OUTPUT,LAMBDA,GRAD,HESSIAN] =
barrier(funfcn,X,A,B,Aeq,Beq,l,u,confcn,options.HessFcn, ...

Error in NonlinearProgram/fmincon (line 1349)
     [x,objval,exitflag] = fmincon(@obj.objective,x0,full(obj.Ain),...

Error in NonlinearProgram/solve (line 924)
         [x,objval,exitflag,infeasible_constraint_name] = fmincon(obj,x0);

Error in DirectTrajectoryOptimization/solveTraj (line 176)
     [z,F,info,infeasible_constraint_name] = obj.solve(z0);

Error in testMassSpringDamperTrajOpt (line 23)
 [xtraj,utraj,~,~,info] = prog.solveTraj(tf0);

<test_name>systems/plants/test/testMassSpringDamperTrajOpt</test_name>
<error_id>optim:barrier:DerivEqUndefAtX0</error_id>
<error_message>Nonlinear equality constraint derivatives at initial point
contain Inf, NaN, or complex values. Fmincon cannot
continue.</error_message>
strcmp(test_name,'systems/plants/test/testMassSpringDamperTrajOpt') && (strcmp(error_id,'optim:barrier:DerivEqUndefAtX0') || strcmp(error_id,'MATLAB:svd:matrixWithNaNInf'))
RussTedrake commented 9 years ago

just got this one also:

Site: drake006 Build Name: lcm-RobotLocomotion-master Build Server Directory: /tmp/drake-lcm-28788/drake CDash: http://kobol.csail.mit.edu/cd/index.php?project=Drake&date=20141015

Most recent commits to drake included in this build:

37e238b Merge pull request #489 from hongkai-dai/fix-testIK
b017d80 more robust double pend visualizer
eaa225c change testIK tolerance on gaze constraint. delete testIKGaze
fc15ebe Merge pull request #482 from RussTedrake/traj_opt_notes
5ca23e1 Merge pull request #474 from rdeits/quadrotor-3d-demo

systems/plants/test/testMassSpringDamperTrajOpt (unknown issue)

error_id = MATLAB:svd:matrixWithNaNInf
error_message = Input to SVD must not contain NaN or Inf.
is_segfault = 0

                           < M A T L A B (R) >
                 Copyright 1984-2012 The MathWorks, Inc.
                   R2012b (8.0.0.783) 64-bit (glnxa64)
                             August 22, 2012

To get started, type one of these: helpwin, helpdesk, or demo.
For product information, visit www.mathworks.com.

To reproduce this test use rng(1512392754,'twister')

[Warning: Disabling autosave for simulink blocks (to avoid generating a lot
of
*.mdl.autosave files in your directory.  If you aren't a regular Simulink
user
and don't want this disabled, comment out this section in addpath_drake] 
[> In addpath_drake at 74
 In addpath_drake at 4] 
Cannot find required pod snopt
[Warning: system has constraints, but they aren't enforced in the simulink
model
yet.] 
[> In WarningManager>WarningManager.warnOnce at 21
 In DrakeSystem.DrakeSystem>DrakeSystem.getModel at 251
 In DynamicalSystem.simulate at 23
 In DrakeSystem.DrakeSystem>DrakeSystem.simulate at 481
 In
DirectTrajectoryOptimization>DirectTrajectoryOptimization.getInitialVars at
215
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
175
 In testMassSpringDamperTrajOpt at 23] 
[Warning: The default trust-region-reflective algorithm does not solve
problems
with the constraints you have specified. FMINCON will use the active-set
algorithm instead. For information on applicable algorithms, see Choosing
the
Algorithm in the documentation.] 
[> In fmincon at 500
 In NonlinearProgram>NonlinearProgram.fmincon at 1349
 In NonlinearProgram>NonlinearProgram.solve at 924
 In DynamicalSystem.DynamicalSystem>DynamicalSystem.resolveConstraints at
325
 In DrakeSystem.DrakeSystem>DrakeSystem.getInitialState at 53
 In CascadeSystem>CascadeSystem.getInitialState at 84
 In DynamicalSystem.simulate at 96
 In DrakeSystem.DrakeSystem>DrakeSystem.simulate at 481
 In
DirectTrajectoryOptimization>DirectTrajectoryOptimization.getInitialVars at
215
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
175
 In testMassSpringDamperTrajOpt at 23] 
[Warning: Your current settings will run a different algorithm
(interior-point)
in a future release.] 
[> In fmincon at 505
 In NonlinearProgram>NonlinearProgram.fmincon at 1349
 In NonlinearProgram>NonlinearProgram.solve at 924
 In DynamicalSystem.DynamicalSystem>DynamicalSystem.resolveConstraints at
325
 In DrakeSystem.DrakeSystem>DrakeSystem.getInitialState at 53
 In CascadeSystem>CascadeSystem.getInitialState at 84
 In DynamicalSystem.simulate at 96
 In DrakeSystem.DrakeSystem>DrakeSystem.simulate at 481
 In
DirectTrajectoryOptimization>DirectTrajectoryOptimization.getInitialVars at
215
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
175
 In testMassSpringDamperTrajOpt at 23] 
[Warning: The default trust-region-reflective algorithm does not solve
problems
with the constraints you have specified. FMINCON will use the active-set
algorithm instead. For information on applicable algorithms, see Choosing
the
Algorithm in the documentation.] 
[> In fmincon at 500
 In NonlinearProgram>NonlinearProgram.fmincon at 1349
 In NonlinearProgram>NonlinearProgram.solve at 924
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
176
 In testMassSpringDamperTrajOpt at 23] 
[Warning: Your current settings will run a different algorithm
(interior-point)
in a future release.] 
[> In fmincon at 505
 In NonlinearProgram>NonlinearProgram.fmincon at 1349
 In NonlinearProgram>NonlinearProgram.solve at 924
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
176
 In testMassSpringDamperTrajOpt at 23] 
[Warning: Matrix is singular, close to singular or badly scaled. Results
may be
inaccurate. RCOND = NaN.] 
[> In qpsub>eqnsolv at 953
 In qpsub at 157
 In nlconst at 619
 In fmincon at 837
 In NonlinearProgram>NonlinearProgram.fmincon at 1349
 In NonlinearProgram>NonlinearProgram.solve at 924
 In DirectTrajectoryOptimization>DirectTrajectoryOptimization.solveTraj at
176
 In testMassSpringDamperTrajOpt at 23] 
Error using svd
Input to SVD must not contain NaN or Inf.

Error in pinv (line 29)
  [U,S,V] = svd(A,0);

Error in qpsub (line 463)
                   projSD = pinv(projH)*(-Zgf);

Error in nlconst (line 619)
       [SD,lambda,exitflagqp,outputqp,howqp,ACTIND] ...

Error in fmincon (line 837)
   [X,FVAL,LAMBDA,EXITFLAG,OUTPUT,GRAD,HESSIAN]=...

Error in NonlinearProgram/fmincon (line 1349)
     [x,objval,exitflag] = fmincon(@obj.objective,x0,full(obj.Ain),...

Error in NonlinearProgram/solve (line 924)
         [x,objval,exitflag,infeasible_constraint_name] = fmincon(obj,x0);

Error in DirectTrajectoryOptimization/solveTraj (line 176)
     [z,F,info,infeasible_constraint_name] = obj.solve(z0);

Error in testMassSpringDamperTrajOpt (line 23)
 [xtraj,utraj,~,~,info] = prog.solveTraj(tf0);

<test_name>systems/plants/test/testMassSpringDamperTrajOpt</test_name>
<error_id>MATLAB:svd:matrixWithNaNInf</error_id> <error_message>Input to
SVD must not contain NaN or Inf.</error_message>
RussTedrake commented 9 years ago

quick consensus is that this is pretty deep in fmincon and the other solvers work (maybe not our fault?)

hongkai-dai commented 9 years ago

It is actually a fault in our code, that our dynamic computation gives nan results. When x(1) is zero, the dynamics gradient contains nan. The following code shows the error:

>>p = RigidBodyManipulator('MassSpringDamper.urdf');
>> [xdot,dxdot] = p.dynamics(0,[0;1],1)

xdot =

    1.0000
    1.0000

dxdot =

   (2,2)      NaN
   (1,3)        1
   (2,4)        1
>> [xdot,dxdot] = p.dynamics(0,[0.001;1],1)

xdot =

    1.0000
   -0.0100

dxdot =

   (2,2)     -10.0000
   (1,3)       1.0000
   (2,3)      -1.0000
   (2,4)       1.0000
RussTedrake commented 9 years ago

nice catch!

On Nov 1, 2014, at 2:21 AM, Hongkai Dai notifications@github.com wrote:

It is actually a fault in our code, that our dynamic computation gives nan results. When x(1) is zero, the dynamics gradient contains nan. The following code shows the error:

p = RigidBodyManipulator('MassSpringDamper.urdf'); [xdot,dxdot] = p.dynamics(0,[0;1],1)

xdot =

1.0000
1.0000

dxdot =

(2,2) NaN (1,3) 1 (2,4) 1

[xdot,dxdot] = p.dynamics(0,[0.001;1],1)

xdot =

1.0000

-0.0100

dxdot =

(2,2) -10.0000 (1,3) 1.0000 (2,3) -1.0000 (2,4) 1.0000

— Reply to this email directly or view it on GitHub.