atlas testKneeSingularity fails

[RussTedrake]

in my new pull request I check for nans, etc more aggressively, and caught a few tests that we're failing silently. This is one of them.

from: http://drake006.csail.mit.edu/drake/pull-RussTedrake-prius-into-RobotLocomotion-master_20160113-1210-Continuous

examples/Atlas/test/testKneeSingularity (unknown issue)

error_id = Simulink:SFunctions:SFcnErrorStatus
error_message = Error reported by S-function 'DCSFunction' in 'MIMOCascade_4945453/system1/DrakeSys':


DrakeSystem S-Function: error MATLAB:unexpectedCPPexception in MATLAB callback.
See additional debugging information above
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.

 Calling addpath_iris

ans =

     1

 Calling addpath_gurobi
 Calling addpath_snopt
[�Warning: No support surface provided. A horizontal plane at z = 0 will be
assumed]� 
[�> In RigidBodySupportState>RigidBodySupportState.RigidBodySupportState at 57
  In testKneeSingularity at 70
  In fevalPackageSafe at 21]� 
DrakeSystem S-Function: error when calling ''update'' with the following arguments:
  FeedbackSystem

  Properties:
               sys1: [1x1 Atlas]
               sys2: [1x1 bipedControllers.BipedPlanEvalAndControlSystem]
            sys1ind: [72x1 double]
            sys2ind: [0x1 double]
               umin: [30x1 double]
               umax: [30x1 double]
    warning_manager: [1x1 WarningManager]


    8.4370

   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN
   NaN

     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0
     0

Unexpected Standard exception from MEX file.
What() is:In BulletModel::findClosestPointsBtwElements: No closest point found between 140210305437888 and 140210306379168
..

Error in RigidBodyManipulator/collisionDetect (line 69)
  [xA,xB,normal,distance,idxA,idxB] = collisionDetectmex(obj.mex_model_ptr, kinsol.mex_ptr, allow_multiple_contacts,active_collision_options);

Error in RigidBodyManipulator/contactConstraints (line 48)
[phi,normal,xA,xB,idxA,idxB] = collisionDetect(obj,kinsol_no_gradients,allow_multiple_contacts,active_collision_options);

Error in TimeSteppingRigidBodyManipulator/solveMexLCP (line 265)
        [phiC,normal,d,xA,xB,idxA,idxB,mu,n,D] = obj.manip.contactConstraints(kinsol, obj.multiple_contacts);

Error in TimeSteppingRigidBodyManipulator/solveLCP (line 285)
        [obj,z,Mvn,wvn] = solveMexLCP(obj,t,x,u);

Error in TimeSteppingRigidBodyManipulator/update (line 200)
        [obj,z,Mvn,wvn] = solveLCP(obj,t,x,u);

Error in FeedbackSystem/update (line 67)
      if (obj.sys1.getNumDiscStates()) xdn=[xdn;update(obj.sys1,t,x1,sat1(obj,y2+u))]; end

Error in SimulinkModelHandle/sim (line 112)
      varargout{:} = sim(obj.name,varargin{:});

Error in DynamicalSystem/simulate (line 120)
simout = sim(mdl,pstruct);

Error in testKneeSingularity (line 92)
ytraj = simulate(sys, [0, T], x0, struct('gui_control_interface', true));

Error in fevalPackageSafe (line 21)
  feval(f);
Error using SimulinkModelHandle/sim (line 112)
Error reported by S-function 'DCSFunction' in 'MIMOCascade_4945453/system1/DrakeSys':


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

Error in DynamicalSystem/simulate (line 120)
simout = sim(mdl,pstruct);

Error in testKneeSingularity (line 92)
ytraj = simulate(sys, [0, T], x0, struct('gui_control_interface', true));

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


<test_name>examples/Atlas/test/testKneeSingularity</test_name> <error_id>Simulink:SFunctions:SFcnErrorStatus</error_id> <error_message>Error reported by S-function 'DCSFunction' in 'MIMOCascade_4945453/system1/DrakeSys':


DrakeSystem S-Function: error MATLAB:unexpectedCPPexception in MATLAB callback.
See additional debugging information above</error_message> 
matlab exit code: 1

<matlab_test>strcmp(test_name,'examples/Atlas/test/testKneeSingularity')</matlab_test>

[rdeits]

I don't think it's the check for NaNs that's causing this to fail. If the controller puts out nans, the robot will fall, and the test will detect that and fail. I can reproduce this occasionally, but I'm not sure if it's deterministic or not. It also appears to be happening in OH: openhumanoids/oh-distro#42

[rdeits]

I can't reproduce the NaNs on master under linux. But I can see a pretty nonphysical sliding that occurs during the test: https://www.dropbox.com/s/k8jw86k8ymak6st/2016-02-02-atlas_sim_sliding.mp4?dl=0

I believe that FastQP is the culprit. We've disabled it for almost every Atlas test, but not for this one. Disabling it fixes the test on master. See: #1679

[rdeits]

It's also possible that the NaNs are due to a bug which is no longer present in master. I'll try some older revisions to see if I can reproduce it.

[rdeits]

This is proving to be excruciatingly hard to debug because some commits of drake will not compile on gcc 4.7 due to #1650, some won't compile on gcc 4.8 due to #1604 and #1609 and some won't compile on gcc 4.9 due to #1663 :-(

[rdeits]

@RussTedrake I cannot reproduce this on master nor at 0adb56b which is the commit pointed to by oh-distro. Do you have a sha at which the failure occurs?

[rdeits]

This is happening again on drake006 in #1678

I still can't reproduce it anywhere but drake006, so I really don't know what could be happening.