contactConstraints and contactPositions optionally take contact point…

… indexes as arguments

git-svn-id: https://svn.csail.mit.edu/locomotion/robotlib/trunk@6163 c9849af7-e679-4ec6-a44e-fc146a885bd3

systems/plants/@RigidBodyManipulator/contactConstraints.m

@@ -1,4 +1,4 @@
-function [phi,n,D,mu,dn,dD] = contactConstraints(obj,kinsol,body_idx)
+function [phi,n,D,mu,dn,dD] = contactConstraints(obj,kinsol,body_idx,body_contacts)
 %
 % @retval phi  phi(i,1) is the signed distance from the contact
 % point on the robot to the closes object in the world.
@@ -10,13 +10,18 @@
 % @retval mu mu(i,1) is the coefficient of friction for the ith contact
 
 if nargin<3, body_idx = 1:length(obj.body); end
+if nargin<4
+  varargin = {kinsol,body_idx};
+else
+  varargin = {kinsol,body_idx,body_contacts};
+end
 
 if (nargout>4)
-  [contact_pos,J,dJ] = contactPositions(obj,kinsol,body_idx);
+  [contact_pos,J,dJ] = contactPositions(obj,varargin{:});
 elseif (nargout>1)
-  [contact_pos,J] = contactPositions(obj,kinsol,body_idx);
+  [contact_pos,J] = contactPositions(obj,varargin{:});
 else
-  contact_pos = contactPositions(obj,kinsol,body_idx);
+  contact_pos = contactPositions(obj,varargin{:});
 end
 
 %      axis equal;

systems/plants/@RigidBodyManipulator/contactPositions.m

@@ -1,4 +1,7 @@
-function [contact_pos,J,dJ] = contactPositions(obj,kinsol,body_idx)
+function [contact_pos,J,dJ] = contactPositions(obj,kinsol,body_idx,body_contacts)
+% body_idx is an array of body indexes
+% body_contacts is a cell array of vectors containing contact point indices
+% 
 % @retval p(:,i)=[px;py;pz] is the position of the ith contact point 
 % @retval J is dpdq
 % @retval dJ is ddpdqdq
@@ -12,25 +15,39 @@
   body_idx = 1:length(obj.body);
 end
 
+if nargin<4
+  n_contact_pts = size([obj.body(body_idx).contact_pts],2);
+  body_contacts = [];
+else
+  n_contact_pts = length(body_idx)*vertcat(body_contacts{:});
+end
+
 d=length(obj.gravity);  % 2 for planar, 3 for 3D
-contact_pos = zeros(d,size([obj.body(body_idx).contact_pts],2))*kinsol.q(1);  
+contact_pos = zeros(d,n_contact_pts)*kinsol.q(1);
+
 if (nargout>1) 
-  J = zeros(size([obj.body(body_idx).contact_pts],2)*d,obj.num_q); 
+  J = zeros(n_contact_pts*d,obj.num_q); 
 end
 if (nargout>2)
-  dJ = sparse(size([obj.body(body_idx).contact_pts],2)*d,obj.num_q^2);
+  dJ = sparse(n_contact_pts*d,obj.num_q^2);
 end
 
 count=0;
 for i=1:length(body_idx)
-  nC = size(obj.body(body_idx(i)).contact_pts,2);
+  if isempty(body_contacts)
+    nC = size(obj.body(body_idx(i)).contact_pts,2);
+    pts_idx = 1:nC;
+  else
+    pts_idx = body_contacts{i};
+    nC = length(pts_idx);
+  end
   if nC>0
     if (nargout>2)
-      [contact_pos(:,count+(1:nC)),J(d*count+(1:d*nC),:),dJ(d*count+(1:d*nC),:)] = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts);
+      [contact_pos(:,count+(1:nC)),J(d*count+(1:d*nC),:),dJ(d*count+(1:d*nC),:)] = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts(:,pts_idx));
     elseif (nargout>1)
-      [contact_pos(:,count+(1:nC)),J(d*count+(1:d*nC),:)] = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts);
+      [contact_pos(:,count+(1:nC)),J(d*count+(1:d*nC),:)] = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts(:,pts_idx));
     else
-      contact_pos(:,count+(1:nC)) = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts);
+      contact_pos(:,count+(1:nC)) = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts(:,pts_idx));
     end
     count = count + nC;
   end

util/angleDiff.m

@@ -10,11 +10,5 @@
 phi1 = mod(phi1, 2*pi);
 
 d = phi2 - phi1;
-d(d > pi) = -(2*pi - d(d > pi));
-d(d < -pi) = (2*pi + d(d < -pi));
-% if d > pi
-%   d = -(2*pi - d);
-% end
-% if d < -pi
-%   d = 2*pi + d;
-% end
+d(d > pi) = d(d > pi) -2*pi;
+d(d < -pi) = d(d < -pi) + 2*pi;