RobotLocomotion / drake

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

frame error in runMixedIntegerSimpleForest #673

Closed RussTedrake closed 9 years ago

RussTedrake commented 9 years ago

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

Most recent commits to drake included in this build:

364335f Merge pull request #666 from RussTedrake/pulley
fa19104 add numerical test on length for the pulleys
afd7caf Merge branch 'pulley' of https://github.com/RussTedrake/drake into
pulley
01f7d1e renamed function CablesAndPulleys to CableLength.  Minor tweaks to
example
a02e099 Merge pull request #15 from kuindersma/fix-torque-calculation

examples/Quadrotor/runMixedIntegerSimpleForest (unknown issue)

error_id = 
error_message = frame dimension does not match number of inputs
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(47517574,'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.
To disable this warning in the future, add
  warning('off','Drake:DisablingSimulinkAutosave')
to your matlab startup.m] 
[> In addpath_drake at 74
 In addpath_drake at 4] 
Added the lcm jar to your javaclasspath (found via cmake)
[Warning: Currently all of your LCM traffic will be broadcast to the
network,
because:
 Loopback interface is not set to multicast.
 Consider running: sudo
 /tmp/drake-master-1644/drake/util/setup_loopback_multicast.sh
] 
[> In checkDependency at 63
 In checkDependency at 74
 In runMixedIntegerEnvironment at 16
 In runMixedIntegerSimpleForest at 21
 In fevalPackageSafe at 21] 
Added the lcmgl jar to your javaclasspath (found via cmake)
Calling addpath_iris
Calling addpath_mosek

MOSEK Version 7.1.0.12 (Build date: 2014-12-12 07:25:47)
Copyright (c) 1998-2014 MOSEK ApS, Denmark. WWW: http://mosek.com
Platform: Linux/64-X86

For help type: help mosekopt.
LCM: Disabling IPV6 support
LCM: TTL set to zero, traffic will not leave localhost.
Running mixed-integer convex program for 3rd-degree polynomial...
Calling addpath_yalmip

MOSEK Version 7.1.0.12 (Build date: 2014-12-12 07:25:47)
Copyright (c) 1998-2014 MOSEK ApS, Denmark. WWW: http://mosek.com
Platform: Linux/64-X86

Computer
 Platform               : Linux/64-X86    
 Cores                  : 4               

Problem
 Name                   :                 
 Objective sense        : min             
 Type                   : CONIC (conic optimization problem)
 Constraints            : 1695            
 Cones                  : 73              
 Scalar variables       : 507             
 Matrix variables       : 0               
 Integer variables      : 12              

Optimizer started.
Mixed integer optimizer started.
Optimizer  - threads                : 4
BRANCHES RELAXS   ACT_NDS  BEST_INT_OBJ         BEST_RELAX_OBJ      
REL_GAP(%)  TIME  
0        1        0        NA                   3.1239667234e-01     NA    
0.2   
0        1        1        NA                   3.1239667234e-01     NA    
0.3   
0        1        1        NA                   3.1239667234e-01     NA    
0.3   
0        1        1        NA                   3.1239667234e-01     NA    
0.4   
0        1        1        NA                   3.1239667234e-01     NA    
0.5   
0        1        1        NA                   3.1239667234e-01     NA    
0.6   
1        2        2        NA                   3.1239667234e-01     NA    
0.7   
3        4        2        NA                   3.1239667234e-01     NA    
0.9   
4        5        3        NA                   3.1239667234e-01     NA    
0.9   
6        7        5        NA                   3.1239667234e-01     NA    
1.0   
9        10       6        NA                   3.1239667234e-01     NA    
1.1   
11       12       4        NA                   3.1239667234e-01     NA    
1.1   
13       14       6        NA                   3.1249808193e-01     NA    
1.2   
17       18       6        NA                   3.1249808193e-01     NA    
1.3   
21       22       4        NA                   3.1249808193e-01     NA    
1.4   
23       24       6        NA                   1.5692746327e+00     NA    
1.4   
26       27       7        NA                   1.5692746327e+00     NA    
1.5   
28       29       5        NA                   1.5692746327e+00     NA    
1.5   
31       32       4        NA                   1.7922490312e+00     NA    
1.6   
33       34       2        1.9510255220e+00     1.7922490312e+00     8.14  
1.6   
34       35       3        1.9510255220e+00     1.7922490312e+00     8.14  
1.6   
37       38       0        1.7922496282e+00     1.7922496282e+00    
0.00e+00    1.7   
An optimal solution satisfying the relative gap tolerance of 1.00e+00(%)
has been located.
The relative gap is 0.00e+00(%).
An optimal solution satisfying the absolute gap tolerance of 0.00e+00 has
been located.
The absolute gap is 0.00e+00.

