Skip to content
Snippets Groups Projects
Commit b621c85b authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Tmp version cholesky = 3us for Revolute.

parent 71c7d3ea
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithm/cholesky.hpp
+ 28
− 34
View file @ b621c85b
......@@ -37,6 +37,8 @@ namespace se3
const int & j,
const int & _j);
template<int N,typename M_t>
static void udut( Eigen::MatrixBase<M_t> & M );
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
......@@ -49,8 +51,9 @@ namespace se3
};
/* Compute the dense UDUt decomposition of M. */
template<int N,typename M_t>
void udut( Eigen::MatrixBase<M_t> & M )
void CholeskyOuterLoopStep::udut( Eigen::MatrixBase<M_t> & M )
{
typedef Eigen::Matrix<double,N,N> MatrixNd;
typedef Eigen::Matrix<double,1,N> VectorNd;
......@@ -73,7 +76,6 @@ namespace se3
}
}
template<int NVJ>
void CholeskyOuterLoopStep::algoSized( const Model& model,
Data& data,
......@@ -87,27 +89,25 @@ namespace se3
const int NVT = data.nvSubtree[j] - NVJ;
Eigen::Block<Eigen::MatrixXd> DUt = U.block(NVJ,0,NVT,NVJ);
DUt = D.segment(_j+NVJ,NVT).asDiagonal() * U.block( _j,_j+NVJ,NVJ,NVT).transpose();
Eigen::Block<Eigen::MatrixXd,NVJ,NVJ> Djj = U.template block<NVJ,NVJ>(_j,_j);
Djj.template triangularView<Eigen::Upper>()
= M.template block<NVJ,NVJ>(_j,_j) - U.block( _j,_j+NVJ,NVJ,NVT) * DUt;
udut<NVJ>(Djj);
D.template segment<NVJ>(_j) = Djj.diagonal();
for( Index i=model.parents[j];i>0;i=model.parents[i])
{
const int _i = idx_v(model.joints[i]);
const int nvi = nv(model.joints[i]);
for( int k=0;k<nvi;++k )
{
U.template block<1,NVJ>(_i+k,_j)
= M.template block<1,NVJ>(_i+k,_j) - U.block(_i+k,_j+1,1,NVT) * DUt;
}
// TODO divide by Djj
assert(false && "TODO divide by Djj");
}
//DUt = D.segment(_j+NVJ,NVT).asDiagonal() * U.block( _j,_j+NVJ,NVJ,NVT).transpose();
DUt = U.block( _j,_j+NVJ,NVJ,NVT).transpose();
Eigen::Block<Eigen::MatrixXd,NVJ,NVJ> Djj = U.template block<NVJ,NVJ>(_j,_j);
// Djj.template triangularView<Eigen::Upper>()
// = M.template block<NVJ,NVJ>(_j,_j) - U.block( _j,_j+NVJ,NVJ,NVT) * DUt;
//udut<NVJ>(Djj);
//D.template segment<NVJ>(_j) = Djj.diagonal();
//for(int i=0;i<NVJ;++i) D[_j+i] = Djj(i,i);
/* The same following loop could be achieved using model.parents, however
* this one fit much better the predictor of the CPU, saving 1/4 of cost. */
// for( int _i=data.parentsRow[_j];_i>=0;_i=data.parentsRow[_i] )
// {
// U.template block<1,NVJ>(_i,_j)
// = M.template block<1,NVJ>(_i,_j) - U.block(_i,_j+1,1,NVT) * DUt;
// // TODO divide by Djj
// assert(false && "TODO divide by Djj");
// }
}
template<>
......@@ -129,17 +129,11 @@ namespace se3
D[_j] = M(_j,_j) - U.row(_j).segment(_j+1,NVT) * DUt;
for( Index i=model.parents[j];i>0;i=model.parents[i])
{
const int _i = idx_v(model.joints[i]);
const int nvi = nv(model.joints[i]);
for( int k=0;k<nvi;++k )
{
U(_i+k,_j) = M(_i+k,_j) - U.row(_i+k).segment(_j+1,NVT).dot(DUt);
U(_i+k,_j) /= D[_j];
}
}
/* The same following loop could be achieved using model.parents, however
* this one fit much better the predictor of the CPU, saving 1/4 of cost. */
for( int _i=data.parentsRow[_j];_i>=0;_i=data.parentsRow[_i] )
U(_i,_j) = (M(_i,_j) - U.row(_i).segment(_j+1,NVT).dot(DUt)) / D[_j];
}
......
src/multibody/joint/joint-variant.hpp
+ 2
− 0
View file @ b621c85b
......@@ -45,6 +45,7 @@ namespace se3
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_nq(), jmodel ); }
};
inline int nq(const JointModelVariant & jmodel) { return Joint_nq::run(jmodel); }
class Joint_idx_q: public boost::static_visitor<int>
{
......@@ -56,6 +57,7 @@ namespace se3
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_idx_q(), jmodel ); }
};
inline int idx_q(const JointModelVariant & jmodel) { return Joint_idx_q::run(jmodel); }
class Joint_idx_v: public boost::static_visitor<int>
{
......
src/multibody/model.hpp
+ 19
− 0
View file @ b621c85b
......@@ -85,10 +85,13 @@ namespace se3
Eigen::MatrixXd U; // Joint Inertia square root (upper triangle)
Eigen::VectorXd D; // Diagonal of UDUT inertia decomposition
Eigen::VectorXd tmp; // Temporary of size NV used in Cholesky
std::vector<int> parentsRow; // First previous non-zero row in M (used in Cholesky)
Data( const Model& ref );
void computeLastChild(const Model& model);
void computeParentsRow(const Model& model);
};
const Eigen::Vector3d Model::gravity981 (0,0,-9.81);
......@@ -178,12 +181,14 @@ namespace se3
,U(ref.nv,ref.nv)
,D(ref.nv)
,tmp(ref.nv)
,parentsRow(ref.nv)
{
for(int i=0;i<model.nbody;++i)
joints.push_back(CreateJointData::run(model.joints[i]));
M.fill(NAN);
for(int i=0;i<ref.nbody;++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
computeLastChild(ref);
computeParentsRow(ref);
}
void Data::computeLastChild(const Model& model)
......@@ -205,6 +210,20 @@ namespace se3
}
}
void Data::computeParentsRow( const Model& model )
{
for( int joint=1;joint<model.nbody;joint++)
{
const int & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
const int idx_vj = idx_v(model.joints[joint]);
if(parent>0) parentsRow[idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parentsRow[idx_vj] = -1;
for( int row=1;row<nvj;++row) parentsRow[idx_vj+row] = idx_vj+row-1;
}
}
} // namespace se3
#endif // ifndef __se3_model_hpp__
unittest/cholesky.cpp
+ 36
− 9
View file @ b621c85b
......@@ -5,9 +5,11 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
//#include "pinocchio/multibody/parser/urdf.hpp"
#include <iostream>
#ifdef NDEBUG
# include <Eigen/Cholesky>
#endif
#include "pinocchio/tools/timer.hpp"
......@@ -36,14 +38,17 @@ int main(int argc, const char ** argv)
// 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(),"ff2");
// model.addBody(model.getBodyId("ff2"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff3");
// model.addBody(model.getBodyId("ff3"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff4");
// model.addBody(model.getBodyId("ff4"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff5");
// model.addBody(model.getBodyId("ff5"),JointModelRX(),SE3::Random(),Inertia::Random(),"root");
model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),SE3::Random(),Inertia::Random(),"root");
if( 1 ) // Without FF
{
model.addBody(model.getBodyId("universe"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff1");
model.addBody(model.getBodyId("ff1"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff2");
model.addBody(model.getBodyId("ff2"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff3");
model.addBody(model.getBodyId("ff3"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff4");
model.addBody(model.getBodyId("ff4"),JointModelRX(),SE3::Random(),Inertia::Random(),"ff5");
model.addBody(model.getBodyId("ff5"),JointModelRX(),SE3::Random(),Inertia::Random(),"root");
}
else
model.addBody(model.getBodyId("universe"),JointModelFreeFlyer(),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");
......@@ -109,6 +114,7 @@ int main(int argc, const char ** argv)
}
timer.toc(std::cout,1000);
for(int i=0;i<model.nv;++i)
for(int j=0;j<model.nv;++j)
{
......@@ -116,6 +122,27 @@ int main(int argc, const char ** argv)
data.M(j,i) = data.M(i,j);
}
#ifdef NDEBUG
Eigen::Matrix<double,33,33> M33 = data.M;
timer.tic();
SMOOTH(1000)
{
M33 = data.M;
//Eigen::LDLT <Eigen::MatrixXd> Mchol(data.M);
CholeskyOuterLoopStep::udut<33>(M33);
}
std::cout << "\t\t"; timer.toc(std::cout,1000);
timer.tic();
SMOOTH(1000)
{
Eigen::LDLT <Eigen::MatrixXd> Mchol(data.M);
}
std::cout << "\t\t"; timer.toc(std::cout,1000);
#endif
data.U.triangularView<Eigen::StrictlyLower>().fill(0);
data.U.diagonal().fill(1);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment