fixed bad commit. let's try again

goretkin · Oct 5, 2014 · 2d1668b · 2d1668b
1 parent f4e9fdc
commit 2d1668b
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 17 deletions.
diff --git a/solvers/NullConstraint.m b/solvers/NullConstraint.m
@@ -2,12 +2,16 @@
 % this constraint does nothing, but having it is easier that handling the
 % possibility of empty matrices in lists of constraints
 
-  function obj = NullConstraint(xdim)
-    obj = obj@Constraint([],[],xdim,inf);
+  methods
+    function obj = NullConstraint(xdim)
+      obj = obj@Constraint([],[],xdim,inf);
+    end
   end
-
-  function varargout = constraintEval(obj,x)
-    varargout = cell(1,nargout);
-    % outputs are intentionally left empty
+
+  methods (Access = protected)
+    function varargout = constraintEval(obj,varargin)
+      varargout = cell(1,nargout);
+      % outputs are intentionally left empty
+    end
   end
-end
+end
diff --git a/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m b/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m
@@ -745,14 +745,14 @@
       if (model.num_contact_pairs>0)
         nonpenetration_constraint = FunctionHandleConstraint(zeros(model.num_contact_pairs,1),inf(model.num_contact_pairs,1),model.getNumPositions,@nonpenetrationConstraint,2);
         nonpenetration_constraint = nonpenetration_constraint.setName(cellstr(num2str([idxA;idxB]','non-penetration: body %d <-> body %d')));
-      else
-        nonpenetration_constraint = NullConstraint(model.getNumPositions);
-      end
-      if isempty(model.contact_constraint_id)
-        [model,id] = addStateConstraint(model,nonpenetration_constraint,1:model.getNumPositions);
-        model.contact_constraint_id = id;
-      else
-        model = updateStateConstraint(model,model.contact_constraint_id,nonpenetration_constraint,1:model.getNumPositions);
+        if isempty(model.contact_constraint_id)
+          [model,id] = addStateConstraint(model,nonpenetration_constraint,1:model.getNumPositions);
+          model.contact_constraint_id = id;
+        else
+          model = updateStateConstraint(model,model.contact_constraint_id,nonpenetration_constraint,1:model.getNumPositions);
+        end
+      elseif ~isempty(model.contact_constraint_id)
+        model = updateStateConstraint(model,model.contact_constraint_id,NullConstraint(model.getNumPositions),1:model.getNumPositions);
       end
 
       for j=1:length(model.loop)

diff --git a/systems/plants/test/contactSensorTest.m b/systems/plants/test/contactSensorTest.m
@@ -16,8 +16,8 @@
 
 [ytraj,xtraj] = simulate(p,[0 T]);
 
-v = p.constructVisualizer();
-v.playback(ytraj,struct('slider',true));
+%v = p.constructVisualizer();
+%v.playback(ytraj,struct('slider',true));
 
 % should find initial conditions for the brick which are resting on the
 % ground.