Merge pull request RobotLocomotion#1257 from tkoolen/tk-fix-testRunni…

…ngPlanner Fix testRunningPlanner
aidiyang · Aug 16, 2015 · 02cd9de · 02cd9de
2 parents 76293b8 + d661db0
commit 02cd9de
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 14 deletions.
diff --git a/drake/examples/Atlas/test/testRunningPlanner.m b/drake/examples/Atlas/test/testRunningPlanner.m
@@ -427,9 +427,13 @@
   % Solve trajectory optimization
   tic
   %profile on;
-  [x_sol,~,~] = cdfkp.solve(x_seed);
+  [x_sol,~,exitflag] = cdfkp.solve(x_seed);
   %profile off;
   toc
+
+  if exitflag > 10
+    error(['Trajectory optimization failed. exitflag = ' num2str(exitflag)]);
+  end
 
   % Parse trajectory optimization output
   sol.x_sol = x_sol;

diff --git a/drake/solvers/trajectoryOptimization/SimpleDynamicsFullKinematicsPlanner.m b/drake/solvers/trajectoryOptimization/SimpleDynamicsFullKinematicsPlanner.m
@@ -114,6 +114,7 @@
 
     function data = kinematicsData(obj,q)
       options.compute_gradients = true;
+      options.use_mex = false;
       data = doKinematics(obj.robot,q,[],options);
     end
 

diff --git a/drake/systems/plants/@RigidBodyManipulator/centroidalMomentumMatrix.m b/drake/systems/plants/@RigidBodyManipulator/centroidalMomentumMatrix.m
@@ -19,23 +19,10 @@
 
 compute_gradients = nargout > 1;
 
-compute_kinematics = false;
 if ~isstruct(kinsol)  
   % treat input as centroidalMomentumMatrix(model,q)
   q = kinsol;
-  compute_kinematics = true;
-else
-  if ~isfield(kinsol, 'v')
-    kinsol_options.use_mex = kinsol.mex;
-    robot.warning_manager.warnOnce('Drake:centroidalMomentumMatrix:OldKinsol','You called centroidalMomentumMatrix with a kinsol in the old format. Redoing kinematics in new format using q from old kinsol');
-    q = kinsol.q;
-    compute_kinematics = true;
-  end
-end
-
-if compute_kinematics
   kinsol_options.compute_gradients = compute_gradients;
-  kinsol_options.force_new_kinsol = true;
   kinsol = robot.doKinematics(q, [], kinsol_options);
 end