IVIGIT Cholesky.

CMakeLists.txt

@@ -54,6 +54,7 @@ SET(${PROJECT_NAME}_HEADERS
   algorithm/rnea.hpp
   algorithm/crba.hpp
   algorithm/crba2.hpp
+  algorithm/cholesky.hpp
   algorithm/kinematics.hpp
 )
 
@@ -100,6 +101,10 @@ ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL unittest/crba.cpp)
 PKG_CONFIG_USE_DEPENDENCY(crba eigenpy)
 PKG_CONFIG_USE_DEPENDENCY(crba urdfdom)
 
+ADD_EXECUTABLE(cholesky EXCLUDE_FROM_ALL unittest/cholesky.cpp)
+PKG_CONFIG_USE_DEPENDENCY(cholesky eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(cholesky urdfdom)
+
 ADD_EXECUTABLE(dg EXCLUDE_FROM_ALL unittest/dg.cpp)
 PKG_CONFIG_USE_DEPENDENCY(dg eigenpy)
 

src/algorithm/chol.m

+clear U L M parent tree 
+
+parent = [ 0 1    2 3    2 5    2 7    8 9    8 11 ];
+n=12;
+M = zeros(n);
+for i=1:n
+    j=i;
+    while j>0
+        M(i,j) = rand;
+        M(j,i) = M(i,j);
+        L(i,j) = 1;
+        U(j,i) = 1;
+        j=parent(j);
+    end
+end
+    
+tree = zeros(1,n);
+for i=n:-1:2
+    if(tree(i)==0) tree(i) = i; end
+    tree(parent(i)) = max(tree(parent(i)), tree(i) );
+end
+
+D = zeros(n,1);
+U = eye(n);
+for i=n:-1:1
+    for j=n:-1:i+1
+        subtree = j+1:tree(j);
+        U(i,j) = inv(D(j)) * (M(i,j) - U(i,subtree)*diag(D(subtree))*U(j,subtree)');
+    end
+    subtree = i+1:tree(i);
+    D(i) = M(i,i) - U(i,subtree)*diag(D(subtree))*U(i,subtree)';
+end
+
+Msav = M;
+D = zeros(n,1);
+U = eye(n);
+for k=n:-1:1
+    i=parent(k);
+    while i>0
+        a = M(i,k) / M(k,k);
+        M(i,i) = M(i,i) - a * M(i,k);
+        j = parent(i);
+        while j>0
+            M(j,i) = M(j,i) - a * M(j,k);
+            j=parent(j);
+        end
+        M(i,k) = a;
+        i=parent(i);
+    end
+end
+
+U = triu(M,1) + eye(n); 
+D = diag(M);
+M = Msav;
+norm(U*diag(D)*U' - M)
\ No newline at end of file

src/algorithm/cholesky.hpp

+#ifndef __se3_cholesky_hpp__
+#define __se3_cholesky_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include <iostream>
+  
+namespace se3
+{
+  inline const Eigen::MatrixXd&
+  cholesky(const Model &           model, 
+	   Data&                   data );
+
+
+  template<typename D>
+  void multSqrt( const Model &                  model, 
+		 Data&                          data ,
+		 const Eigen::MatrixBase<D> &   v);
+
+} // namespace se3 
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3 
+{
+  template<typename JointModel,typename D>
+  void udut( Eigen::MatrixBase<D> & U,
+	     const JointModel& )
+  {
+    /* TODO */
+  }
+
+
+  // struct CholeskyInnerStep : public fusion::JointVisitor<CholeskyInnerStep>
+  // {
+  //   typedef boost::fusion::vector<const Model&,
+  // 				  Data&,
+  // 				  const int &,
+  // 				  const int &>  ArgsType;
+    
+  //   JOINT_VISITOR_INIT(CholeskyOuterStep);
+ 
+  //   template<typename JointModel>
+  //   static void algo(const JointModelBase<JointModel> & jmodel,
+  // 		     JointDataBase<typename JointModel::JointData> & ,
+  // 		     const Model& model,
+  // 		     Data& data,
+  // 		     int j)
+  //   {
+      
+  //   }
+  // };
+  
+  // struct CholeskyOuterStep : public fusion::JointVisitor<CholeskyOuterStep>
+  // {
+  //   typedef boost::fusion::vector<const Model&,
+  // 				  Data&,
+  // 				  const int &>  ArgsType;
+    
+  //   JOINT_VISITOR_INIT(CholeskyOuterStep);
+
+  //   template<typename JointModel>
+  //   static void algo(const JointModelBase<JointModel> & jmodel,
+  // 		     JointDataBase<typename JointModel::JointData> & ,
+  // 		     const Model& model,
+  // 		     Data& data,
+  // 		     int j)
+  //   {
+  //     typename Model::Index parent = jmodel.parents[j];
+  //     while( parent>0 )
+  // 	{
+	  
+  // 	}
+
+  //     const int 
+  // 	idx = jmodel.idx_v(),
+  // 	nv = JointModel::nv,
+  // 	idx_tree = idx+nv,
+  // 	nv_tree = data.nvSubtree[j];
+      
+  //     data.U.template block<nv,nv>(j,j)
+  // 	-= data.U.block(idx,idx_tree,nv,nv_tree)
+  // 	* data.D.segment(idx_tree,nv_tree).asDiagonal()
+  // 	* data.U.block(idx,idx_tree,nv,nv_tree).transpose();
+      
+  //     Eigen::Block<Eigen::MatrixXd,nv,nv> Ujj = data.U.template block<nv,nv>(j,j);
+  //     udut(Ujj,jmodel);
+  //   }
+  // };
+
+ 
+  template<typename JointModel_k,typename JointModel_i>
+  struct CholeskyLoopJStep : public fusion::JointVisitor< CholeskyLoopJStep<JointModel_k,JointModel_i> >
+  {
+    typedef boost::fusion::vector<const JointModelBase<JointModel_k>&,
+				  const JointModelBase<JointModel_i>&,
+				  const Model&,
+				  Data&,
+				  const int &,
+				  const int &,
+				  const double &>  ArgsType;
+    
+    CholeskyLoopJStep( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {}
+    using fusion::JointVisitor< CholeskyLoopJStep<JointModel_k,JointModel_i>  >::run;
+    JointDataVariant & jdata;			
+    ArgsType args;
+
+    template<typename JointModel_j>
+    static void algo(const JointModelBase<JointModel_j> & jj_model,
+		     JointDataBase<typename JointModel_j::JointData> &,
+		     const JointModelBase<JointModel_k> & jk_model,
+		     const JointModelBase<JointModel_i> & ji_model,
+		     const Model& model,
+		     Data& data,
+		     const int & k,
+		     const int & i,
+		     const int & j)
+    {
+    }
+  };
+  
+ 
+  template<typename JointModel_k>
+  struct CholeskyLoopIStep : public fusion::JointVisitor< CholeskyLoopIStep<JointModel_k> >
+  {
+    typedef boost::fusion::vector<const JointModelBase<JointModel_k>&,
+  				  const Model&,
+  				  Data&,
+  				  const int &,
+  				  const int &>  ArgsType;
+    
+    //JOINT_VISITOR_INIT(CholeskyLoopIStep);
+    CholeskyLoopIStep( JointDataVariant & jdata,ArgsType args ) : jdata(jdata),args(args) {}
+    using fusion::JointVisitor< CholeskyLoopIStep<JointModel_k>  >::run;				       
+    JointDataVariant & jdata;			
+    ArgsType args;
+
+
+    template<typename JointModel_i>
+    static void algo(const JointModelBase<JointModel_i> & ji_model,
+  		     JointDataBase<typename JointModel_i::JointData> &,
+  		     const JointModelBase<JointModel_k> & jk_model,
+  		     const Model& model,
+  		     Data& data,
+  		     const int & k,
+  		     const int & i)
+    {
+      
+      double a = data.M( jk_model.idx_v(),ji_model.idx_v() )/data.M(k,k);
+      typedef typename Model::Index Index;
+      for( Index j=i;j>0;j=model.parents[j])
+	{
+       	  CholeskyLoopJStep<JointModel_k,JointModel_i>
+	    ::run(model.joints[j],data.joints[j],
+		  typename CholeskyLoopJStep<JointModel_k,JointModel_i>
+		  ::ArgsType(jk_model,ji_model,model,data,k,i,a) );
+       	}
+    }
+  };
+  
+  struct CholeskyLoopKStep : public fusion::JointVisitor<CholeskyLoopKStep>
+  {
+    typedef boost::fusion::vector<const Model&,
+  				  Data&,
+  				  const int &>  ArgsType;
+    
+    JOINT_VISITOR_INIT(CholeskyLoopKStep);
+ 
+    template<typename JointModel>
+    static void algo(const JointModelBase<JointModel> & jk_model,
+  		     JointDataBase<typename JointModel::JointData> & jk_data,
+  		     const Model& model,
+  		     Data& data,
+  		     const int & k )
+    {
+      typedef typename Model::Index Index;
+      for( Index i=model.parents[k];i>0;i=model.parents[i])
+  	{
+  	  CholeskyLoopIStep<JointModel>
+  	    ::run(model.joints[i],data.joints[i],
+  	     	  typename CholeskyLoopIStep<JointModel>
+  		  ::ArgsType(jk_model,model,data,k,i) );
+  	}
+    }
+  };
+  
+  /*
+   * U=zeros(n,n); D=zeros(n,1);
+   * for j=n:-1:1
+   *     for k=n:-1:j+1
+   *         subtree = k+1:n;
+   *         U(j,k) = inv(D(k)) * (A(j,k) - U(k,subtree)*diag(D(subtree))*U(j,subtree)');
+   *     end
+   *     subtree = j+1:n;
+   *     D(j) = A(j,j) - U(j,subtree)*diag(D(subtree))*U(j,subtree)';
+   * end
+   * U = U + diag(ones(n,1));
+   */
+  inline const Eigen::MatrixXd&
+  cholesky(const Model &           model, 
+	   Data&                   data )
+  {
+    data.U = data.M;
+    for( int j=model.nbody-1;j>=0;--j )
+      {
+	CholeskyLoopKStep::run(model.joints[j],data.joints[j],
+	 		       CholeskyLoopKStep::ArgsType(model,data,j));
+
+       }
+
+    return data.U;
+  }
+
+  template<typename D>
+  void multSqrt( const Model &                  model, 
+		 Data&                          data ,
+		 const Eigen::MatrixBase<D> &   v)
+  {
+    /* for i = n:1
+     *    res[i] = L(i,i) x[i]
+     *    while j>0
+     *       y[i] += L(i,j) x[j]
+     */
+
+    
+
+
+  }
+  
+
+
+} // namespace se3 
+#endif // ifndef __se3_cholesky_hpp__

src/multibody/joint/joint-variant.hpp

@@ -6,8 +6,8 @@
 
 namespace se3
 {
-  typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ,JointModelFreeFlyer> JointModelVariant;
-  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataFreeFlyer > JointDataVariant;
+  typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ  /*,JointModelFreeFlyer*/> JointModelVariant;
+  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ  /*, JointDataFreeFlyer*/ > JointDataVariant;
 
   typedef std::vector<JointModelVariant> JointModelVector;
   typedef std::vector<JointDataVariant> JointDataVector;

unittest/cholesky.cpp

+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/crba2.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+//#include "pinocchio/multibody/parser/urdf.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+
+//#define __SSE3__
+#include <fenv.h>
+#ifdef __SSE3__
+#include <pmmintrin.h>
+#endif
+
+int main(int argc, const char ** argv)
+{
+#ifdef __SSE3__
+  _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
+#endif
+
+
+  using namespace Eigen;
+  using namespace se3;
+
+  SE3::Matrix3 I3 = SE3::Matrix3::Identity();
+
+  se3::Model model;
+
+  // std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
+  // if(argc>1) filename = argv[1];
+  // model = se3::buildModel(filename,false);
+
+
+  model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff1");
+  model.addBody(model.getBodyId("ff1"),JointModelRX(),SE3::Random(),Inertia::Random(),"root");
+
+  model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg1");
+  model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg2");
+
+  model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg1");
+  model.addBody(model.getBodyId("rleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg2");
+
+  model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso1");
+  model.addBody(model.getBodyId("torso1"),JointModelRX(),SE3::Random(),Inertia::Random(),"chest");
+
+  model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm1");
+  model.addBody(model.getBodyId("rarm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm2");
+  model.addBody(model.getBodyId("rarm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm3");
+
+  model.addBody(model.getBodyId("chest"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm1");
+  model.addBody(model.getBodyId("larm1"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm2");
+  model.addBody(model.getBodyId("larm2"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm3");
+
+  std::cout << model << std::endl;
+
+  se3::Data data(model);
+  VectorXd q = VectorXd::Zero(model.nq);
+  crba2(model,data,q);
+  
+  std::cout << "M = [\n" << data.M << "];" << std::endl;
+  for(int i=0;i<model.nv;++i)
+    for(int j=i;j<model.nv;++j)
+      {
+	data.M(i,j) = round(data.M(i,j))+1;
+	data.M(j,i) = data.M(i,j);
+      }
+  std::cout << "M = [\n" << data.M << "];" << std::endl;
+	
+  //cholesky(model,data)
+
+  return 0;
+}