Merge pull request #5 from mposa/collision-dynamics-integration

Minor bug fixes, worked on fallingBrickLCP example.

systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m

@@ -1176,7 +1176,12 @@ function applyTransform(lcmgl,T)
       % handle the normal = [0;0;1] case
       ind=(1-normal(3,:))<10e-8;  % since it's a unit normal, i can just check the z component
       t1(:,ind) = [ones(1,sum(ind)); zeros(2,sum(ind))];
-      ind=~ind;
+
+      % handle the normal = [0;0;-1] case
+      indneg=(1+normal(3,:))<10e-8;  % since it's a unit normal, i can just check the z component
+      t1(:,indneg) = [-ones(1,sum(indneg)); zeros(2,sum(indneg))];
+
+      ind=~(ind | indneg);
 
       % now the general case
       t1(:,ind) = [normal(2,ind);-normal(1,ind);zeros(1,sum(ind))]; % cross(normal,[0;0;1]) normalized

systems/plants/@RigidBodyManipulator/contactPositions.m

@@ -1,63 +1,49 @@
-function [contact_pos,J,dJ] = contactPositions(obj,kinsol,body_idx,body_contacts)
-error('The contactPositions function has been deprecated. See contactConstraints and collisionDetect')
-% % @param body_idx is an array of body indexes
-% % @param 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
-% % @ingroup Collision
-%       
-% if ~isstruct(kinsol)  
-%   % treat input as contactPositions(obj,q)
-%   kinsol = doKinematics(obj,kinsol,nargout>2);
-% end
+function [contact_pos,J,dJ, body_idx] = contactPositions(obj,kinsol,varargin)
+% function [contact_pos,J,dJ] = contactPositions(obj,kinsol,allow_multiple_contacts,active_collision_options)
+% Compute the contact positions and Jacobians, if interested in the
+% inertial frame positions of these points. Most applications should see
+% the contactConstraints function instead.
+%
+% @param obj
+% @param kinsol
+% @param allow_multiple_contacts Allow multiple contacts per body pair.
+%      Optional, defaults to false.
+% @param active_collision_options A optional struct to determine which
+%    bodies and collision groups are checked. See collisionDetect.
 % 
-% if nargin<3
-%   body_idx = 1:length(obj.body);
-% end
-% 
-% if nargin<4
-%   n_contact_pts = size([obj.body(body_idx).contact_pts],2);
-%   body_contacts = [];
-% else
-%   if isa(body_contacts,'cell')
-%     n_contact_pts = sum(cellfun('length',body_contacts));
-%   else
-%     n_contact_pts = length(body_contacts);
-%     body_contacts = {body_contacts};
-%   end
-% end
-% 
-% d=3; %obj.dim;  % 2 for planar, 3 for 3D
-% contact_pos = zeros(d,n_contact_pts)*kinsol.q(1);
-% 
-% if (nargout>1) 
-%   J = zeros(n_contact_pts*d,obj.num_q)*kinsol.q(1); 
-% end
-% if (nargout>2)
-%   dJ = sparse(n_contact_pts*d,obj.num_q^2);
-% end
-% 
-% count=0;
-% for i=1:length(body_idx)
-%   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(:,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(:,pts_idx));
-%     else
-%       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
-% end
-% 
-% end
+% @retval contact_pos (3 x 2m) is the set of inertial positions of the
+% contacts (nearest point on a body to another body)
+% @retval J (6m x n) = d/dq contact_pos
+% @retval dJ (6m x n^2) = dd/dqdq contact_pos
+% @retval body_idx (2m x 1) body indices
+
+if ~isstruct(kinsol)  
+  % treat input as contactPositions(obj,q)
+  kinsol = doKinematics(obj,kinsol);
+end
+
+[~,~,xA,xB,idxA,idxB] = obj.collisionDetect(kinsol,varargin{:});
+
+nq = obj.getNumPositions;
+nC = length(idxA);
+body_idx = [idxA;idxB];
+x_body = [xA xB];
+contact_pos = zeros(3,2*nC);
+
+if nargout > 1,
+  J = zeros(6*nC,nq);
+end
+if nargout > 2,
+  dJ = zeros(6*nC,nq^2);
+end
+
+for i=1:2*nC,
+  if (nargout>2)
+    [contact_pos(:,i),J((1:3) + 3*(i-1),:),dJ((1:3) + 3*(i-1),:)] = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
+  elseif (nargout>1)
+    [contact_pos(:,i),J((1:3) + 3*(i-1),:)] = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
+  else
+    contact_pos(:,i) = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
+  end
+end
+end

systems/plants/@RigidBodyManipulator/contactPositionsJdot.m

@@ -1,52 +1,50 @@
-function [contact_pos,J,Jdot] = contactPositionsJdot(obj,kinsol,body_idx,body_contacts)
-% @retval p(:,i)=[px;py;pz] is the position of the ith contact point 
-% @retval J is dpdq
-% @retval Jdot is d(dpdq)dt
-% @ingroup Collision
-
-typecheck(kinsol,'struct');
+function [contact_pos,J,Jdot] = contactPositionsJdot(obj,kinsol,varargin)
+% function [contact_pos,J,dJ] = contactPositionsJdot(obj,kinsol,body_idx,body_contacts)
+% Compute the contact positions and Jacobians, if interested in the
+% inertial frame positions of these points. Most applications should see
+% the contactConstraints function instead.
+%
+% @param obj
+% @param kinsol
+% @param allow_multiple_contacts Allow multiple contacts per body pair.
+%      Optional, defaults to false.
+% @param active_collision_options A optional struct to determine which
+%    bodies and collision groups are checked. See collisionDetect.
+% 
+% @retval contact_pos (3 x 2m) is the set of inertial positions of the
+% contacts (nearest point on a body to another body)
+% @retval J (6m x n) = d/dq contact_pos
+% @retval dJ (6m x n^2) = dd/dqdq contact_pos
+% @retval body_idx (2m x 1) body indices
 
-if nargin<3
-  body_idx = 1:length(obj.body);
+if ~isstruct(kinsol)  
+  % treat input as contactPositions(obj,q)
+  kinsol = doKinematics(obj,kinsol);
 end
 
-if nargin<4
-  n_contact_pts = size([obj.body(body_idx).contact_pts],2);
-  body_contacts = [];
-else
-  n_contact_pts = sum(cellfun('length',body_contacts));
-end
+[~,~,xA,xB,idxA,idxB] = obj.collisionDetect(kinsol,varargin{:});
 
-d=length(obj.gravity);  % 2 for planar, 3 for 3D
-contact_pos = zeros(d,n_contact_pts)*kinsol.q(1);
+nq = obj.getNumPositions;
+nC = length(idxA);
+body_idx = [idxA;idxB];
+x_body = [xA xB];
+contact_pos = zeros(3,2*nC);
 
-if (nargout>1) 
-  J = zeros(n_contact_pts*d,obj.num_q); 
+if nargout > 1,
+  J = zeros(6*nC,nq);
 end
-if (nargout>2)
-  Jdot = zeros(n_contact_pts*d,obj.num_q); 
+if nargout > 2,
+  Jdot = zeros(6*nC,nq);
 end
 
-count=0;
-for i=1:length(body_idx)
-  if isempty(body_contacts)
-    nC = size(obj.body(body_idx(i)).contact_pts,2);
-    pts_idx = 1:nC;
+for i=1:2*nC,
+  if (nargout>2)
+    [contact_pos(:,i),J((1:3) + 3*(i-1),:)] = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
+    Jdot((1:3) + 3*(i-1),:) = forwardJacDot(obj,kinsol,body_idx(i),x_body(:,i));
+  elseif (nargout>1)
+    [contact_pos(:,i),J((1:3) + 3*(i-1),:)] = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
   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),:)] = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts(:,pts_idx));
-      Jdot(d*count+(1:d*nC),:) = forwardJacDot(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(:,pts_idx));
-    else
-      contact_pos(:,count+(1:nC)) = forwardKin(obj,kinsol,body_idx(i),obj.body(body_idx(i)).contact_pts(:,pts_idx));
-    end
-    count = count + nC;
+    contact_pos(:,i) = obj.forwardKin(kinsol,body_idx(i),x_body(:,i));
   end
 end
-
-end
+end

systems/plants/TimeSteppingRigidBodyManipulator.m

@@ -485,7 +485,7 @@ function checkDirty(obj)
           if any(active)
             z(active) = pathlcp(M(active,active),w(active));
           end
-          
+
           inactive = ~active(1:(nL+nP+nC));  % only worry about the constraints that really matter.
           missed = (M(inactive,inactive)*z(inactive)+w(inactive) < 0);
           if ~any(missed), break; end
@@ -847,7 +847,11 @@ function checkDirty(obj)
     end
 
     function num_c = getNumContacts(obj)
-      num_c = obj.manip.num_contacts;
+      error('getNumContacts is no longer supported, in anticipation of alowing multiple contacts per body pair. Use getNumContactPairs for cases where the number of contacts is fixed');
+    end
+
+    function n=getNumContactPairs(obj)
+      n = obj.manip.getNumContactPairs;
     end
 
     function c = getBodyContacts(obj,body_idx)

systems/plants/test/ball.urdf

@@ -9,6 +9,12 @@
       </geometry>
       <origin xyz="0 0 0"/>
     </visual>
+    <visual>
+      <geometry>
+        <sphere radius="0.02"/>
+      </geometry>
+      <origin xyz="0 .1 0"/>
+    </visual>
     <collision>
       <geometry>
         <sphere radius="0.1"/>