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

Working with FF: 3.9us.

parent 3e1f801e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithm/cholesky.hpp
+ 86
− 30
View file @ 71c7d3ea
......@@ -44,46 +44,102 @@ namespace se3
const Model& model,
Data& data)
{
typedef typename Model::Index Index;
Eigen::MatrixXd & M = data.M;
Eigen::MatrixXd & U = data.U;
Eigen::VectorXd & D = data.D;
const int j = jmodel.id();
const int _j = jmodel.idx_v();
const int nvSubtree = data.nvSubtree[j];
D[_j] = M(_j,_j);
for( int m=1;m<nvSubtree;++m )
D[_j] -= U(_j,_j+m) * D[_j+m] * U(_j,_j+m);
Eigen::VectorXd::SegmentReturnType DUt = data.tmp.head(nvSubtree-1);
if(nvSubtree>1)
DUt = U.row(_j).segment(_j+1,nvSubtree-1).transpose()
.cwiseProduct(D.segment(_j+1,nvSubtree-1));
for( Index i=model.parents[j];i>0;i=model.parents[i])
{
const int _i = idx_v(model.joints[i]);
U(_i,_j) = M(_i,_j);
if( nvSubtree>1 )
U(_i,_j) -= U.row(_i).segment(_j+1,nvSubtree-1).dot(DUt);
U(_i,_j) /= D[_j];
}
// algoSized<JointModel::NV>(model,data,j,_j);
algoSized<JointModel::NV>(model,data,jmodel.id(),jmodel.idx_v());
}
};
template<int N,typename M_t>
void udut( Eigen::MatrixBase<M_t> & M )
{
typedef Eigen::Matrix<double,N,N> MatrixNd;
typedef Eigen::Matrix<double,1,N> VectorNd;
typedef typename M_t::DiagonalReturnType D_t;
typedef typename M_t::template TriangularViewReturnType<Eigen::StrictlyUpper>::Type U_t;
VectorNd tmp;
D_t D = M.diagonal();
U_t U = M.template triangularView<Eigen::StrictlyUpper>();
for(int j=N-1;j>=0;--j )
{
typename VectorNd::SegmentReturnType DUt = tmp.tail(N-j-1);
if( j<N-1 ) DUt = M.row(j).tail(N-j-1).transpose().cwiseProduct( D.tail(N-j-1) );
D[j] -= M.row(j).tail(N-j-1).dot(DUt);
for(int i=j-1;i>=0;--i)
{ U(i,j) -= M.row(i).tail(N-j-1).dot(DUt); U(i,j) /= D[j]; }
}
}
template<int NVJ>
void CholeskyOuterLoopStep::algoSized( const Model& model,
Data& data,
const int & j,
const int & _j)
{
typedef typename Model::Index Index;
Eigen::MatrixXd & M = data.M;
Eigen::MatrixXd & U = data.U;
Eigen::VectorXd & D = data.D;
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");
}
}
template<>
void CholeskyOuterLoopStep::algoSized<1>( const Model& model,
Data& data,
const int & j,
const int & _j)
{
Eigen::MatrixXd & M = data.M;
Eigen::MatrixXd & U = data.U;
Eigen::VectorXd & D = data.D;
typedef typename Model::Index Index;
Eigen::MatrixXd & M = data.M;
Eigen::MatrixXd & U = data.U;
Eigen::VectorXd & D = data.D;
const int NVT = data.nvSubtree[j]-1;
Eigen::VectorXd::SegmentReturnType DUt = data.tmp.head(NVT);
if(NVT)
DUt = U.row(_j).segment(_j+1,NVT).transpose()
.cwiseProduct(D.segment(_j+1,NVT));
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];
}
}
}
......
src/multibody/joint/joint-variant.hpp
+ 2
− 2
View file @ 71c7d3ea
......@@ -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
+ 20
− 20
View file @ 71c7d3ea
......@@ -37,12 +37,13 @@ int main(int argc, const char ** argv)
// 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"),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");
model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg1");
model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg2");
......@@ -98,15 +99,7 @@ int main(int argc, const char ** argv)
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
crba(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);
}
StackTicToc timer(StackTicToc::US); timer.tic();
#ifdef NDEBUG
SMOOTH(1000)
......@@ -118,13 +111,20 @@ int main(int argc, const char ** argv)
for(int i=0;i<model.nv;++i)
for(int j=0;j<model.nv;++j)
if(isnan(data.M(i,j))) data.M(i,j) = 0;
{
if(isnan(data.M(i,j))) data.M(i,j) = 0;
data.M(j,i) = data.M(i,j);
}
// std::cout << "M = [\n" << data.M << "];" << std::endl;
// std::cout << "U = [\n" << data.U << "];" << std::endl;
// std::cout << "D = [\n" << data.D.transpose() << "];" << std::endl;
data.U.triangularView<Eigen::StrictlyLower>().fill(0);
data.U.diagonal().fill(1);
#ifndef NDEBUG
std::cout << "M = [\n" << data.M << "];" << std::endl;
std::cout << "U = [\n" << data.U << "];" << std::endl;
std::cout << "D = [\n" << data.D.transpose() << "];" << std::endl;
// std::cout << "UDU = [\n" << (data.U*data.D.asDiagonal()*data.U.transpose()) << "];" << std::endl;
#endif
assert((data.U*data.D.asDiagonal()*data.U.transpose()).isApprox(data.M));
......
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