ported over some of the analytic gradients from posadev (just for man…

…ipulatorDynamics, not the contacts, constraints, or implicit cases… yet).

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

systems/plants/PlanarRigidBodyManipulator.m

@@ -48,13 +48,170 @@
 %      obj = obj.setNumVelocityConstraints(0);%size([obj.model.body.ground_contact],2));
     end
 
-    function [H,C,B] = manipulatorDynamics(obj,q,qd)
+    function [H,C,B,dH,dC,dB] = manipulatorDynamics(obj,q,qd)
       m = obj.model.featherstone;
-      [H,C] = HandCp(m,q,qd,{},obj.model.gravity);
-      C=C+m.damping'.*qd;
-      B = obj.model.B;
-    end
+
+      if (nargout>3)
+        % featherstone's HandCp with analytic gradients
+        a_grav = [0;obj.model.gravity];
+
+        S = cell(m.NB,1);
+        Xup = cell(m.NB,1);
+
+        v = cell(m.NB,1);
+        avp = cell(m.NB,1);
+
+        %Derivatives
+        dXupdq = cell(m.NB,1);
+        dvdq = cell(m.NB,1);  %dvdq{i,j} is d/dq(j) v{i}
+        dvdqd = cell(m.NB,1);
+        davpdq = cell(m.NB,1);
+        davpdqd = cell(m.NB,1);
+        fvp = cell(m.NB,1);
+        dfvpdq = cell(m.NB,1);
+        dfvpdqd = cell(m.NB,1);
+
+
+        for i = 1:m.NB
+          dvdq{i} = zeros(3,m.NB)*q(1);
+          dvdqd{i} = zeros(3,m.NB)*q(1);
+          davpdq{i} = zeros(3,m.NB)*q(1);
+          davpdqd{i} = zeros(3,m.NB)*q(1);
+          dfvpdq{i} = zeros(3,m.NB)*q(1);
+          dfvpdqd{i} = zeros(3,m.NB)*q(1);
 
