implements loop constraints for 3D

implements gradients for loop constraints (four bar simulation is MUCH MUCH faster)
updated loop joint urdf documentation

fix clear command at the bottom of configure so that the checkDependency cache gets cleared.

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

configure.m

@@ -185,7 +185,7 @@
   end
 end
 
-clear shared/checkDependency;  % makes sure that the persistent variable in the dependency checker gets cleared
+clear util/checkDependency;  % makes sure that the persistent variable in the dependency checker gets cleared
 
 end
 

doc/urdf.tex

@@ -19,7 +19,7 @@ \chapter{Universal Robot Description Format (URDF)}
 \item Attributes
 \begin{itemize}
 \item \mcode{name} (required)
-\item \mcode{type} (required). Can be any of the same types as a \mcode{<joint>}. See the \mcode{<joint>} element documentation. 
+\item \mcode{type} (required). Currently must be 'continuous'.  The intent is to eventually support all of the same types as a \mcode{<joint>}. See the \mcode{<joint>} element documentation. 
 \end{itemize}
 \item Elements
 \begin{itemize}

systems/plants/@PlanarRigidBodyManipulator/PlanarRigidBodyManipulator.m

@@ -249,49 +249,6 @@
       v = PlanarRigidBodyVisualizer(obj);
     end
 
-    function phi = positionConstraints(obj,q)
-      % so far, only loop constraints are implemented
-      phi=loopConstraints(obj,q);
-    end
-
-    function phi = loopConstraints(obj,q)
-      % handle kinematic loops
-      % note: each loop adds two constraints
-      phi=[];
-      jsign = [obj.body(cellfun(@(a)~isempty(a),{obj.body.parent})).jsign]';
-      q = jsign.*q;
-
-      for i=1:length(obj.loop)
-        % for each loop, add the constraints on T1(q) and T2(q), // todo: finish this
-        % where
-        % T1 is the transformation from the least common ancestor to the
-        % constraint in link1 coordinates
-        % T2 is the transformation from the least common ancester to the
-        % constraint in link2 coordinates
-        loop=obj.loop(i);
-
-        T1 = loop.T1;
-        b=loop.body1;
-        while (b~=loop.least_common_ancestor)
-          TJ = Tjcalcp(b.jcode,q(b.dofnum));
-          T1 = b.Ttree*TJ*T1;
-          b = b.parent;
-        end
-        T2 = loop.T2;
-        b=loop.body2;
-        while (b~=loop.least_common_ancestor)
-          TJ = Tjcalcp(b.jcode,q(b.dofnum));
-          T2 = b.Ttree*TJ*T2;
-          b = b.parent;
-        end
-
-        if (loop.jcode==1)  % pin joint adds constraint that the transformations must match in position at the origin
-          phi = [phi; [1,0,0; 0,1,0]*(T1*[0;0;1] - T2*[0;0;1])];
-        else
-          error('not implemented yet');
-        end
-      end
-    end
   end
 
   methods (Access=protected)
@@ -430,16 +387,13 @@
       link1Node = node.getElementsByTagName('link1').item(0);
       link1 = findLink(model,char(link1Node.getAttribute('link')));
       loop.body1 = link1;
-      loop.T1 = loop.parseLink(link1Node,options);
+      loop.pt1 = loop.parseLink(link1Node,options);
 
       link2Node = node.getElementsByTagName('link2').item(0);
       link2 = findLink(model,char(link2Node.getAttribute('link')));
       loop.body2 = link2;
-      loop.T2 = loop.parseLink(link2Node,options);
+      loop.pt2 = loop.parseLink(link2Node,options);
 
-      %% find the lowest common ancestor
-      loop.least_common_ancestor = leastCommonAncestor(loop.body1,loop.body2);
-
       axis=[1;0;0];  % default according to URDF documentation
       axisnode = node.getElementsByTagName('axis').item(0);
       if ~isempty(axisnode)
@@ -451,26 +405,18 @@
 
       type = char(node.getAttribute('type'));
       switch (lower(type))
-        case {'revolute','continuous'}
+        case 'continuous'
           loop.jcode=1;          
-          if dot(axis,model.view_axis)<(1-1e-6)
+          if abs(dot(axis,model.view_axis))<(1-1e-6)
             axis
             model.view_axis
-            error('revolute joints must align with the viewing axis');
-            % note: i don't support negative angles here yet (via jsign),
-            % but could
+            error('i do not yet support continuous joints that do not align with the viewing axis');
+            % note: i could potentially support this (and the fixed joint 
+            % type) by creating a fixed joint here, then finding the least 
+            % common ancestor (or actually any non-fixed ancestor) and 
+            % turning that joint into a loop joint. 
           end
 
-        case 'prismatic'
-          if dot(axis,model.x_axis)>(1-1e-6)
-            loop.jcode=2;
-          elseif dot(axis,model.y_axis)>(1-1e-6)
-            loop.jcode=3;
-          else
-            error('axis must be aligned with x or z');
-            % note: i don't support negative angles here yet (via jsign),
-            % but could
-          end
         otherwise
           error(['joint type ',type,' not supported (yet?)']);
       end

systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m

@@ -279,10 +279,8 @@
       end
 
       if (length(model.loop)>0)
-        warning('Drake:RigidBodyManipulator:UnsupportedLoopJoint','haven''t reimplemented position and velocity constraints yet'); 
+        model = model.setNumPositionConstraints(3*length(model.loop));  % should be 5? for continous joints once they enforce the joint axis constraint.
       end
-%      obj = obj.setNumPositionConstraints(2*length(obj.model.loop)+size([obj.model.body.ground_contact],2));
-%      obj = obj.setNumVelocityConstraints(0);%size([obj.model.body.ground_contact],2));
 
       model.joint_limit_min = [model.body.joint_limit_min]';
       model.joint_limit_max = [model.body.joint_limit_max]';
@@ -482,6 +480,30 @@ function drawKinematicTree(model)
       % note: intentionally jump straight to second order system (skipping manipulator)... even though it's bad form
       [varargout{:}] = pdcontrol@SecondOrderSystem(sys,Kp,Kd,index);
     end    
+
+    function [phi,dphi,ddphi] = positionConstraints(obj,q)
+      % so far, only loop constraints are implemented
+      [phi,dphi,ddphi]=loopConstraints(obj,q);
+    end
+
+    function [phi,dphi,ddphi] = loopConstraints(obj,q)
+      % handle kinematic loops
+      phi=[];dphi=[];ddphi=[];
+
+      kinsol = doKinematics(obj,q,true);
+
+      for i=1:length(obj.loop)
+        % for each loop, add the constraints that the pt1 on body1 is in
+        % the same location as pt2 on body2
+
+        [pt1,J1,dJ1] = obj.forwardKin(kinsol,obj.loop(i).body1,obj.loop(i).pt1);
+        [pt2,J2,dJ2] = obj.forwardKin(kinsol,obj.loop(i).body2,obj.loop(i).pt2);
+
+        phi = [phi; pt1-pt2];
+        dphi = [dphi; J1-J2];
+        ddphi = [ddphi; dJ1-dJ2];
+      end
+    end    
 
   end
 
@@ -671,9 +693,39 @@ function drawKinematicTree(model)
 
 
     function model = parseLoopJoint(model,node,options)
-      error('not implemented yet for 3D');
-    end
-
+      loop = RigidBodyLoop();
+      loop.name = char(node.getAttribute('name'));
+      loop.name = regexprep(loop.name, '\.', '_', 'preservecase');
+
+      link1Node = node.getElementsByTagName('link1').item(0);
+      link1 = findLink(model,char(link1Node.getAttribute('link')));
+      loop.body1 = link1;
+      loop.pt1 = loop.parseLink(link1Node,options);
+
+      link2Node = node.getElementsByTagName('link2').item(0);
+      link2 = findLink(model,char(link2Node.getAttribute('link')));
+      loop.body2 = link2;
+      loop.pt2 = loop.parseLink(link2Node,options);
+
+      axis=[1;0;0];  % default according to URDF documentation
+      axisnode = node.getElementsByTagName('axis').item(0);
+      if ~isempty(axisnode)
+        if axisnode.hasAttribute('xyz')
+          axis = reshape(str2num(char(axisnode.getAttribute('xyz'))),3,1);
+          axis = axis/(norm(axis)+eps); % normalize
+        end
+      end
+
+      type = char(node.getAttribute('type'));
+      switch (lower(type))
+        case {'continuous'}
+          warning('3D loop joints do not properly enforce the joint axis constraint.  (they perform more like a ball joint).  See bug 1389');
+        otherwise
+          error(['joint type ',type,' not supported (yet?)']);
+      end
+
+      model.loop=[model.loop,loop];
+    end    
 
   end
 

systems/plants/PlanarRigidBodyLoop.m

@@ -6,7 +6,7 @@
 
   methods (Static)
 
-    function T = parseLink(node,options)
+    function pt = parseLink(node,options)
       xyz=zeros(3,1); rpy=zeros(3,1);
       origin = node.getElementsByTagName('origin').item(0); 
       if ~isempty(origin)
@@ -31,8 +31,7 @@
         rpyangle=0;
       end
 
-      xy = [options.x_axis'; options.y_axis']*xyz;
-      T = [rotmat(rpyangle),xy; 0,0,1];
+      pt = [options.x_axis'; options.y_axis']*xyz;
     end
 
   end

systems/plants/RigidBodyLoop.m

@@ -3,16 +3,15 @@
   properties
     name
     body1
-    T1
+    pt1
     body2
-    T2
-    least_common_ancestor
+    pt2
   end
 
   methods (Static)
 
 
-    function T = parseLink(node,options)
+    function pt = parseLink(node,options)
       xyz=zeros(3,1); rpy=zeros(3,1);
       origin = node.getElementsByTagName('origin').item(0); 
       if ~isempty(origin)
@@ -24,8 +23,7 @@
         end
       end
 
-      error('still need to update this for the 3D case');
-      T = [rotmat(theta),xyz;0,0,1];
+      pt = xyz;
     end
 
   end