From e9a3e6ed4903089d15cd54f00f15222503b9c446 Mon Sep 17 00:00:00 2001
From: Russ Tedrake <russt@mit.edu>
Date: Sat, 23 Aug 2014 13:02:13 -0400
Subject: [PATCH] working on replacing dynamical system constraint methods with
 the new constraint classes

---
 systems/@DrakeSystem/DrakeSystem.m            |  4 +--
 systems/SpotPolynomialSystem.m                |  1 +
 systems/plants/@Manipulator/Manipulator.m     | 15 +++++-----
 .../plants/TimeSteppingRigidBodyManipulator.m | 30 ++++++++-----------
 4 files changed, 23 insertions(+), 27 deletions(-)

diff --git a/systems/@DrakeSystem/DrakeSystem.m b/systems/@DrakeSystem/DrakeSystem.m
index 83fb4a300562..d9df5116ae1d 100644
--- a/systems/@DrakeSystem/DrakeSystem.m
+++ b/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
diff --git a/systems/SpotPolynomialSystem.m b/systems/SpotPolynomialSystem.m
index 75280926aebc..f8afd682e413 100644
--- a/systems/SpotPolynomialSystem.m
+++ b/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)
diff --git a/systems/plants/@Manipulator/Manipulator.m b/systems/plants/@Manipulator/Manipulator.m
index 24c864c66140..53a1c9ebe889 100644
--- a/systems/plants/@Manipulator/Manipulator.m
+++ b/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)
diff --git a/systems/plants/TimeSteppingRigidBodyManipulator.m b/systems/plants/TimeSteppingRigidBodyManipulator.m
index f621fbfad3ce..454d2f521de0 100644
--- a/systems/plants/TimeSteppingRigidBodyManipulator.m
+++ b/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