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

First working version for cholesky for Revolute only, using Featherstone litteral.

parent b60978be
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithm/chol.m
+ 78
− 29
View file @ bafef1e9
clear U L M parent tree
parent = [ 0 1 2 3 2 5 2 7 8 9 8 11 ];
n=12;
%parent = [ 0 1 2 3 2 5 2 7 8 9 10 8 12 13 ];
%parent = [ 0 1 2 3 4 5 6 7 8 9 10 11 6 13 14 15 16 17 6 19 20 21 ...
% 22 23 24 25 20 27 28 29 30 31 ];
n = size(parent,2);
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
M
% M = [
% 32 -8 -1 2 2 1 -14 2 -3 0 3 -4 0 1
% -8 27 2 -1 1 2 -11 12 6 2 1 3 0 1
% -1 2 4 1 0 0 0 0 0 0 0 0 0 0
% 2 -1 1 3 0 0 0 0 0 0 0 0 0 0
% 2 1 0 0 3 1 0 0 0 0 0 0 0 0
% 1 2 0 0 1 2 0 0 0 0 0 0 0 0
% -14 -11 0 0 0 0 29 -13 -1 0 0 5 4 0
% 2 12 0 0 0 0 -13 19 4 2 2 2 -2 2
% -3 6 0 0 0 0 -1 4 9 2 -1 0 0 0
% 0 2 0 0 0 0 0 2 2 3 2 0 0 0
% 3 1 0 0 0 0 0 2 -1 2 4 0 0 0
% -4 3 0 0 0 0 5 2 0 0 0 7 1 1
% 0 0 0 0 0 0 4 -2 0 0 0 1 3 1
% 1 1 0 0 0 0 0 2 0 0 0 1 1 1];
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)');
method=3
if method==1
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
end
if method==2
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);
%ak = [i k a]
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);
%ij= [ i j M(i,j)]
j=parent(j);
end
M(i,k) = a;
%ik = [ i k M(i,k)]
i=parent(i);
end
end
subtree = i+1:tree(i);
D(i) = M(i,i) - U(i,subtree)*diag(D(subtree))*U(i,subtree)';
U = triu(M,1) + eye(n);
D = diag(M);
M = Msav;
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);
% i<k => U(i,k) != 0, U(k,k) = 1
% M(i,k) = sum_{m=1}^{n} U(i,m) D(m) U(k,m)'
% = sum_{m=k+1}^{n} U(i,m) D(m) U(k,m)' + U(i,k) D(k)
% => U(i,k) = ( M(i,k) - sum_{m>k} ... ) D{k}^{-1}
if method==3
D = zeros(n,1);
U = eye(n);
for k=n:-1:1
subtree = k+1:tree(k);
D(k) = M(k,k) - U(k,subtree)*diag(D(subtree))*U(k,subtree)';
i=parent(k);
while i>0
U(i,k) = (M(i,k) - U(i,subtree)*diag(D(subtree))*U(k,subtree)') ...
/ D(k);
i=parent(i);
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
+ 36
− 155
View file @ bafef1e9
......@@ -22,183 +22,62 @@ 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,
JointDataBase<typename JointModel::JointData> &,
const Model& model,
Data& data,
const int & k )
{
typedef typename Model::Index Index;
Eigen::MatrixXd & U = data.U;
const int & _k = jk_model.idx_v();
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) );
}
{
const int _i = idx_v(model.joints[i]);
const double a = 1/U(_k,_k) * U(_i,_k);
U(_i,_i) -= a * U(_i,_k);
for( Index j=model.parents[i];j>0;j=model.parents[j])
{
const int _j = idx_v(model.joints[j]);
U(_j,_i) -= a * U(_j,_k);
}
U(_i,_k) = a;
}
}
};
/*
* 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 )
{
/* 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
*/
data.U = data.M;
for( int j=model.nbody-1;j>=0;--j )
{
......@@ -206,6 +85,8 @@ namespace se3
CholeskyLoopKStep::ArgsType(model,data,j));
}
data.D = data.U.diagonal();
data.U.diagonal().fill(1);
return data.U;
}
......
src/multibody/joint/joint-variant.hpp
+ 3
− 0
View file @ bafef1e9
......@@ -33,6 +33,7 @@ namespace se3
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_nv(), jmodel ); }
};
inline int nv(const JointModelVariant & jmodel) { return Joint_nv::run(jmodel); }
class Joint_nq: public boost::static_visitor<int>
{
......@@ -66,6 +67,8 @@ namespace se3
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( Joint_idx_v(), jmodel ); }
};
inline int idx_v(const JointModelVariant & jmodel) { return Joint_idx_v::run(jmodel); }
} // namespace se3
......
unittest/cholesky.cpp
+ 37
− 7
View file @ bafef1e9
......@@ -4,7 +4,6 @@
#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"
......@@ -39,13 +38,25 @@ int main(int argc, const char ** argv)
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("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("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg1");
model.addBody(model.getBodyId("lleg1"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg2");
model.addBody(model.getBodyId("lleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg3");
model.addBody(model.getBodyId("lleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg4");
model.addBody(model.getBodyId("lleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg5");
model.addBody(model.getBodyId("lleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"lleg6");
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("rleg2"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg3");
model.addBody(model.getBodyId("rleg3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg4");
model.addBody(model.getBodyId("rleg4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg5");
model.addBody(model.getBodyId("rleg5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rleg6");
model.addBody(model.getBodyId("root"),JointModelRX(),SE3::Random(),Inertia::Random(),"torso1");
model.addBody(model.getBodyId("torso1"),JointModelRX(),SE3::Random(),Inertia::Random(),"chest");
......@@ -53,27 +64,46 @@ int main(int argc, const char ** argv)
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("rarm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm4");
model.addBody(model.getBodyId("rarm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm5");
model.addBody(model.getBodyId("rarm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"rarm6");
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");
model.addBody(model.getBodyId("larm3"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm4");
model.addBody(model.getBodyId("larm4"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm5");
model.addBody(model.getBodyId("larm5"),JointModelRX(),SE3::Random(),Inertia::Random(),"larm6");
std::cout << model << std::endl;
// std::cout << model << std::endl;
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
crba2(model,data,q);
crba(model,data,q);
std::cout << "M = [\n" << data.M << "];" << std::endl;
//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;
//std::cout << "M = [\n" << data.M << "];" << std::endl;
//cholesky(model,data)
StackTicToc timer(StackTicToc::US); timer.tic();
#ifdef NDEBUG
SMOOTH(1000)
#endif
{
cholesky(model,data);
}
timer.toc(std::cout,1000);
//std::cout << "U = [\n" << data.U << "];" << std::endl;
//std::cout << "D = [\n" << data.D << "];" << std::endl;
//std::cout << "UDU = [\n" << (data.U*data.D.asDiagonal()*data.U.transpose()) << "];" << std::endl;
assert((data.U*data.D.asDiagonal()*data.U.transpose()).isApprox(data.M));
return 0;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment