From a2c81f468f1c3ab6ed28ef545ea7b310349e7623 Mon Sep 17 00:00:00 2001
From: Florian Valenza <florian.valenza@laas.fr>
Date: Wed, 22 Apr 2015 13:43:23 +0200
Subject: [PATCH] [Major] Switching Model::Index from int to std::size_t
---
src/algorithm/center-of-mass.hpp | 16 +++---
src/algorithm/cholesky.hpp | 16 +++---
src/algorithm/crba.hpp | 8 +--
src/algorithm/jacobian.hpp | 10 ++--
src/algorithm/kinematics.hpp | 6 +--
src/algorithm/rnea.hpp | 24 ++++-----
src/multibody/model.hpp | 78 ++++++++++++++--------------
src/python/data.hpp | 4 +-
src/python/eigen_container.hpp | 4 +-
src/simulation/compute-all-terms.hpp | 50 +++++++++---------
unittest/cholesky.cpp | 12 ++---
unittest/jacobian.cpp | 12 ++---
unittest/joints.cpp | 7 +--
unittest/symmetric.cpp | 8 +--
14 files changed, 125 insertions(+), 130 deletions(-)
diff --git a/src/algorithm/center-of-mass.hpp b/src/algorithm/center-of-mass.hpp
index 35e9efff7..a85383e81 100644
--- a/src/algorithm/center-of-mass.hpp
+++ b/src/algorithm/center-of-mass.hpp
@@ -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],
diff --git a/src/algorithm/cholesky.hpp b/src/algorithm/cholesky.hpp
index b8fa5008e..7814967ca 100644
--- a/src/algorithm/cholesky.hpp
+++ b/src/algorithm/cholesky.hpp
@@ -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;
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index 9af712354..4d6516f4a 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -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));
diff --git a/src/algorithm/jacobian.hpp b/src/algorithm/jacobian.hpp
index 8fd91625f..e0c21aa32 100644
--- a/src/algorithm/jacobian.hpp
+++ b/src/algorithm/jacobian.hpp
@@ -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));
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 878fefca8..d6443ce0d 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -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];
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index 630e8a893..06d5834c3 100644
--- a/src/algorithm/rnea.hpp
+++ b/src/algorithm/rnea.hpp
@@ -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));
}
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index dc0b7a69f..c7954fbf0 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -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;
}
}
}
diff --git a/src/python/data.hpp b/src/python/data.hpp
index 296c2c2c6..20fd16f16 100644
--- a/src/python/data.hpp
+++ b/src/python/data.hpp
@@ -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")
diff --git a/src/python/eigen_container.hpp b/src/python/eigen_container.hpp
index 39948bdcd..f7c0a8e1d 100644
--- a/src/python/eigen_container.hpp
+++ b/src/python/eigen_container.hpp
@@ -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(); }
diff --git a/src/simulation/compute-all-terms.hpp b/src/simulation/compute-all-terms.hpp
index 9880578bd..7b88ffdb5 100644
--- a/src/simulation/compute-all-terms.hpp
+++ b/src/simulation/compute-all-terms.hpp
@@ -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]);
- jmodel.jointForce(data.nle) = jdata.S().transpose()*data.f[(size_t) i];
+ jmodel.jointForce(data.nle) = jdata.S().transpose()*data.f[i];
if(parent>0)
{
/* Yli += liXi Yi */
- data.Ycrb[(size_t) parent] += data.liMi[(size_t) i].act(data.Ycrb[(size_t) i]);
+ data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
- = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[(size_t) i]);
- forceSet::se3Action(data.liMi[(size_t) i],
- data.Fcrb[(size_t) i].block(0,jmodel.idx_v(),6,data.nvSubtree[(size_t) i]),
+ = data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
+ forceSet::se3Action(data.liMi[i],
+ data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
jF);
- data.f[(size_t) parent] += data.liMi[(size_t) i].act(data.f[(size_t) i]);
+ data.f[parent] += data.liMi[i].act(data.f[i]);
}
}
};
@@ -130,13 +130,13 @@ namespace se3
data.v[0].setZero ();
data.a[0] = -model.gravity;
- for(size_t i=1;i<(size_t) model.nbody;++i)
+ for(Model::Index i=1;i<(Model::Index) model.nbody;++i)
{
CATForwardStep::run(model.joints[i],data.joints[i],
CATForwardStep::ArgsType(model,data,q,v));
}
- for(size_t i=(size_t)(model.nbody-1);i>0;--i)
+ for(Model::Index i=(Model::Index)(model.nbody-1);i>0;--i)
{
CATBackwardStep::run(model.joints[i],data.joints[i],
CATBackwardStep::ArgsType(model,data));
diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp
index 8a5ae53e9..9e35eaad2 100644
--- a/unittest/cholesky.cpp
+++ b/unittest/cholesky.cpp
@@ -147,7 +147,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 2 & 31 )
{
std::vector<Eigen::VectorXd> randvec(NBT);
- for(int i=0;i<NBT;++i ) randvec[i] = Eigen::VectorXd::Random(model.nv);
+ for(int i=0;i<NBT;++i ) randvec[(std::size_t)i] = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd zero = Eigen::VectorXd(model.nv);
Eigen::VectorXd res (model.nv);
@@ -157,7 +157,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
- se3::cholesky::solve(model,data,randvec[_smooth]);
+ se3::cholesky::solve(model,data,randvec[(std::size_t)_smooth]);
}
if(verbose) std::cout << "solve =\t\t";
timer.toc(std::cout,NBT);
@@ -168,7 +168,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
- se3::cholesky::Uv(model,data,randvec[_smooth]);
+ se3::cholesky::Uv(model,data,randvec[(std::size_t)_smooth]);
}
if(verbose) std::cout << "Uv =\t\t";
timer.toc(std::cout,NBT);
@@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
- se3::cholesky::Uiv(model,data,randvec[_smooth]);
+ se3::cholesky::Uiv(model,data,randvec[(std::size_t)_smooth]);
}
if(verbose) std::cout << "Uiv =\t\t";
timer.toc(std::cout,NBT);
@@ -190,7 +190,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
Eigen::VectorXd res;
SMOOTH(NBT)
{
- res = se3::cholesky::Mv(model,data,randvec[_smooth]);
+ res = se3::cholesky::Mv(model,data,randvec[(std::size_t)_smooth]);
}
if(verbose) std::cout << "Mv =\t\t";
timer.toc(std::cout,NBT);
@@ -200,7 +200,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
timer.tic();
SMOOTH(NBT)
{
- se3::cholesky::Mv(model,data,randvec[_smooth],true);
+ se3::cholesky::Mv(model,data,randvec[(std::size_t)_smooth],true);
}
if(verbose) std::cout << "UDUtv =\t\t";
timer.toc(std::cout,NBT);
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index def40f759..b813840fe 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -27,7 +27,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):model.nbody-1;
+ Model::Index idx = model.existBodyName("rarm2")?model.getBodyId("rarm2"):(Model::Index)(model.nbody-1);
MatrixXd Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
@@ -35,7 +35,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd qdot = VectorXd::Random(model.nv);
VectorXd qddot = VectorXd::Zero(model.nv);
rnea( model,data,q,qdot,qddot );
- Motion v = data.oMi[idx].act( data.v[idx] );
+ Motion v = data.oMi[(std::size_t)idx].act( data.v[(std::size_t)idx] );
is_matrix_absolutely_closed(v.toVector(),Jrh*qdot,1e-12);
@@ -43,7 +43,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
MatrixXd rhJrh(6,model.nv); rhJrh.fill(0);
getJacobian<true>(model,data,idx,rhJrh);
MatrixXd XJrh(6,model.nv);
- motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
+ motionSet::se3Action( data.oMi[(std::size_t)idx].inverse(), Jrh,XJrh );
is_matrix_absolutely_closed(XJrh,rhJrh,1e-12);
@@ -94,7 +94,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 1 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -109,7 +109,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 2 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
@@ -124,7 +124,7 @@ BOOST_AUTO_TEST_CASE ( test_timings )
if( flag >> 3 & 1 )
{
computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):(Model::Index)(model.nbody-1);
Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
timer.tic();
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index 25f84f77c..9fbfa4af6 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -631,11 +631,6 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
using namespace se3;
- Motion expected_v_J (Motion::Zero ());
- Motion expected_c_J (Motion::Zero ());
-
- SE3 expected_configuration (SE3::Identity ());
-
Eigen::Vector3d axis;
axis << 1.0, 0.0, 0.0;
@@ -788,7 +783,7 @@ BOOST_AUTO_TEST_CASE ( test_merge_body )
model.addBody (model.getBodyId("universe"), JointModelRX (), SE3::Identity (), inertiaRoot, "root");
model.mergeFixedBody(model.getBodyId("root"), liMi, inertiaFixedBodyAtJoint);
- Inertia mergedInertia(model.inertias[model.getBodyId("root")]);
+ Inertia mergedInertia(model.inertias[(size_t)(model.getBodyId("root"))]);
double expected_mass=2;
Eigen::Vector3d expected_com(Eigen::Vector3d::Zero());expected_com << 1.125, 0.5, 0.;
diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp
index f72584114..d8cb9a41e 100644
--- a/unittest/symmetric.cpp
+++ b/unittest/symmetric.cpp
@@ -198,13 +198,13 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
std::vector<Symmetric3> Sres (NBT);
std::vector<Matrix3> Rs (NBT);
for(int i=0;i<NBT;++i)
- Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+ Rs[(std::size_t)i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
std::cout << "Pinocchio: ";
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
{
- timeSym3(S,Rs[_smooth],Sres[_smooth]);
+ timeSym3(S,Rs[(std::size_t)_smooth],Sres[(std::size_t)_smooth]);
}
timer.toc(std::cout,NBT);
}
@@ -284,13 +284,13 @@ BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
std::vector<Eigen::Matrix3d> Sres (NBT);
std::vector<Eigen::Matrix3d> Rs (NBT);
for(int i=0;i<NBT;++i)
- Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+ Rs[(std::size_t)i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
std::cout << "Eigen: ";
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
{
- timeSelfAdj(Rs[_smooth],M,Sres[_smooth]);
+ timeSelfAdj(Rs[(std::size_t)_smooth],M,Sres[(std::size_t)_smooth]);
}
timer.toc(std::cout,NBT);
}
--
GitLab