From b60978be864854ade73c5d550f2776b0faa0f85a Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 5 Sep 2014 10:37:06 +0200
Subject: [PATCH] IVIGIT Cholesky.
---
CMakeLists.txt | 5 +
src/algorithm/chol.m | 55 ++++++
src/algorithm/cholesky.hpp | 232 ++++++++++++++++++++++++++
src/multibody/joint/joint-variant.hpp | 4 +-
unittest/cholesky.cpp | 79 +++++++++
5 files changed, 373 insertions(+), 2 deletions(-)
create mode 100644 src/algorithm/chol.m
create mode 100644 src/algorithm/cholesky.hpp
create mode 100644 unittest/cholesky.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9caa4619d..1993ab0f8 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/src/algorithm/chol.m b/src/algorithm/chol.m
new file mode 100644
index 000000000..2455d50e7
--- /dev/null
+++ b/src/algorithm/chol.m
@@ -0,0 +1,55 @@
+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
diff --git a/src/algorithm/cholesky.hpp b/src/algorithm/cholesky.hpp
new file mode 100644
index 000000000..1b938db3e
--- /dev/null
+++ b/src/algorithm/cholesky.hpp
@@ -0,0 +1,232 @@
+#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__
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 311719750..17010c35f 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/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;
diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp
new file mode 100644
index 000000000..1ced4990f
--- /dev/null
+++ b/unittest/cholesky.cpp
@@ -0,0 +1,79 @@
+#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;
+}
--
GitLab