Objective of best integer solution : 1.792249628223e+00      
Best objective bound               : 1.792249628223e+00      
Construct solution objective       : Not employed
Construct solution # roundings     : 0
User objective cut value           : 0
Number of cuts generated           : 0
Number of branches                 : 37
Number of relaxations solved       : 38
Number of interior point iterations: 477
Number of simplex iterations       : 0
Time spend presolving the root     : 0.03
Time spend in the heuristic        : 0.00
Time spend in the sub optimizers   : 0.00
 Time spend optimizing the root   : 0.13
Mixed integer optimizer terminated. Time: 1.70

Optimizer terminated. Time: 1.70    

Integer solution solution summary
 Problem status  : PRIMAL_FEASIBLE
 Solution status : INTEGER_OPTIMAL
 Primal.  obj: 1.7922496282e+00    Viol.  con: 1e-07    var: 0e+00   
cones: 0e+00    itg: 0e+00  
Optimizer summary
 Optimizer                 -                        time: 1.70    
   Interior-point          - iterations : 0         time: 0.00    
     Basis identification  -                        time: 0.00    
       Primal              - iterations : 0         time: 0.00    
       Dual                - iterations : 0         time: 0.00    
       Clean primal        - iterations : 0         time: 0.00    
       Clean dual          - iterations : 0         time: 0.00    
       Clean primal-dual   - iterations : 0         time: 0.00    
   Simplex                 -                        time: 0.00    
     Primal simplex        - iterations : 0         time: 0.00    
     Dual simplex          - iterations : 0         time: 0.00    
     Primal-dual simplex   - iterations : 0         time: 0.00    
   Mixed integer           - relaxations: 38        time: 1.70    

Elapsed time is 6.392542 seconds.
done!
Running semidefinite program for 5th-degree polynomial...

MOSEK Version 7.1.0.12 (Build date: 2014-12-12 07:25:47)
Copyright (c) 1998-2014 MOSEK ApS, Denmark. WWW: http://mosek.com
Platform: Linux/64-X86

Computer
 Platform               : Linux/64-X86    
 Cores                  : 4               

Problem
 Name                   :                 
 Objective sense        : min             
 Type                   : CONIC (conic optimization problem)
 Constraints            : 865             
 Cones                  : 1               
 Scalar variables       : 665             
 Matrix variables       : 72              
 Integer variables      : 0               

