working on replacing dynamical system constraint methods with the new…

… constraint classes

systems/@DrakeSystem/DrakeSystem.m

@@ -362,8 +362,8 @@
       obj.num_xcon_eq = obj.num_xcon_eq + sum(con.lb == con.ub);
       obj.num_xcon_ineq = obj.num_xcon_ineq + sum(con.lb ~= con.ub);
       id = numel(obj.state_constraints);
-    end
-    
+  end
+
     function obj = updateStateConstraint(obj,id,con)
       % @param id is the identifier returned from addStateConstraint
       % @param con is a constraint object

systems/SpotPolynomialSystem.m

@@ -226,6 +226,7 @@
         end
       end
       obj.p_state_constraints = p_state_constraints;
+      error('todo: need to reset the drake.state_constraints with this new constraint');
     end
 
     function p_state_constraints = getPolyStateConstraints(obj)

systems/plants/@Manipulator/Manipulator.m

@@ -158,7 +158,7 @@
       % which can be enforced directly in the manipulator dynamics.
       % This method will also register phi (and it's time derivative)
       % as state constraints for the dynamical system.
-      
+
       typecheck(con,'Constraint');
       assert(con.xdim == obj.num_positions,'DrakeSystem:InvalidPositionConstraint','Position constraints must take a vector that is the same size as the number of positions of this system as an input');
       assert(all(con.lb == con.ub));
@@ -169,7 +169,7 @@
 
       obj.warning_manager.warnOnce('Drake:Manipulator:Todo','still need to add time derivatives of position constraints');
     end
-    
+
     function obj = addVelocityEqualityConstraint(obj,con)
       % Adds a velocity constraint of the form psi(q,v) = constant
       % (with dpsidv~=0) which can be enforced directly in the manipulator dynamics.
@@ -220,7 +220,7 @@
     function n = getNumVelocities(obj);
       n = obj.num_velocities;
     end
-    
+
     function varargout = positionConstraints(obj,q)
       % Implements position constraints of the form phi(q) = 0
 
@@ -231,7 +231,7 @@
         v{1} = v{1} - obj.state_constraints{obj.position_constraint_ids(i)}.lb;  % center it around 0
         for j=1:nargout
           varargout{j} = vertcat(varargout{j},v{j});
-        end
+    end
       end
     end
 
@@ -249,7 +249,7 @@
           varargout{j} = vertcat(varargout{j},v{j});
         end
       end
-    end
+        end
 
     function n = getNumJointLimitConstraints(obj)
       % returns number of constraints imposed by finite joint limits
@@ -451,9 +451,8 @@ function num_contacts(obj)
         [obj,obj.joint_limit_constraint_id] = addStateConstraint(obj,con);
       else
         obj = updateStateConstraint(obj,obj.joint_limit_constraint_id,con);
-      end
-    end
-
+  end  
+
   end  
 
   properties (SetAccess = private, GetAccess = public)

systems/plants/TimeSteppingRigidBodyManipulator.m

@@ -568,20 +568,20 @@ function checkDirty(obj)
 
     function varargout = pdcontrol(sys,Kp,Kd,index)
       if nargin<4, index=[]; end
-        [pdff,pdfb] = pdcontrol(sys.manip,Kp,Kd,index);
+      [pdff,pdfb] = pdcontrol(sys.manip,Kp,Kd,index);
       pdfb = setInputFrame(pdfb,sys.manip.getStateFrame());
-        pdfb = setOutputFrame(pdfb,sys.getInputFrame());
-        pdff = setOutputFrame(pdff,sys.getInputFrame());
-        if nargout>1
-          varargout{1} = pdff;
-          varargout{2} = pdfb;
-        else
-          % note: design the PD controller with the (non time-stepping
-          % manipulator), but build the closed loop system with the
-          % time-stepping manipulator:
-          varargout{1} = cascade(pdff,feedback(sys,pdfb));
-        end
+      pdfb = setOutputFrame(pdfb,sys.getInputFrame());
+      pdff = setOutputFrame(pdff,sys.getInputFrame());
+      if nargout>1
+        varargout{1} = pdff;
+        varargout{2} = pdfb;
+      else
+        % note: design the PD controller with the (non time-stepping
+        % manipulator), but build the closed loop system with the
+        % time-stepping manipulator:
+        varargout{1} = cascade(pdff,feedback(sys,pdfb));
       end
+    end
 
   end
 
@@ -715,7 +715,7 @@ function checkDirty(obj)
       [obj.manip,manip_id] = obj.manip.addStateConstraint(obj,con);
       assert(id==manip_id);
     end
-    
+
     function obj = updateStateConstraint(obj,id,con)
       obj = updateStateConstraint@DrakeSystem(obj,id,con);
       obj.manip = updateStateConstraint(obj.manip,id,con);
@@ -944,10 +944,6 @@ function checkDirty(obj)
       model.manip = setParams(model.manip,p);
     end
 
-    function [lb,ub] = getStateLimits(obj)
-      [lb,ub] = obj.manip.getStateLimits();
-    end
-
     function terrain_contact_point_struct = getTerrainContactPoints(obj,varargin)
       terrain_contact_point_struct = getTerrainContactPoints(obj.manip,varargin{:});
     end