-
Florent Lamiraux authoredFlorent Lamiraux authored
matrix-inertia.cpp 9.58 KiB
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
* This file is part of sot-dynamic.
* sot-dynamic is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-dynamic is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
#include <fstream>
#include <vector>
#include <map>
#include <sot-dynamic/matrix-inertia.h>
#include <jrl/dynamics/Joint.h>
#include <jrl/dynamics/HumanoidDynamicMultiBody.h>
#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
#include <sot/core/debug.hh>
using namespace dynamicsJRLJapan;
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
static matrix3d skewSymmetric(const vector3d& v)
{
matrix3d res;
res(0, 0) = 0.0;
res(1, 1) = 0.0;
res(2, 2) = 0.0;
res(0, 1) = -v[2];
res(0, 2) = v[1];
res(1, 0) = v[2];
res(1, 2) = -v[0];
res(2, 0) = -v[1];
res(2, 1) = v[0];
return res;
}
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
MatrixInertia::
MatrixInertia( CjrlHumanoidDynamicRobot* aHDR )
:aHDR_( aHDR )
,aHDMB_( 0x0 )
{
sotDEBUGIN(25);
if( aHDR!=NULL ) init( aHDR );
sotDEBUGOUT(25);
}
void MatrixInertia::
init( CjrlHumanoidDynamicRobot* aHDR )
{
sotDEBUGIN(25);
aHDR_ = aHDR;
aHDMB_ = dynamic_cast<dynamicsJRLJapan::HumanoidDynamicMultiBody*>(aHDR_);
/* STEP 2: get the joints and resize internal vectors according to
* number of joints. */
joints_ = aHDMB_->jointVector();
sotDEBUG(25) << "Joints:" << joints_.size() << endl;
parentIndex_.resize(joints_.size());
inertia_.resize(joints_.size() + 5, joints_.size() + 5);
phi.resize( joints_.size() );
iVpi.resize( joints_.size() );
iVpiT.resize( joints_.size() );
Ic.resize( joints_.size() );
/* STEP 3: create the index of parents. */
initParents();
/* STEP 4: initialize phi (dof table) for each joint. */
initDofTable();
sotDEBUGOUT(25);
}
void MatrixInertia::
initParents( void )
{
sotDEBUGIN(25);
std::map<CjrlJoint*, int> m;
for(size_t i = 0; i < joints_.size(); ++i)
m[joints_[i]] = i;
sotDEBUG(15) << "Parent Indexes:" << std::endl;
for(size_t i = 0; i < joints_.size(); ++i)
{
if(joints_[i]->parentJoint() == 0x0)
{
parentIndex_[i] = -1;
sotDEBUG(15) << "parent of\t" << i << "\t("
<< static_cast<Joint*>(joints_[i])->getName()
<< "):\t" << -1 << std::endl;
}
else
{
parentIndex_[i] = m[joints_[i]->parentJoint()];
sotDEBUG(15) << "parent of\t" << i << ":\t("
<< static_cast<Joint*>(joints_[i])->getName()
<< "):\t" << m[joints_[i]->parentJoint()]
<< "\t(" << static_cast<Joint*>
(joints_[m[joints_[i]->parentJoint()]])->getName()
<< ")" << std::endl;
}
}
sotDEBUGOUT(25);
}
void MatrixInertia::
initDofTable( void )
{
sotDEBUGIN(25);
for(size_t i = 0; i < joints_.size(); ++i)
{
Joint* j = static_cast<Joint*>(joints_[i]);
vector3d z = j->axe();
ml::Vector & phi_i = phi[i];
phi_i.resize(6); phi_i.fill(0.);
for( int n=0;n<3;++n ) phi_i(n+3) = z(n); sotDEBUG(25) << phi_i <<endl;
}
sotDEBUGOUT(25);
}
MatrixInertia::~MatrixInertia()
{}
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
void MatrixInertia::update( void )
{
sotDEBUGIN(25);
const vectorN & currentConf = aHDMB_->currentConfiguration();
sotDEBUG(45) << "q = [ " << currentConf << endl;
sotDEBUG(15) << "Spatial transforms:" << std::endl;
const size_t SIZE = joints_.size();
for(size_t i = 0; i < SIZE; ++i)
{
sotDEBUG(25) << "Joint " << i << ": rank = "
<< joints_[i]->rankInConfiguration() <<endl;
Joint* j = static_cast<Joint*>(joints_[i]);
/* iRpi: Rotation from joint i to parent of i. */
MatrixRotation piRi;
{
/* phi_[1] = phi_(1:3) is the rotation axis of the joint. */
matrix3d Ri; j->getStaticRotation(Ri);
vector3d axeJ = Ri*j->axe();
const double & q = currentConf(joints_[i]->rankInConfiguration());
sotDEBUG(45) << "q"<<i<<" = " <<q <<endl;
matrix3d piRi_tmp;
j->RodriguesRotation(axeJ,q,piRi_tmp);
for( unsigned int loopi=0;loopi<3;++loopi )
for( unsigned int loopj=0;loopj<3;++loopj )
piRi( loopi,loopj ) = piRi_tmp(loopi,loopj);
}
ml::Vector piTi(3);
{
vector3d piTi_tmp;
j->getStaticTranslation( piTi_tmp );
for( unsigned int loopi=0;loopi<3;++loopi ) piTi(loopi) = piTi_tmp(loopi);
}
/* Twist matrix iXpi: transfo of the velocities between i en pi (parent i).
* iXpi = [ iRpi Skew( iRpi.t )*iRpi ]
* [ 0 iRpi ] */
MatrixHomogeneous piMi; piMi.buildFrom( piRi,piTi );
MatrixHomogeneous iMpi; piMi.inverse(iMpi);
sotDEBUG(45) << "piMi = " <<piMi <<endl;
sotDEBUG(45) << "iMpi = " <<iMpi <<endl;
iVpi[i].buildFrom( iMpi );
iVpi[i].transpose( iVpiT[i] );
sotDEBUG(25) << "iVpi" << i << " = " <<iVpi[i] <<endl;
}
sotDEBUGOUT(25);
}
void MatrixInertia::computeInertiaMatrix()
{
sotDEBUGIN(25);
inertia_.fill(0.0);
const size_t SIZE = joints_.size();
/* Compute the local 6D inertia matrices. */
for( size_t i = 0;i<SIZE;++i )
{ /* Position of the mass in the joint frame. */
vector3d com = joints_[i]->linkedBody()->localCenterOfMass();
matrix3d Sc = skewSymmetric(com);
/* Inertia of the link. */
matrix3d Icm = joints_[i]->linkedBody()->inertiaMatrix();
double m = joints_[i]->linkedBody()->mass();
/* Ic_ is the inertia 6D matrix of the joint in the joint frame.
* Ic = [ Ai+mSc.Sc' mSc ]
* [ mSc' mId ]
*/
sotDEBUG(45) << "com"<<i<<" = [ " << com <<"]"<<endl;
sotDEBUG(45) << "Sc"<<i<<" = [ " << Sc<<"]"<<endl;
sotDEBUG(45) << "Icm"<<i<<" = [ " << Icm<<"]"<<endl;
matrix3d Sct = Sc.Transpose();
matrix3d Irr = Sc*Sct;
ml::Matrix & Ici = Ic[i]; Ici.resize(6,6);
for( unsigned int loopi=0;loopi<3;++loopi )
for( unsigned int loopj=0;loopj<3;++loopj )
{
/*TT*/if( loopi==loopj ) Ici( loopi,loopj ) = m;
else Ici( loopi,loopj ) = 0.;
/*TR*/Ici( loopi,loopj+3 ) = m*Sct( loopi,loopj );
/*RT*/Ici( loopi+3,loopj ) = m*Sc( loopi,loopj );
/*RR*/Ici( loopi+3,loopj+3 ) = m*Irr( loopi,loopj )+ Icm( loopi,loopj );
}
sotDEBUG(25) << "Ic" << i << " = " << Ici;
}
ml::Vector Fi(6);
ml::Matrix iVpiT_Ici(6,6);
ml::Matrix iVpiT_Ici_iVpi(6,6);
for( int i=SIZE-1;i>=1;--i )
{
const unsigned int iRank = joints_[i]->rankInConfiguration();
ml::Matrix & Ici = Ic[i];
sotMatrixTwist & iVpii = iVpi[i];
MatrixForce & iVpiiT = iVpiT[i];
ml::Vector & phii = phi[i];
/* F = Ic_i . phi_i */
Fi = Ici*phii;
/* H_ii = phi_i' . F */
/*DEBUGinertia_(i + 5, i + 5) = phii.scalarProduct(Fi);*/
inertia_(iRank,iRank) = phii.scalarProduct(Fi);
sotDEBUG(30) << "phi"<<i<<" = " << phii;
sotDEBUG(35) << "Fi"<<i<<" = " << Fi << endl;
sotDEBUG(25) << "IcA"<<i<<" = " << Ici << endl;
sotDEBUG(45) << "Joint " << i << " in " << iRank <<endl;
/* Ic_pi = Ic_pi + iXpi' Ic_i iXpi */
iVpiiT.multiply(Ici,iVpiT_Ici);
iVpiT_Ici.multiply( iVpii,iVpiT_Ici_iVpi );
sotDEBUG(45) << "Icpi"<<parentIndex_[i]<<" = " << Ic[parentIndex_[i]] ;
Ic[ parentIndex_[i] ] += iVpiT_Ici_iVpi;
sotDEBUG(45) << "Icpi"<<parentIndex_[i]<<"_"<<i<<" = " << Ic[parentIndex_[i]] ;
sotDEBUG(45) << "Vpi"<<i<<" = " << iVpii ;
size_t j = i;
while(parentIndex_[j] != 0)
{
/* F = jXpj' . F */ Fi = iVpiT[j]*Fi;
/* j = pj */
j = parentIndex_[j];
/* Hij = Hji = F' phi_j */
inertia_(iRank,joints_[j]->rankInConfiguration())
= inertia_(joints_[j]->rankInConfiguration(),iRank)
= Fi.scalarProduct( phi[j]);
sotDEBUG(35) << "Fi = " << Fi << endl;
sotDEBUG(35) << "FiXphi = " << inertia_(j + 5, i + 5) << endl;
}
/* When parentIndex_[j] == 0: FREE FLYER. */
Fi = iVpiT[j]*Fi;
for(size_t k = 0; k < 6; ++k)
{
inertia_(iRank, k) = inertia_(k,iRank) = Fi(k);
}
}
/* --- FREE FLYER = Ic0 --- */
const ml::Matrix & Ic0 = Ic[0];
for(size_t i = 0; i < 6; ++i)
for(size_t j = 0; j < 6; ++j)
{inertia_(i, j) = Ic0(i,j);}
sotDEBUGOUT(25);
}
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
void MatrixInertia::
getInertiaMatrix(double* A)
{
for(size_t i = 0; i < (joints_.size() + 5); ++i)
for(size_t j = 0; j < (joints_.size() + 5); ++j)
A[i * (joints_.size() + 5) + j] = inertia_(i, j);
}
const maal::boost::Matrix& MatrixInertia::
getInertiaMatrix( void )
{ return inertia_; }