+          [ XJ, S{i} ] = jcalcp( m.jcode(i), q(i) );
+          vJ = S{i}*qd(i);
+          dvJdqd = S{i};
+
+          Xup{i} = XJ * m.Xtree{i};
+          dXJdq = djcalcp(m.jcode(i), q(i));
+          dXupdq{i} = dXJdq * m.Xtree{i};
+
+          if m.parent(i) == 0
+            v{i} = vJ;
+            dvdqd{i}(:,i) = dvJdqd;
+
+            avp{i} = Xup{i} * -a_grav;
+            davpdq{i}(:,i) = dXupdq{i} * -a_grav;
+          else
+            j = m.parent(i);
+            v{i} = Xup{i}*v{j} + vJ;
+
+            dvdq{i} = Xup{i}*dvdq{j};
+            dvdq{i}(:,i) = dvdq{i}(:,i) + dXupdq{i}*v{j};
+
+            dvdqd{i} = Xup{i}*dvdqd{j};
+            dvdqd{i}(:,i) = dvdqd{i}(:,i) + dvJdqd;
+
+            avp{i} = Xup{i}*avp{j} + crmp(v{i})*vJ;
+
+            davpdq{i} = Xup{i}*davpdq{j};
+            davpdq{i}(:,i) = davpdq{i}(:,i) + dXupdq{i}*avp{j};
+            for k=1:m.NB,
+              davpdq{i}(:,k) = davpdq{i}(:,k) + ...
+                dcrmp(v{i},vJ,dvdq{i}(:,k),zeros(3,1));
+            end
+
+            dvJdqd_mat = zeros(3,m.NB);
+            dvJdqd_mat(:,i) = dvJdqd;
+            davpdqd{i} = Xup{i}*davpdqd{j} + dcrmp(v{i},vJ,dvdqd{i},dvJdqd_mat);
+          end
+          fvp{i} = m.I{i}*avp{i} + crfp(v{i})*m.I{i}*v{i};
+          dfvpdq{i} = m.I{i}*davpdq{i} + dcrfp(v{i},m.I{i}*v{i},dvdq{i},m.I{i}*dvdq{i});
+          dfvpdqd{i} = m.I{i}*davpdqd{i} + dcrfp(v{i},m.I{i}*v{i},dvdqd{i},m.I{i}*dvdqd{i});
+        end
+
+        dC = zeros(m.NB,2*m.NB)*q(1);
+        IC = m.I;				% composite inertia calculation
+        dIC = cell(m.NB, m.NB);
+        dIC = cellfun(@(a) zeros(3), dIC,'UniformOutput',false);
+
+        for i = m.NB:-1:1
+          C(i,1) = S{i}' * fvp{i};
+          dC(i,:) = S{i}'*[dfvpdq{i} dfvpdqd{i}];
+          if m.parent(i) ~= 0
+            fvp{m.parent(i)} = fvp{m.parent(i)} + Xup{i}'*fvp{i};
+            dfvpdq{m.parent(i)} = dfvpdq{m.parent(i)} + Xup{i}'*dfvpdq{i};
+            dfvpdq{m.parent(i)}(:,i) = dfvpdq{m.parent(i)}(:,i) + dXupdq{i}'*fvp{i};
+            dfvpdqd{m.parent(i)} = dfvpdqd{m.parent(i)} + Xup{i}'*dfvpdqd{i};
+
+            IC{m.parent(i)} = IC{m.parent(i)} + Xup{i}'*IC{i}*Xup{i};
+            for k=1:m.NB,
+              dIC{m.parent(i),k} = dIC{m.parent(i),k} + Xup{i}'*dIC{i,k}*Xup{i};
+            end
+            dIC{m.parent(i),i} = dIC{m.parent(i),i} + ...
+              dXupdq{i}'*IC{i}*Xup{i} + Xup{i}'*IC{i}*dXupdq{i};
+          end
+        end
+        C=C+m.damping'.*qd;
+        dC(:,m.NB+1:end) = dC(:,m.NB+1:end) + diag(m.damping);
+
+        % minor adjustment to make TaylorVar work better.
+        %H = zeros(m.NB);
+        H=zeros(m.NB)*q(1);
+
+        %Derivatives wrt q(k)
+        dH = zeros(m.NB^2,2*m.NB)*q(1);
+        for k = 1:m.NB
+          for i = 1:m.NB
+            fh = IC{i} * S{i};
+            dfh = dIC{i,k} * S{i};  %dfh/dqk
+            H(i,i) = S{i}' * fh;
+            dH(i + (i-1)*m.NB,k) = S{i}' * dfh;
+            j = i;
+            while m.parent(j) > 0
+              if j==k,
+                dfh = Xup{j}' * dfh + dXupdq{k}' * fh;
+              else
+                dfh = Xup{j}' * dfh;
+              end
+              fh = Xup{j}' * fh;
+
+              j = m.parent(j);
+              H(i,j) = S{j}' * fh;
+              H(j,i) = H(i,j);
+              dH(i + (j-1)*m.NB,k) = S{j}' * dfh;
+              dH(j + (i-1)*m.NB,k) = dH(i + (j-1)*m.NB,k);
+            end
+          end
+        end
+
+        B = obj.model.B;
+        dB = zeros(m.NB*obj.num_u,2*m.NB);
+      else
+        [H,C] = HandCp(m,q,qd,{},obj.model.gravity);
+        C=C+m.damping'.*qd;
+        B = obj.model.B;
+      end
+    end
+
+    function [xdot, dxdot] = dynamics(obj,t,x,u)
+      % Provides the dynamics interface for sodynamics.  This function
+      % does not handle contact or joint limits!
+      q=x(1:obj.num_q); qd=x((obj.num_q+1):end);
+
+      if nargout > 1
+        if (obj.num_xcon>0)
+          error('Not yet supported.');
+        end
+
+        [H,C,B,dH,dC,dB] = obj.manipulatorDynamics(q,qd);
+        Hinv = inv(H);
+
+        qdd = Hinv*(B*u - C);
+        dxdot = [zeros(obj.num_q,1+obj.num_q), eye(obj.num_q),...
+          zeros(obj.num_q,obj.num_u);...
+          zeros(obj.num_q,1),...
+          -Hinv*matGradMult(dH(:,1:obj.num_q),qdd) - Hinv*dC(:,1:obj.num_q),...
+          -Hinv*dC(:,1+obj.num_q:end), Hinv*B];
+        xdot = [qd;qdd];
+      else
+        qdd = obj.sodynamics(t,q,qd,u);
+        xdot = [qd;qdd];
+      end
+    end
+
     function phi = positionConstraints(obj,q)
       error('not implemented yet');  % need to pull from posadev
       phi=[];

systems/plants/test/testGradients.m

@@ -0,0 +1,12 @@
+function testGradients
+
+p = PlanarRigidBodyManipulator('../../../examples/Acrobot/Acrobot.urdf');
+options.grad_method = {'user','taylorvar'};
+
+for i=1:100
+  t = rand;
+  x = randn(4,1);
+  u = randn;
+
+  [xdot,dxdot] = geval(@p.dynamics,t,x,u,options);
+end

thirdParty/spatial/HandCp.m

@@ -43,21 +43,17 @@
   end
 end
 
-for i = model.NB:-1:1
-  C(i,1) = S{i}' * fvp{i};
-  if model.parent(i) ~= 0
-    fvp{model.parent(i)} = fvp{model.parent(i)} + Xup{i}'*fvp{i};
-  end
-end
-
 IC = model.I;				% composite inertia calculation
 
 for i = model.NB:-1:1
+  C(i,1) = S{i}' * fvp{i};
   if model.parent(i) ~= 0
+    fvp{model.parent(i)} = fvp{model.parent(i)} + Xup{i}'*fvp{i};
     IC{model.parent(i)} = IC{model.parent(i)} + Xup{i}'*IC{i}*Xup{i};
   end
 end
 
