Addeds partial jacobian computation.

src/algorithm/jacobian.hpp

@@ -17,14 +17,14 @@ namespace se3
 /* --- Details -------------------------------------------------------------------- */
 namespace se3 
 {
-  struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
+  struct JacobiansForwardStep : public fusion::JointVisitor<JacobiansForwardStep>
   {
     typedef boost::fusion::vector< const se3::Model&,
 				   se3::Data&,
 				   const Eigen::VectorXd &
 				   > ArgsType;
 
-    JOINT_VISITOR_INIT(JacobianForwardStep);
+    JOINT_VISITOR_INIT(JacobiansForwardStep);
 
     template<typename JointModel>
     static void algo(const se3::JointModelBase<JointModel> & jmodel,
@@ -52,13 +52,13 @@ namespace se3
 
 
   inline const Eigen::MatrixXd&
-  computeJacobian(const Model & model, Data& data,
-		  const Eigen::VectorXd & q)
+  computeJacobians(const Model & model, Data& data,
+		   const Eigen::VectorXd & q)
   {
     for( int i=1;i<model.nbody;++i )
       {
-	JacobianForwardStep::run(model.joints[i],data.joints[i],
-			     JacobianForwardStep::ArgsType(model,data,q));
+	JacobiansForwardStep::run(model.joints[i],data.joints[i],
+				  JacobiansForwardStep::ArgsType(model,data,q));
       }
 
     return data.J;
@@ -80,6 +80,59 @@ namespace se3
       }
   }
 
+
+  struct JacobianForwardStep : public fusion::JointVisitor<JacobianForwardStep>
+  {
+    typedef boost::fusion::vector< const se3::Model&,
+				   se3::Data&,
+				   const Eigen::VectorXd &
+				   > ArgsType;
+
+    JOINT_VISITOR_INIT(JacobianForwardStep);
+
+    template<typename JointModel>
+    static void algo(const se3::JointModelBase<JointModel> & jmodel,
+		     se3::JointDataBase<typename JointModel::JointData> & jdata,
+		     const se3::Model& model,
+		     se3::Data& data,
+		     const Eigen::VectorXd & q)
+    {
+      using namespace Eigen;
+      using namespace se3;
+
+      const Model::Index & i = jmodel.id();
+      const Model::Index & parent = model.parents[i];
+
+      jmodel.calc(jdata.derived(),q);
+      
+      data.liMi[i] = model.jointPlacements[i]*jdata.M();
+      data.iMf[parent] = data.liMi[i]*data.iMf[i];
+
+      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.iMf[i].inverse().act(jdata.S());
+    }
+
+  };
+
+  /* Compute the jacobian in the local frame. */
+  const Eigen::MatrixXd&
+  jacobian(const Model & model, Data& data,
+	   const Eigen::VectorXd & q,
+	   const Model::Index & idx )
+  {
+    data.iMf[idx] = SE3::Identity();
+    for( int i=idx;i>0;i=model.parents[i] )
+      {
+	JacobianForwardStep::run(model.joints[i],data.joints[i],
+				 JacobianForwardStep::ArgsType(model,data,q));
+      }
+
+    return data.J;
+  }
+
+
+
+
+
 } // namespace se3
 
 #endif // ifndef __se3_jacobian_hpp__

unittest/jacobian.cpp

 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/spatial/act-on-set.hpp"
 #include "pinocchio/tools/timer.hpp"
 #include "pinocchio/multibody/parser/sample-models.hpp"
@@ -27,14 +28,14 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
       timer.tic();
       SMOOTH(NBT)
       {
-	computeJacobian(model,data,q);
+	computeJacobians(model,data,q);
       }
       if(verbose) std::cout << "Compute =\t";
       timer.toc(std::cout,NBT);
     }
   if( flag >> 1 & 1 )
     {
-      computeJacobian(model,data,q);
+      computeJacobians(model,data,q);
       Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1; 
       Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
 
@@ -48,7 +49,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
     }
   if( flag >> 2 & 1 )
     {
-      computeJacobian(model,data,q);
+      computeJacobians(model,data,q);
       Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1; 
       Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
 
@@ -60,6 +61,20 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
       if(verbose) std::cout << "Change frame =\t";
       timer.toc(std::cout,NBT);
     }
+  if( flag >> 2 & 1 )
+    {
+      computeJacobians(model,data,q);
+      Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1; 
+      Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+
+      timer.tic();
+      SMOOTH(NBT)
+      {
+	jacobian(model,data,q,idx);
+      }
+      if(verbose) std::cout << "Single jacobian =\t";
+      timer.toc(std::cout,NBT);
+    }
 }
 
 void assertValues(const se3::Model & model, se3::Data& data)
@@ -68,7 +83,7 @@ void assertValues(const se3::Model & model, se3::Data& data)
   using namespace se3;
 
   VectorXd q = VectorXd::Zero(model.nq);
-  computeJacobian(model,data,q);
+  computeJacobians(model,data,q);
 
   Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):model.nbody-1; 
   MatrixXd Jrh(6,model.nv); Jrh.fill(0);
@@ -88,6 +103,10 @@ void assertValues(const se3::Model & model, se3::Data& data)
     MatrixXd XJrh(6,model.nv); 
     motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
     assert( XJrh.isApprox(rhJrh) );
+
+    data.J.fill(0);
+    XJrh = jacobian(model,data,q,idx);
+    assert( XJrh.isApprox(rhJrh) );
   }
 
   std::cout << "Jrh = [ " << Jrh << " ];" << std::endl;