update relative position constraints to avoid call to bodyKin when us…

…ing new kinsol.
ckguo · Feb 16, 2015 · 9f66a04 · 9f66a04
1 parent ecb81fb
commit 9f66a04
Showing 1 changed file with 22 additions and 6 deletions.
diff --git a/solvers/+drakeFunction/+kinematic/RelativePosition.m b/solvers/+drakeFunction/+kinematic/RelativePosition.m
@@ -64,25 +64,41 @@
       % @retval dJ  -- [3*n_pts x nq^2] second-derivatives of pos w.r.t.
       %                q in geval format
       compute_second_derivatives = (nargout > 2);
+      options.base_or_frame_id = obj.frameB;
+      options.in_terms_of_qdot = true;
       if compute_second_derivatives
         kinsol = obj.rbm.doKinematics(q,true,false);
         if obj.frameA == 0
           [pts_in_world,JA,dJA] = getCOM(obj.rbm,kinsol);
+          [pts_in_B,P,JB,dP,dJB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
+          J = JB + P*JA;
+          dJ = dJB + reshape(matGradMultMat(P,JA,dP,reshape(dJA,numel(JA),[])),size(dJB));
         else
-          [pts_in_world,JA,dJA] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,0);
+          if obj.rbm.use_new_kinsol
+            [pts_in_B,J,dJ] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,options);
+          else
+            [pts_in_world,JA,dJA] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,0);
+            [pts_in_B,P,JB,dP,dJB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
+            J = JB + P*JA;
+            dJ = dJB + reshape(matGradMultMat(P,JA,dP,reshape(dJA,numel(JA),[])),size(dJB));
+          end
         end
-        [pts_in_B,P,JB,dP,dJB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
-        dJ = dJB + reshape(matGradMultMat(P,JA,dP,reshape(dJA,numel(JA),[])),size(dJB));
       else
         kinsol = obj.rbm.doKinematics(q);
         if obj.frameA == 0
           [pts_in_world,JA] = getCOM(obj.rbm,kinsol);
+          [pts_in_B,P,JB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
+          J = JB + P*JA;
         else
-          [pts_in_world,JA] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,0);
+          if obj.rbm.use_new_kinsol
+            [pts_in_B,J] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,options);
+          else
+            [pts_in_world,JA] = forwardKin(obj.rbm,kinsol,obj.frameA,obj.pts_in_A,0);
+            [pts_in_B,P,JB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
+            J = JB + P*JA;
+          end
         end
-        [pts_in_B,P,JB] = obj.rbm.bodyKin(kinsol,obj.frameB,pts_in_world);
       end
-      J = JB + P*JA;
       pos = reshape(pts_in_B,[],1);
     end
   end