Optimizer started.
Conic interior-point optimizer started.
Presolve started.
Linear dependency checker started.
Linear dependency checker terminated.
Eliminator started.
Total number of eliminations : 208
Eliminator terminated.
Eliminator started.
Total number of eliminations : 215
Eliminator terminated.
Eliminator - tries                  : 2                 time               
: 0.00            
Eliminator - elim's                 : 215             
Lin. dep.  - tries                  : 1                 time               
: 0.00            
Lin. dep.  - number                 : 0               
Presolve terminated. Time: 0.00    
Optimizer  - threads                : 4               
Optimizer  - solved problem         : the primal      
Optimizer  - Constraints            : 470
Optimizer  - Cones                  : 1
Optimizer  - Scalar variables       : 271               conic              
: 26              
Optimizer  - Semi-definite variables: 72                scalarized         
: 432             
Factor     - setup time             : 0.01              dense det. time    
: 0.00            
Factor     - ML order time          : 0.00              GP order time      
: 0.00            
Factor     - nonzeros before factor : 6474              after factor       
: 7801            
Factor     - dense dim.             : 1                 flops              
: 2.31e+05        
ITE PFEAS    DFEAS    GFEAS    PRSTATUS   POBJ              DOBJ           
MU       TIME  
0   1.0e+00  3.2e+01  2.0e+00  0.00e+00   1.000000000e+00   0.000000000e+00
1.0e+00  0.02  
1   5.5e-01  1.8e+01  1.1e+00  -3.11e-01  4.110106122e-01  
-1.266944552e-01  5.5e-01  0.04  
2   2.3e-01  7.5e+00  4.6e-01  1.50e-01   -5.070509956e-01 
-7.323647774e-01  2.3e-01  0.05  
3   7.2e-02  2.3e+00  1.4e-01  7.06e-01   -2.645180961e+00 
-2.740626582e+00  7.2e-02  0.07  
4   3.8e-02  1.2e+00  7.6e-02  1.02e+00   -3.401179171e+00 
-3.448961302e+00  3.8e-02  0.07  
5   2.4e-02  7.8e-01  4.8e-02  9.65e-01   -3.837627960e+00 
-3.865878891e+00  2.4e-02  0.08  
6   5.5e-03  1.8e-01  1.1e-02  1.08e+00   -4.478726555e+00 
-4.484821165e+00  5.5e-03  0.09  
7   1.1e-03  3.7e-02  2.3e-03  9.84e-01   -4.714361474e+00 
-4.715464589e+00  1.1e-03  0.11  
8   1.5e-04  4.9e-03  3.1e-04  1.01e+00   -4.761028914e+00 
-4.761171967e+00  1.5e-04  0.12  
9   8.0e-06  2.6e-04  1.6e-05  1.00e+00   -4.768387811e+00 
-4.768395214e+00  8.0e-06  0.12  
10  4.0e-07  1.3e-05  8.1e-07  1.00e+00   -4.768770618e+00 
-4.768770994e+00  4.0e-07  0.14  
11  1.3e-08  8.3e-08  3.2e-09  1.00e+00   -4.768790887e+00 
-4.768790886e+00  2.6e-09  0.14  
Interior-point optimizer terminated. Time: 0.14. 

Optimizer terminated. Time: 0.15    

Interior-point solution summary
 Problem status  : PRIMAL_AND_DUAL_FEASIBLE
 Solution status : OPTIMAL
 Primal.  obj: -4.7687908867e+00   Viol.  con: 2e-08    var: 0e+00   
barvar: 0e+00    cones: 0e+00  
 Dual.    obj: -4.7687908858e+00   Viol.  con: 0e+00    var: 1e-07   
barvar: 0e+00    cones: 0e+00  
Optimizer summary
 Optimizer                 -                        time: 0.15    
   Interior-point          - iterations : 11        time: 0.14    
     Basis identification  -                        time: 0.00    
       Primal              - iterations : 0         time: 0.00    
       Dual                - iterations : 0         time: 0.00    
       Clean primal        - iterations : 0         time: 0.00    
       Clean dual          - iterations : 0         time: 0.00    
       Clean primal-dual   - iterations : 0         time: 0.00    
   Simplex                 -                        time: 0.00    
     Primal simplex        - iterations : 0         time: 0.00    
     Dual simplex          - iterations : 0         time: 0.00    
     Primal-dual simplex   - iterations : 0         time: 0.00    
   Mixed integer           - relaxations: 0         time: 0.00    

Elapsed time is 0.986414 seconds.
done!
Inverting differentially flat system...
done!
Computing stabilizing controller with TVLQR...
[Warning: todo: still need to add state constriants for the feedback
system] 
[> In WarningManager>WarningManager.warnOnce at 21
 In FeedbackSystem>FeedbackSystem.FeedbackSystem at 56
 In DrakeSystem.DrakeSystem>DrakeSystem.feedback at 535
 In Manipulator.Manipulator>Manipulator.feedback at 331
 In runMixedIntegerEnvironment at 130
 In runMixedIntegerSimpleForest at 21
 In fevalPackageSafe at 21] 
done!
Simulating the system...
[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 488
 In runMixedIntegerEnvironment at 136
 In runMixedIntegerSimpleForest at 21
 In fevalPackageSafe at 21] 
