Skip to content
GitLab
Commit a2c81f46 authored by Florian Valenza's avatar Florian Valenza
Browse files

[Major] Switching Model::Index from int to std::size_t

parent 7ae3b0f9
Hide whitespace changes
Inline Side-by-side
src/algorithm/center-of-mass.hpp
View file @ a2c81f46
......@@ -55,8 +55,8 @@ namespace se3
using namespace Eigen;
using namespace se3;
const std::size_t & i = jmodel.id();
const std::size_t & parent = model.parents[i];
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -80,13 +80,13 @@ namespace se3
data.mass[0] = 0;
data.com[0].setZero ();
for( int i=1;i<model.nbody;++i )
for( Model::Index i=1;i<(Model::Index)(model.nbody);++i )
{
data.com[i] = model.inertias[i].mass()*model.inertias[i].lever();
data.mass[i] = model.inertias[i].mass();
}
for( int i=model.nbody-1;i>0;--i )
for( Model::Index i=(Model::Index)(model.nbody-1);i>0;--i )
{
CenterOfMassForwardStep
::run(model.joints[i],data.joints[i],
......@@ -125,7 +125,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -160,7 +160,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
data.com[parent] += data.com[i];
......@@ -192,13 +192,13 @@ namespace se3
{
data.com[0].setZero ();
data.mass[0] = 0;
for( int i=1;i<model.nbody;++i )
for( Model::Index i=1;i<(Model::Index)model.nbody;++i )
{
JacobianCenterOfMassForwardStep
::run(model.joints[i],data.joints[i],
JacobianCenterOfMassForwardStep::ArgsType(model,data,q));
}
for( int i=model.nbody-1;i>0;--i )
for( Model::Index i= (Model::Index) (model.nbody-1);i>0;--i )
{
JacobianCenterOfMassBackwardStep
::run(model.joints[i],data.joints[i],
......
src/algorithm/cholesky.hpp
View file @ a2c81f46
......@@ -59,7 +59,7 @@ namespace se3
for(int j=model.nv-1;j>=0;--j )
{
const int NVT = data.nvSubtree_fromRow[j]-1;
const int NVT = data.nvSubtree_fromRow[(Model::Index)j]-1;
Eigen::VectorXd::SegmentReturnType DUt = data.tmp.head(NVT);
if(NVT)
DUt = U.row(j).segment(j+1,NVT).transpose()
......@@ -67,7 +67,7 @@ namespace se3
D[j] = M(j,j) - U.row(j).segment(j+1,NVT) * DUt;
for( int _i=data.parents_fromRow[j];_i>=0;_i=data.parents_fromRow[_i] )
for( int _i=data.parents_fromRow[(Model::Index)j];_i>=0;_i=data.parents_fromRow[(Model::Index)_i] )
U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) / D[j];
}
......@@ -88,7 +88,7 @@ namespace se3
const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv
v[k] += U.row(k).segment(k+1,nvt[k]-1) * v.segment(k+1,nvt[k]-1);
v[k] += U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1);
return v.derived();
}
......@@ -106,7 +106,7 @@ namespace se3
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int i=model.nv-2;i>=0;--i ) // You can start from nv-2 (no child in nv-1)
v.segment(i+1,nvt[i]-1) += U.row(i).segment(i+1,nvt[i]-1).transpose()*v[i];
v.segment(i+1,nvt[(Model::Index)i]-1) += U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i];
return v.derived();
}
......@@ -129,7 +129,7 @@ namespace se3
const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1)
v[k] -= U.row(k).segment(k+1,nvt[k]-1) * v.segment(k+1,nvt[k]-1);
v[k] -= U.row(k).segment(k+1,nvt[(Model::Index)k]-1) * v.segment(k+1,nvt[(Model::Index)k]-1);
return v.derived();
}
......@@ -148,7 +148,7 @@ namespace se3
const Eigen::MatrixXd & U = data.U;
const std::vector<int> & nvt = data.nvSubtree_fromRow;
for( int i=0;i<model.nv-1;++i ) // You can stop one step before nv.
v.segment(i+1,nvt[i]-1) -= U.row(i).segment(i+1,nvt[i]-1).transpose()*v[i];
v.segment(i+1,nvt[(Model::Index)i]-1) -= U.row(i).segment(i+1,nvt[(Model::Index)i]-1).transpose()*v[i];
return v.derived();
}
......@@ -170,8 +170,8 @@ namespace se3
for( int k=model.nv-1;k>=0;--k )
{
res[k] = M.row(k).segment(k,nvt[k]) * v.segment(k,nvt[k]);
res.segment(k+1,nvt[k]-1) += M.row(k).segment(k+1,nvt[k]-1).transpose()*v[k];
res[k] = M.row(k).segment(k,nvt[(Model::Index)k]) * v.segment(k,nvt[(Model::Index)k]);
res.segment(k+1,nvt[(Model::Index)k]-1) += M.row(k).segment(k+1,nvt[(Model::Index)k]-1).transpose()*v[k];
}
return res;
......
src/algorithm/crba.hpp
View file @ a2c81f46
......@@ -37,7 +37,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const typename JointModel::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
......@@ -66,7 +66,7 @@ namespace se3
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
const Model::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
/* F[1:6,i] = Y*S */
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
......@@ -100,13 +100,13 @@ namespace se3
crba(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( int i=1;i<model.nbody;++i )
for( Model::Index i=1;i<(Model::Index)(model.nbody);++i )
{
CrbaForwardStep::run(model.joints[i],data.joints[i],
CrbaForwardStep::ArgsType(model,data,q));
}
for( int i=model.nbody-1;i>0;--i )
for( Model::Index i=(Model::Index)(model.nbody-1);i>0;--i )
{
CrbaBackwardStep::run(model.joints[i],data.joints[i],
CrbaBackwardStep::ArgsType(model,data));
......
src/algorithm/jacobian.hpp
View file @ a2c81f46
......@@ -36,7 +36,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -55,7 +55,7 @@ namespace se3
computeJacobians(const Model & model, Data& data,
const Eigen::VectorXd & q)
{
for( int i=1;i<model.nbody;++i )
for( Model::Index i=1; i< (Model::Index) model.nbody;++i )
{
JacobiansForwardStep::run(model.joints[i],data.joints[i],
JacobiansForwardStep::ArgsType(model,data,q));
......@@ -76,7 +76,7 @@ namespace se3
const SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
for(int j=colRef;j>=0;j=data.parents_fromRow[j])
for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j])
{
if(! localFrame ) J.col(j) = data.J.col(j);
else J.col(j) = oMjoint.actInv(Motion(data.J.col(j))).toVector();
......@@ -103,7 +103,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
const Model::Index & i = jmodel.id();
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q);
......@@ -123,7 +123,7 @@ namespace se3
const Model::Index & idx )
{
data.iMf[idx] = SE3::Identity();
for( int i=idx;i>0;i=model.parents[i] )
for( Model::Index i=idx;i>0;i=model.parents[i] )
{
JacobianForwardStep::run(model.joints[i],data.joints[i],
JacobianForwardStep::ArgsType(model,data,q));
......
src/algorithm/kinematics.hpp
View file @ a2c81f46
......@@ -46,7 +46,7 @@ namespace se3
data.liMi[i] = model.jointPlacements[i] * jdata.M ();
if (parent>0)
data.oMi[i] = data.oMi[(size_t) parent] * data.liMi[i];
data.oMi[i] = data.oMi[parent] * data.liMi[i];
else
data.oMi[i] = data.liMi[i];
}
......@@ -98,8 +98,8 @@ namespace se3
if(parent>0)
{
data.oMi[i] = data.oMi[(size_t) parent]*data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[(size_t) parent]);
data.oMi[i] = data.oMi[parent]*data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[parent]);
}
else
data.oMi[i] = data.liMi[i];
......
src/algorithm/rnea.hpp
View file @ a2c81f46
......@@ -44,16 +44,16 @@ namespace se3
jmodel.calc(jdata.derived(),q,v);
const Model::Index & parent = model.parents[i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
const Model::Index & parent = model.parents[(Model::Index)i];
data.liMi[(Model::Index)i] = model.jointPlacements[(Model::Index)i]*jdata.M();
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
data.v[(Model::Index)i] = jdata.v();
if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.a[(Model::Index)i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ;
data.a[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
return 0;
}
......@@ -74,9 +74,9 @@ namespace se3
Data& data,
int i)
{
const Model::Index & parent = model.parents[i];
jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[i];
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
const Model::Index & parent = model.parents[(Model::Index)i];
jmodel.jointForce(data.tau) = jdata.S().transpose()*data.f[(Model::Index)i];
if(parent>0) data.f[(Model::Index)parent] += data.liMi[(Model::Index)i].act(data.f[(Model::Index)i]);
}
};
......@@ -91,13 +91,13 @@ namespace se3
for( int i=1;i<model.nbody;++i )
{
RneaForwardStep::run(model.joints[i],data.joints[i],
RneaForwardStep::run(model.joints[(Model::Index)i],data.joints[(Model::Index)i],
RneaForwardStep::ArgsType(model,data,i,q,v,a));
}
for( int i=model.nbody-1;i>0;--i )
{
RneaBackwardStep::run(model.joints[i],data.joints[i],
RneaBackwardStep::run(model.joints[(Model::Index)i],data.joints[(Model::Index)i],
RneaBackwardStep::ArgsType(model,data,i));
}
......
src/multibody/model.hpp
View file @ a2c81f46
......@@ -18,13 +18,13 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
namespace se3
{
struct Model;
struct Data;
class Model;
class Data;
class Model
{
public:
typedef int Index;
typedef std::size_t Index;
int nq; // Dimension of the configuration representation
int nv; // Dimension of the velocity vector space
......@@ -101,7 +101,7 @@ namespace se3
std::vector<Matrix6x> Fcrb; // Spatial forces set, used in CRBA
std::vector<Model::Index> lastChild; // Index of the last child (for CRBA)
std::vector<int> lastChild; // Index of the last child (for CRBA)
std::vector<int> nvSubtree; // Dimension of the subtree motion space (for CRBA)
Eigen::MatrixXd U; // Joint Inertia square root (upper triangle)
......@@ -138,7 +138,7 @@ namespace se3
std::ostream& operator<< ( std::ostream & os, const Model& model )
{
os << "Nb bodies = " << model.nbody << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl;
for(int i=0;i<model.nbody;++i)
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
{
os << " Joint "<<model.names[i] << ": parent=" << model.parents[i]
<< ( model.hasVisual[i] ? " (has visual) " : "(doesnt have visual)" ) << std::endl;
......@@ -156,7 +156,7 @@ namespace se3
"abcdefghijklmnopqrstuvwxyz";
for (int i=0; i<len;++i)
res += alphanum[std::rand() % (sizeof(alphanum) - 1)];
res += alphanum[(std::rand() % (sizeof(alphanum) - 1))];
return res;
}
......@@ -169,10 +169,10 @@ namespace se3
&&(nbody==(int)parents.size())&&(nbody==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
Index idx = nbody ++;
Model::Index idx = (Model::Index) (nbody ++);
joints .push_back(j.derived());
boost::get<D&>(joints.back()).setIndexes(idx,nq,nv);
boost::get<D&>(joints.back()).setIndexes((int)idx,nq,nv);
inertias .push_back(Y);
parents .push_back(parent);
......@@ -191,7 +191,7 @@ namespace se3
bool visual )
{
Index idx = nFixBody++;
Model::Index idx = (Model::Index) (nFixBody++);
fix_lastMovingParent.push_back(lastMovingParent);
fix_lmpMi .push_back(placementFromLastMoving);
fix_hasVisual .push_back(visual);
......@@ -211,7 +211,7 @@ namespace se3
res = std::find(names.begin(),names.end(),name) - names.begin();
assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
assert( (res>=0)&&(res<nbody)&&"The body name you asked do not exist" );
return int(res);
return Model::Index(res);
}
bool Model::existBodyName( const std::string & name ) const
{
......@@ -222,42 +222,42 @@ namespace se3
const std::string& Model::getBodyName( Model::Index index ) const
{
assert( (index>=0)&&(index<nbody) );
assert( (index>=0)&&(index < (Model::Index)nbody) );
return names[index];
}
Data::Data( const Model& ref )
:model(ref)
,joints(0)
,a(ref.nbody)
,v(ref.nbody)
,f(ref.nbody)
,oMi(ref.nbody)
,liMi(ref.nbody)
,a((std::size_t)ref.nbody)
,v((std::size_t)ref.nbody)
,f((std::size_t)ref.nbody)
,oMi((std::size_t)ref.nbody)
,liMi((std::size_t)ref.nbody)
,tau(ref.nv)
,nle(ref.nv)
,Ycrb(ref.nbody)
,Ycrb((std::size_t)ref.nbody)
,M(ref.nv,ref.nv)
,Fcrb(ref.nbody)
,lastChild(ref.nbody)
,nvSubtree(ref.nbody)
,Fcrb((std::size_t)ref.nbody)
,lastChild((std::size_t)ref.nbody)
,nvSubtree((std::size_t)ref.nbody)
,U(ref.nv,ref.nv)
,D(ref.nv)
,tmp(ref.nv)
,parents_fromRow(ref.nv)
,nvSubtree_fromRow(ref.nv)
,parents_fromRow((std::size_t)ref.nv)
,nvSubtree_fromRow((std::size_t)ref.nv)
,J(6,ref.nv)
,iMf(ref.nbody)
,com(ref.nbody)
,mass(ref.nbody)
,iMf((std::size_t)ref.nbody)
,com((std::size_t)ref.nbody)
,mass((std::size_t)ref.nbody)
,Jcom(3,ref.nv)
{
for(int i=0;i<model.nbody;++i)
for(Model::Index i=0;i<(Model::Index)(model.nbody);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
/* Init for CRBA */
M.fill(NAN);
for(int i=0;i<ref.nbody;++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
computeLastChild(ref);
/* Init for Cholesky */
......@@ -274,32 +274,32 @@ namespace se3
std::fill(lastChild.begin(),lastChild.end(),-1);
for( int i=model.nbody-1;i>=0;--i )
{
if(lastChild[i] == -1) lastChild[i] = i;
const Index & parent = model.parents[i];
lastChild[parent] = std::max(lastChild[i],lastChild[parent]);
if(lastChild[(Model::Index)i] == -1) lastChild[(Model::Index)i] = i;
const Index & parent = model.parents[(Model::Index)i];
lastChild[parent] = std::max(lastChild[(Model::Index)i],lastChild[parent]);
nvSubtree[i]
= idx_v(model.joints[lastChild[i]]) + nv(model.joints[lastChild[i]])
- idx_v(model.joints[i]);
nvSubtree[(Model::Index)i]
= idx_v(model.joints[(Model::Index)lastChild[(Model::Index)i]]) + nv(model.joints[(Model::Index)lastChild[(Model::Index)i]])
- idx_v(model.joints[(Model::Index)i]);
}
}
void Data::computeParents_fromRow( const Model& model )
{
for( int joint=1;joint<model.nbody;joint++)
for( Model::Index joint=1;joint<(Model::Index)(model.nbody);joint++)
{
const Model::Index & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
const int idx_vj = idx_v(model.joints[joint]);
if(parent>0) parents_fromRow[idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[idx_vj] = -1;
nvSubtree_fromRow[idx_vj] = nvSubtree[joint];
if(parent>0) parents_fromRow[(Model::Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[(Model::Index)idx_vj] = -1;
nvSubtree_fromRow[(Model::Index)idx_vj] = nvSubtree[joint];
for( int row=1;row<nvj;++row)
{
parents_fromRow[idx_vj+row] = idx_vj+row-1;
nvSubtree_fromRow[idx_vj+row] = nvSubtree[joint]-row;
parents_fromRow[(Model::Index)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(Model::Index)(idx_vj+row)] = nvSubtree[joint]-row;
}
}
}
......
src/python/data.hpp
View file @ a2c81f46
......@@ -60,7 +60,7 @@ namespace se3
.ADD_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia")
.ADD_DATA_PROPERTY_CONST(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
.ADD_DATA_PROPERTY(std::vector<Model::Index>,lastChild,"Index of the last child (for CRBA)")
.ADD_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
.ADD_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
......@@ -84,7 +84,7 @@ namespace se3
IMPL_DATA_PROPERTY(std::vector<Inertia>,Ycrb,"Inertia of the sub-tree composit rigid body")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,M,"Joint Inertia")
IMPL_DATA_PROPERTY_CONST(std::vector<Matrix6x>,Fcrb,"Spatial forces set, used in CRBA")
IMPL_DATA_PROPERTY(std::vector<Model::Index>,lastChild,"Index of the last child (for CRBA)")
IMPL_DATA_PROPERTY(std::vector<int>,lastChild,"Index of the last child (for CRBA)")
IMPL_DATA_PROPERTY(std::vector<int>,nvSubtree,"Dimension of the subtree motion space (for CRBA)")
IMPL_DATA_PROPERTY_CONST(Eigen::MatrixXd,U,"Joint Inertia square root (upper triangle)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,D,"Diagonal of UDUT inertia decomposition")
......
src/python/eigen_container.hpp
View file @ a2c81f46
......@@ -39,7 +39,7 @@ namespace se3
assert( Ys.size()<INT_MAX );
if( i<0 ) i = int(Ys.size())+i;
assert( (i>=0) && (i<int(Ys.size())) );
return Ys[i];
return Ys[(std::size_t)i];
}
static void setItem( stdVectorAligned & Ys,
......@@ -48,7 +48,7 @@ namespace se3
assert( Ys.size()<INT_MAX );
if( i<0 ) i = int(Ys.size())+i;
assert( (i>=0) && (i<int(Ys.size())) );
Ys[i] = Y;
Ys[(std::size_t)i] = Y;
}
static typename stdVectorAligned::size_type length( const stdVectorAligned & Ys )
{ return Ys.size(); }
......
src/simulation/compute-all-terms.hpp
View file @ a2c81f46
......@@ -41,33 +41,33 @@ namespace se3
using namespace Eigen;
using namespace se3;
const typename JointModel::Index & i = jmodel.id();
const Model::Index & parent = model.parents[(size_t) i];
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
jmodel.calc(jdata.derived(),q,v);
// CRBA
data.liMi[(size_t) i] = model.jointPlacements[(size_t) i]*jdata.M();
data.Ycrb[(size_t) i] = model.inertias[(size_t) i];
data.liMi[i] = model.jointPlacements[i]*jdata.M();
data.Ycrb[i] = model.inertias[i];
// Jacobian + NLE
data.v[(size_t) i] = jdata.v();
data.v[i] = jdata.v();
if(parent>0)
{
data.oMi[(size_t) i] = data.oMi[(size_t) parent]*data.liMi[(size_t) i];
data.v[(size_t) i] += data.liMi[(size_t) i].actInv(data.v[(size_t) parent]);
data.oMi[i] = data.oMi[parent]*data.liMi[i];
data.v[i] += data.liMi[i].actInv(data.v[parent]);
}
else
{
data.oMi[(size_t) i] = data.liMi[(size_t) i];
data.oMi[i] = data.liMi[i];
}
data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[(size_t) i].act(jdata.S());
data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
data.a[(size_t) i] = jdata.c() + (data.v[(size_t) i] ^ jdata.v());
data.a[(size_t) i] += data.liMi[(size_t) i].actInv(data.a[(size_t) parent]);
data.a[i] = jdata.c() + (data.v[i] ^ jdata.v());
data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[(size_t) i] = model.inertias[(size_t) i]*data.a[(size_t) i] + model.inertias[(size_t) i].vxiv(data.v[(size_t) i]); // -f_ext
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
}
};
......@@ -92,31 +92,31 @@ namespace se3
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
const Model::Index & i = jmodel.id();
const Model::Index & parent = model.parents[(size_t) i];
const Model::Index & i = (Model::Index) jmodel.id();
const Model::Index & parent = model.parents[i];
/* F[1:6,i] = Y*S */
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[(size_t) i] * jdata.S();
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
/* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[(size_t) i])
= jdata.S().transpose()*data.Fcrb[(size_t) i].block(0,jmodel.idx_v(),6,data.nvSubtree[(size_t) i]);
data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
= jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);