+
 % minor adjustment to make TaylorVar work better.
 %H = zeros(model.NB);
 H=zeros(model.NB)*q(1);

util/dXpln.m

@@ -0,0 +1,18 @@
+function dX = dXpln( theta, r, varIndex)
+
+% dXpln  coordinate transform derivative wrt theta for planar vectors.
+% dXpln(theta,r) calculates the derivative of the coordinate transform 
+% matrix from A to B coordinates for planar motion vectors, where 
+% coordinate frame B is located at point r (2D vector expressed in A 
+% coords) relative to frame A, and is rotated by an angle theta (radians) 
+% relative to A.
+
+c = cos(theta);
+s = sin(theta);
+if varIndex == 1
+dX = [0 0 0; c*r(1)+s*r(2) -s c; -s*r(1)+c*r(2) -c -s];
+elseif varIndex == 2
+  dX = [0 0  0; s 0 0; c 0 0];
+elseif varIndex == 3
+  dX = [0 0 0; -c 0 0; s 0 0];
+end

util/dcrfp.m

@@ -0,0 +1,8 @@
+function  dvcross = dcrfp( v ,x, dv, dx)
+% @return d/dy crfp(v)*x
+% @param v
+% @param x
+% @param dv = dv/dy
+% @param dx = dx/dy
+dvcross = [-dv(3,:)*x(2) - v(3)*dx(2,:) + dv(2,:)*x(3) + v(2)*dx(3,:);...
+  -dv(1,:)*x(3) - v(1)*dx(3,:); dv(1,:)*x(2) + v(1)*dx(2,:)];

util/dcrmp.m

@@ -0,0 +1,11 @@
+function  dvcross = dcrmp( v ,x, dv, dx)
+% @return d/dy crmp(v)*x
+% @param v
+% @param x
+% @param dv = dv/dy
+% @param dx = dx/dy
+n = size(dv,2);
+dvcross = [zeros(1,n); dv(3,:)*x(1) + v(3)*dx(1,:) - dv(1,:)*x(3) - v(1)*dx(3,:);...
+  -dv(2,:)*x(1) - v(2)*dx(1,:) + dv(1,:)*x(2) + v(1)*dx(2,:)];
+end
+

util/djcalcp.m

@@ -0,0 +1,16 @@
+function  dXj = djcalcp( code, q )
+
+% jcalcp  Calculate derivative of joint transform
+% [dXj]=djcalcp(code,q) calculates the joint transform derivative
+% matrix for revolute (code==1), x-axis prismatic (code==2) and y-axis
+% prismatic (code==3) joints.
+
+if code == 1				% revolute joint
+  dXj = dXpln( q, [0 0] ,1);
+elseif code == 2			% x-axis prismatic joint
+  dXj = dXpln( 0, [q 0] ,2);
+elseif code == 3			% y-axis prismatic joint
+  dXj = dXpln( 0, [0 q] ,3);
+else
+  error( 'unrecognised joint code' );
+end

util/geval.m

@@ -20,6 +20,9 @@
 %      geval(p,fun,varargin) 
 % to tell geval that there are p different outputs.
 %
+% If the output is a matrix (or ND-array), then the gradients are reshaped
+% so that size(df) = [prod(size(f)), prod(size(a))]
+%
 % Higher order gradients are output as a q x r^o sparse matrix , where 
 %    q is the dimension of the output, r is the dimension of the input, and o is the order.
 %    e.g. 

util/matGradMult.m

@@ -0,0 +1,40 @@
+function [y] = matGradMult(A,b,transposeA)
+% [y] = matGradMult(A,b)
+% If A is the result of dB/dX for matrix B and vector X, as output by
+% geval, then return A*b (a matrix).
+% @param vector b
+% @param gradient of A wrt. a vector, written as a matrix
+% geval)
+% @return A*b
+
+if exist('transposeA','var')
+  if transposeA
+    n = length(b);
+    m = size(A,2);
+    k = size(A,1)/n;
+    y = zeros(m,n);
+    for i=1:m,
+      y(:,i) = reshape(A(:,i),n,k)'*b;
+    end
+    return;
+  end
+end
+n = length(b);
+m = size(A,2);
+k = size(A,1)/n;
+
+B = sparse(repmat((1:k)',n,1),1:k*n,reshape(repmat(b',k,1),k*n,1),k,k*n);
+y = B*A;
+% B_row=repmat(reshape(diag([b;zeros(k-n-1,1)]),1,(k-1)^2),1,k);
+% B =  reshape(B_row(1:k*k*n),k*n,k)';
+% y = B*A;
+% return
+
+% tensor = reshape(A,k,n,m);  tensor method was actually a little slower
+% % (<10% difference)
+% y = zeros(k,m); %was mxn-->changed to nxm then changed again
+% for i=1:m,  %was n
+%   y(:,i) = reshape(A(:,i),k,n)*b;
+% %   y(:,i) = tensor(:,:,i)*b;
+% end
+end