Calling addpath_snopt
done!
Error using DynamicalSystem/setInputFrame (line 432)
frame dimension does not match number of inputs

Error in runMixedIntegerEnvironment (line 146)
v =
v.setInputFrame(sys.getOutputFrame().getFrameByName('quadrotorPosition'));

Error in runMixedIntegerSimpleForest (line 21)
runMixedIntegerEnvironment(r, start, goal, lb, ub, seeds, degree,
n_segments, n_regions);

Error in fevalPackageSafe (line 21)
 feval(f);

<test_name>examples/Quadrotor/runMixedIntegerSimpleForest</test_name>
<error_id></error_id> <error_message>frame dimension does not match number
of inputs</error_message> 
strcmp(error_message,'frame dimension does not match number of inputs') && strcmp(test_name,'examples/Quadrotor/runMixedIntegerSimpleForest')
rdeits commented 9 years ago

This does not occur for me on RobotLocomotion/drake master on my machine. I'll try to reproduce it on another machine.

rdeits commented 9 years ago

Both my mac and my DRC laptop run the example without encountering this error on drake/master.

rdeits commented 9 years ago

This also doesn't occur for me at 364335f which is the commit at which the attached error message was generated. I have no idea why it shows up on the build servers and not on my machines...

RussTedrake commented 9 years ago

i'll take a look on my machines.

On Jan 2, 2015, at 2:49 PM, Robin Deits notifications@github.com wrote:

This also doesn't occur for me at 364335f which is the commit at which the attached error message was generated. I have no idea why it shows up on the build servers and not on my machines...

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

RussTedrake commented 9 years ago

can you try running it after calling

global g_disable_visualizers; g_disable_visualizers=true;

On Jan 2, 2015, at 2:49 PM, Robin Deits notifications@github.com wrote:

This also doesn't occur for me at 364335f which is the commit at which the attached error message was generated. I have no idea why it shows up on the build servers and not on my machines...

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

RussTedrake commented 9 years ago

have you seen this one?

Simulating the system...
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 253
  In DynamicalSystem.simulate at 23
  In DrakeSystem.DrakeSystem>DrakeSystem.simulate at 490
  In runMixedIntegerEnvironment at 136
  In runMixedIntegerSimpleForest at 21 
 Calling addpath_snopt
DrakeSystem S-Function: error when calling ''getInitialState'' with the following arguments:
  FeedbackSystem with properties:

               sys1: [1x1 Quadrotor]
               sys2: [1x1 AffineSystem]
            sys1ind: [12x1 double]
            sys2ind: [0x1 double]
               umin: [4x1 double]
               umax: [4x1 double]
    warning_manager: [1x1 WarningManager]

An error occurred while trying to determine whether "permute" is a function name.

Error in cross (line 64)
    c = ipermute(c,perm);

Error in RigidBodyManipulator/surfaceTangents (line 1914)
      t2 = cross(t1,normal);

Error in RigidBodyManipulator/contactConstraints (line 60)
d = obj.surfaceTangents(normal);

Error in RigidBodyManipulator/compile/nonpenetrationConstraint (line 802)
          [phi,~,~,~,~,~,~,~,dphi] = contactConstraints(model,kinsol,false,model.contact_options);

Error in FunctionHandleConstraint/constraintEval (line 44)
      [varargout{:}] = obj.eval_handle(varargin{:});

Error in Constraint/eval (line 121)
          [varargout{:}] = obj.constraintEval(varargin{:});

Error in NonlinearProgram/objectiveAndNonlinearConstraints (line 568)
          [f(f_count+(1:obj.nlcon{i}.num_cnstr)),G(f_count+(1:obj.nlcon{i}.num_cnstr),obj.nlcon_xind_stacked{i})] = ...

Error in NonlinearProgram/snopt/snopt_userfun (line 1286)
        [f,G] = objectiveAndNonlinearConstraints(obj,x_all);

Error in snoptUserfun (line 6)
  [f,G] = SNOPT_USERFUN(x);

