use_new_kinsol is a protected property instead of a constant, allowin…

…g comparison between new and old kinsols. zapped regression test .mat file.

systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m

@@ -32,6 +32,8 @@
     robot_state_frames;
     robot_position_frames;
     robot_velocity_frames;
+
+    use_new_kinsol = false;
   end
 
   properties (Access=public)  % i think these should be private, but probably needed to access them from mex? - Russ
@@ -42,10 +44,6 @@
     collision_filter_groups;  % map of CollisionFilterGroup objects
   end
 
-  properties (Constant)
-    use_new_kinsol = false;
-  end
-
   methods
     function obj = RigidBodyManipulator(filename,options)
       % Construct a new rigid body manipulator object with a single (empty)
@@ -65,6 +63,10 @@
       obj.collision_filter_groups = PassByValueMap('KeyType','char','ValueType','any');
       obj.collision_filter_groups('no_collision') = CollisionFilterGroup();
 
+      if isfield(options,'use_new_kinsol')
+        obj.use_new_kinsol = options.use_new_kinsol;
+      end
+
       if (nargin>0 && ~isempty(filename))
         [path,name,ext]=fileparts(filename);
         if strcmpi(ext,'.urdf')
@@ -126,6 +128,11 @@
   end
 
   methods
+    function obj = setNewKinsolFlag(obj,flag)
+      obj.use_new_kinsol = flag;
+      obj = compile(obj);
+    end
+
     function [Vq, dVq] = qdotToV(obj, q)
       compute_gradient = nargout > 1;
 

systems/plants/test/createAtlas.m

@@ -1,6 +1,9 @@
 %NOTEST
-function robot = createAtlas(floatingJointType)
+function robot = createAtlas(floatingJointType,options)
 options.floating = floatingJointType;
+if strcmp(floatingJointType,'quat')
+  options.use_new_kinsol = true;
+end
 w = warning('off','Drake:RigidBodyManipulator:UnsupportedVelocityLimits');
 warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints');
 robot = RigidBodyManipulator(fullfile('../../../examples/Atlas/urdf/atlas_minimal_contact.urdf'),options);

systems/plants/test/testManipulatorDynamics.m

@@ -1,25 +1,27 @@
 function testManipulatorDynamics
 testActuatedPendulum();
-regressionTestAtlasRPY();
+testAtlasRPY();
 checkGradients(createAtlas('rpy'));
 checkHdotMinus2CoriolisMatrixSkewSymmetricMatrix(createAtlas('rpy'))
 
-if RigidBodyManipulator.use_new_kinsol
-  testBrickQuaternion();
-  testAtlasQuat();
-  checkGradients(createFallingBrick('quat'));
-  checkGradients(createAtlas('quat'));
-end
+testBrickQuaternion();
+testAtlasQuat();
+checkGradients(createFallingBrick('quat'));
+checkGradients(createAtlas('quat'));
+
 end
 
 function robot = createFallingBrick(floating_type)
 options.floating = floating_type;
+options.use_new_kinsol = true;
 robot = RigidBodyManipulator('FallingBrick.urdf',options);
 end
 
 function testBrickQuaternion()
 options.floating = 'quat';
+options.use_new_kinsol = true;
 r = RigidBodyManipulator('FallingBrick.urdf',options);
+
 nv = r.getNumVelocities();
 q = getRandomConfiguration(r);
 v = randn(nv, 1);
@@ -58,44 +60,32 @@ function testActuatedPendulum()
 checkMex(m);
 end
 
-function regressionTestAtlasRPY()
-replaceMatFile = false;
-
-rng(23415, 'twister');
+function testAtlasRPY()
 
 r = createAtlas('rpy');
+r_newkinsol = setNewKinsolFlag(r,true);
 nq = r.getNumPositions();
 nv = r.getNumVelocities();
 
 nTests = 5;
-Hs = cell(nTests, 1);
-Cs = cell(nTests, 1);
-
 for i = 1 : nTests
   q = randn(nq, 1);
   v = randn(nv, 1);
   [H, C, B] = manipulatorDynamics(r, q, v, false);
-  Hs{i} = H;
-  Cs{i} = C;
-end
+  [H_new, C_new, B_new] = manipulatorDynamics(r_newkinsol, q, v, false);
 
-filename = 'regressionTestAtlasManipulatorDynamics.mat';
-if replaceMatFile
-  save(filename, varname(Hs), varname(Cs), varname(B));
-else
-  data = load(filename);
-  for i = 1 : nTests
-    valuecheck(data.Hs{i}, Hs{i}, 1e-10);
-    valuecheck(data.Cs{i}, Cs{i}, 1e-10);
-  end
-  valuecheck(data.B, B);
+  valuecheck(H_new,H);
+  valuecheck(C_new,C);
+  valuecheck(B_new,B);
 end
+
 checkMex(r);
 
 end
 
 function testAtlasQuat()
-r = createAtlas('quat');
+options.use_new_kinsol = true;
+r = createAtlas('quat',options);
 nv = r.getNumVelocities();
 checkMex(r);
 
@@ -105,7 +95,7 @@ function testAtlasQuat()
   v = randn(nv, 1);
   [H, C, B] = manipulatorDynamics(r, q, v, false);
   kinetic_energy = 1/2 * v' * H * v;
-  
+
   kinsol = r.doKinematics(q, false, false, v);
   kinetic_energy_via_kinsol = computeKineticEnergy(r, kinsol);
   valuecheck(kinetic_energy_via_kinsol, kinetic_energy, 1e-10);
@@ -141,7 +131,6 @@ function checkGradients(robot)
 end
 
 function checkMex(robot)
-rng(1, 'twister');
 q = getRandomConfiguration(robot);
 v = randn(robot.getNumVelocities(), 1);
 [H, C, B, dH, dC, dB] = manipulatorDynamics(robot, q, v, false);

systems/plants/test/testdHomogTrans.m

@@ -1,12 +1,13 @@
 function testdHomogTrans()
 testFallingBrick('rpy');
-if RigidBodyManipulator.use_new_kinsol
-  testFallingBrick('quat');
-end
+testFallingBrick('quat');
 end
 
 function testFallingBrick(floatingType)
 options.floating = floatingType;
+if strcmp(floatingType,'quat')
+  options.use_new_kinsol = true;
+end
 m = RigidBodyManipulator('FallingBrick.urdf',options);
 nq = m.getNumPositions();
 q = getRandomConfiguration(m);