Error in NonlinearProgram/snopt (line 1349)
        [x_free,objval,exitflag,xmul,Fmul] = NonlinearProgramSnoptmex(x0_free, ...

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

Error in DynamicalSystem/resolveConstraints (line 325)
      [x,~,exitflag,infeasible_constraint_name] = solve(prog,x0);

Error in DrakeSystem/getInitialState (line 53)
            [x0,success] = resolveConstraints(obj,x0);

Error in FeedbackSystem/getInitialState (line 83)
      x0=encodeX(obj,getInitialState(obj.sys1),getInitialState(obj.sys2));

Error in getInitialState (line 27)

Error in sl (line 15)
[varargout{1:nargout}]=feval(varargin{:});

Error in Simulink.BlockDiagram.getInitialState (line 26)

Error in DynamicalSystem/stateVectorToStructure (line 663)
        obj.structured_x = Simulink.BlockDiagram.getInitialState(mdl);

Error in DynamicalSystem/simulate (line 54)
  x0 = stateVectorToStructure(obj,x0,mdl);

Error in DrakeSystem/simulate (line 490)
        [varargout{:}] = simulate@DynamicalSystem(obj,varargin{:});

Error in runMixedIntegerEnvironment (line 136)
xtraj_sim = simulate(sys,[0 tf],x0);

Error in runMixedIntegerSimpleForest (line 21)
runMixedIntegerEnvironment(r, start, goal, lb, ub, seeds, degree, n_segments, n_regions);
Error using getInitialState (line 27)
Error reported by S-function 'DCSFunction' in
'FeedbackSystem_063587519665577088/DrakeSys':

DrakeSystem S-Function: error MATLAB:err_while_looking_up_function in MATLAB
callback.
See additional debugging information above

Error in sl (line 15)
[varargout{1:nargout}]=feval(varargin{:});

Error in Simulink.BlockDiagram.getInitialState (line 26)

Error in DynamicalSystem/stateVectorToStructure (line 663)
        obj.structured_x = Simulink.BlockDiagram.getInitialState(mdl);

Error in DynamicalSystem/simulate (line 54)
  x0 = stateVectorToStructure(obj,x0,mdl);

Error in DrakeSystem/simulate (line 490)
        [varargout{:}] = simulate@DynamicalSystem(obj,varargin{:});

Error in runMixedIntegerEnvironment (line 136)
xtraj_sim = simulate(sys,[0 tf],x0);

Error in runMixedIntegerSimpleForest (line 21)
runMixedIntegerEnvironment(r, start, goal, lb, ub, seeds, degree, n_segments,
n_regions);

i'm having it regularly here (but not always??), but it's proving to be a real pain to debug.

On Jan 2, 2015, at 3:23 PM, Russ Tedrake russt@csail.mit.edu wrote:

can you try running it after calling

global g_disable_visualizers; g_disable_visualizers=true;

On Jan 2, 2015, at 2:49 PM, Robin Deits notifications@github.com wrote:

This also doesn't occur for me at 364335f which is the commit at which the attached error message was generated. I have no idea why it shows up on the build servers and not on my machines...

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

rdeits commented 9 years ago

Ew, no, I haven't seen that one before. Here's a guess: yalmip has an @ndsdpvar/permute.m which normally wouldn't be a problem, BUT, ndsdpvar has no classdef (it's not a full-fledged class, it seems), so perhaps that's confusing things? I've been using yalmip with tbxmanager, whereas you're using the yalmip pod, and perhaps those systems set the path in different ways? I'll keep looking...

rdeits commented 9 years ago

Okay, thanks for the visualizers tip: that's indeed the problem. The issue is that the NullVisualizer requires 12 inputs (6 positions and 6 velocities), but the BotVisualizer expects 6 inputs. Everything seems to work if I just pass the full 12 states into the visualizer.

RussTedrake commented 9 years ago

that sounds right. the state coordinate frame has a transform down to the position frame, which gets automatically inserted in the BotVisualizer case.

On Jan 2, 2015, at 11:14 PM, Robin Deits notifications@github.com wrote:

Okay, thanks for the visualizers tip: that's indeed the problem. The issue is that the NullVisualizer requires 12 inputs (6 positions and 6 velocities), but the BotVisualizer expects 6 inputs. Everything seems to work if I just pass the full 12 states into the visualizer.

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