Commit d7b1e3a9 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Remove useless ifndef

parent 53e962bf
...@@ -194,9 +194,7 @@ namespace se3 ...@@ -194,9 +194,7 @@ namespace se3
const Eigen::VectorXd & v, const Eigen::VectorXd & v,
const Eigen::VectorXd & tau) const Eigen::VectorXd & tau)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
data.a[0] = -model.gravity; data.a[0] = -model.gravity;
......
...@@ -31,9 +31,7 @@ namespace se3 ...@@ -31,9 +31,7 @@ namespace se3
const bool computeSubtreeComs, const bool computeSubtreeComs,
const bool updateKinematics) const bool updateKinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.mass[0] = 0; data.mass[0] = 0;
data.com[0].setZero (); data.com[0].setZero ();
...@@ -80,9 +78,7 @@ namespace se3 ...@@ -80,9 +78,7 @@ namespace se3
const bool computeSubtreeComs, const bool computeSubtreeComs,
const bool updateKinematics) const bool updateKinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
using namespace se3; using namespace se3;
data.mass[0] = 0; data.mass[0] = 0;
...@@ -140,9 +136,7 @@ namespace se3 ...@@ -140,9 +136,7 @@ namespace se3
const bool computeSubtreeComs, const bool computeSubtreeComs,
const bool updateKinematics) const bool updateKinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
using namespace se3; using namespace se3;
data.mass[0] = 0; data.mass[0] = 0;
...@@ -201,9 +195,7 @@ namespace se3 ...@@ -201,9 +195,7 @@ namespace se3
inline const SE3::Vector3 & inline const SE3::Vector3 &
getComFromCrba(const Model & model, Data & data) getComFromCrba(const Model & model, Data & data)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever()); return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
} }
...@@ -284,9 +276,7 @@ namespace se3 ...@@ -284,9 +276,7 @@ namespace se3
const bool computeSubtreeComs, const bool computeSubtreeComs,
const bool updateKinematics) const bool updateKinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.com[0].setZero (); data.com[0].setZero ();
data.mass[0] = 0; data.mass[0] = 0;
...@@ -320,9 +310,7 @@ namespace se3 ...@@ -320,9 +310,7 @@ namespace se3
inline const Data::Matrix3x & inline const Data::Matrix3x &
getJacobianComFromCrba(const Model & model, Data & data) getJacobianComFromCrba(const Model & model, Data & data)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
const SE3 & oM1 = data.liMi[1]; const SE3 & oM1 = data.liMi[1];
// As the 6 first rows of M*a are a wrench, we just need to multiply by the // As the 6 first rows of M*a are a wrench, we just need to multiply by the
......
...@@ -204,9 +204,7 @@ namespace se3 ...@@ -204,9 +204,7 @@ namespace se3
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Eigen::VectorXd & v) const Eigen::VectorXd & v)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
data.a[0].setZero(); data.a[0].setZero();
data.a_gf[0] = -model.gravity; data.a_gf[0] = -model.gravity;
......
...@@ -103,9 +103,7 @@ namespace se3 ...@@ -103,9 +103,7 @@ namespace se3
crba(const Model & model, Data& data, crba(const Model & model, Data& data,
const Eigen::VectorXd & q) const Eigen::VectorXd & q)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i ) for( Model::JointIndex i=1;i<(Model::JointIndex)(model.njoints);++i )
{ {
...@@ -208,9 +206,7 @@ namespace se3 ...@@ -208,9 +206,7 @@ namespace se3
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Eigen::VectorXd & v) const Eigen::VectorXd & v)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x; typedef Eigen::Block <Data::Matrix6x,3,-1> Block3x;
forwardKinematics(model, data, q); forwardKinematics(model, data, q);
......
...@@ -63,9 +63,7 @@ namespace se3 ...@@ -63,9 +63,7 @@ namespace se3
assert(tau.size() == model.nv); assert(tau.size() == model.nv);
assert(J.cols() == model.nv); assert(J.cols() == model.nv);
assert(J.rows() == gamma.size()); assert(J.rows() == gamma.size());
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
Eigen::VectorXd & a = data.ddq; Eigen::VectorXd & a = data.ddq;
Eigen::VectorXd & lambda_c = data.lambda_c; Eigen::VectorXd & lambda_c = data.lambda_c;
...@@ -130,9 +128,7 @@ namespace se3 ...@@ -130,9 +128,7 @@ namespace se3
assert(q.size() == model.nq); assert(q.size() == model.nq);
assert(v_before.size() == model.nv); assert(v_before.size() == model.nv);
assert(J.cols() == model.nv); assert(J.cols() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
Eigen::VectorXd & impulse_c = data.impulse_c; Eigen::VectorXd & impulse_c = data.impulse_c;
Eigen::VectorXd & dq_after = data.dq_after; Eigen::VectorXd & dq_after = data.dq_after;
......
...@@ -72,9 +72,7 @@ namespace se3 ...@@ -72,9 +72,7 @@ namespace se3
const Eigen::VectorXd & v, const Eigen::VectorXd & v,
const bool update_kinematics) const bool update_kinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.kinetic_energy = 0.; data.kinetic_energy = 0.;
...@@ -94,9 +92,7 @@ namespace se3 ...@@ -94,9 +92,7 @@ namespace se3
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const bool update_kinematics) const bool update_kinematics)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.potential_energy = 0.; data.potential_energy = 0.;
const Motion::ConstLinear_t & g = model.gravity.linear(); const Motion::ConstLinear_t & g = model.gravity.linear();
......
...@@ -85,9 +85,7 @@ namespace se3 ...@@ -85,9 +85,7 @@ namespace se3
Data & data Data & data
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
// The following for loop starts by index 1 because the first frame is fixed // The following for loop starts by index 1 because the first frame is fixed
// and corresponds to the universe.s // and corresponds to the universe.s
...@@ -107,9 +105,7 @@ namespace se3 ...@@ -107,9 +105,7 @@ namespace se3
const Eigen::VectorXd & q const Eigen::VectorXd & q
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
forwardKinematics(model, data, q); forwardKinematics(model, data, q);
framesForwardKinematics(model, data); framesForwardKinematics(model, data);
...@@ -125,9 +121,7 @@ namespace se3 ...@@ -125,9 +121,7 @@ namespace se3
{ {
assert(J.cols() == model.nv); assert(J.cols() == model.nv);
assert(data.J.cols() == model.nv); assert(data.J.cols() == model.nv);
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
const Model::JointIndex & parent = model.frames[frame_id].parent; const Model::JointIndex & parent = model.frames[frame_id].parent;
const SE3 & oMframe = data.oMf[frame_id]; const SE3 & oMframe = data.oMf[frame_id];
......
...@@ -32,9 +32,7 @@ namespace se3 ...@@ -32,9 +32,7 @@ namespace se3
const Eigen::VectorXd & q const Eigen::VectorXd & q
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
forwardKinematics(model, data, q); forwardKinematics(model, data, q);
updateGeometryPlacements(model, data, geomModel, geomData); updateGeometryPlacements(model, data, geomModel, geomData);
...@@ -46,9 +44,7 @@ namespace se3 ...@@ -46,9 +44,7 @@ namespace se3
GeometryData & geomData GeometryData & geomData
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i) for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i)
{ {
...@@ -116,9 +112,7 @@ namespace se3 ...@@ -116,9 +112,7 @@ namespace se3
const bool stopAtFirstCollision const bool stopAtFirstCollision
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
updateGeometryPlacements (model, data, geomModel, geomData, q); updateGeometryPlacements (model, data, geomModel, geomData, q);
...@@ -197,9 +191,7 @@ namespace se3 ...@@ -197,9 +191,7 @@ namespace se3
const Eigen::VectorXd & q const Eigen::VectorXd & q
) )
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
updateGeometryPlacements (model, data, geomModel, geomData, q); updateGeometryPlacements (model, data, geomModel, geomData, q);
return computeDistances<ComputeShortest>(geomModel,geomData); return computeDistances<ComputeShortest>(geomModel,geomData);
} }
......
...@@ -60,9 +60,7 @@ namespace se3 ...@@ -60,9 +60,7 @@ namespace se3
computeJacobians(const Model & model, Data & data, computeJacobians(const Model & model, Data & data,
const Eigen::VectorXd & q) const Eigen::VectorXd & q)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i ) for( Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i )
{ {
...@@ -84,9 +82,7 @@ namespace se3 ...@@ -84,9 +82,7 @@ namespace se3
{ {
assert( J.rows() == data.J.rows() ); assert( J.rows() == data.J.rows() );
assert( J.cols() == data.J.cols() ); assert( J.cols() == data.J.cols() );
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
const SE3 & oMjoint = data.oMi[jointId]; const SE3 & oMjoint = data.oMi[jointId];
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1; int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
...@@ -132,9 +128,7 @@ namespace se3 ...@@ -132,9 +128,7 @@ namespace se3
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Model::JointIndex jointId) const Model::JointIndex jointId)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.iMf[jointId].setIdentity(); data.iMf[jointId].setIdentity();
for( Model::JointIndex i=jointId;i>0;i=model.parents[i] ) for( Model::JointIndex i=jointId;i>0;i=model.parents[i] )
......
...@@ -45,9 +45,7 @@ namespace se3 ...@@ -45,9 +45,7 @@ namespace se3
inline void emptyForwardPass(const Model & model, inline void emptyForwardPass(const Model & model,
Data & data) Data & data)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i) for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{ {
...@@ -60,9 +58,7 @@ namespace se3 ...@@ -60,9 +58,7 @@ namespace se3
inline void updateGlobalPlacements(const Model & model, Data & data) inline void updateGlobalPlacements(const Model & model, Data & data)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i) for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{ {
...@@ -112,9 +108,7 @@ namespace se3 ...@@ -112,9 +108,7 @@ namespace se3
const Eigen::VectorXd & q) const Eigen::VectorXd & q)
{ {
assert(q.size() == model.nq && "The configuration vector is not of right size"); assert(q.size() == model.nq && "The configuration vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i) for (Model::JointIndex i=1; i < (Model::JointIndex) model.njoints; ++i)
{ {
...@@ -168,9 +162,7 @@ namespace se3 ...@@ -168,9 +162,7 @@ namespace se3
{ {
assert(q.size() == model.nq && "The configuration vector is not of right size"); assert(q.size() == model.nq && "The configuration vector is not of right size");
assert(v.size() == model.nv && "The velocity vector is not of right size"); assert(v.size() == model.nv && "The velocity vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
...@@ -231,9 +223,7 @@ namespace se3 ...@@ -231,9 +223,7 @@ namespace se3
assert(q.size() == model.nq && "The configuration vector is not of right size"); assert(q.size() == model.nq && "The configuration vector is not of right size");
assert(v.size() == model.nv && "The velocity vector is not of right size"); assert(v.size() == model.nv && "The velocity vector is not of right size");
assert(a.size() == model.nv && "The acceleration vector is not of right size"); assert(a.size() == model.nv && "The acceleration vector is not of right size");
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
data.a[0].setZero(); data.a[0].setZero();
......
...@@ -91,9 +91,7 @@ namespace se3 ...@@ -91,9 +91,7 @@ namespace se3
const Eigen::VectorXd & v, const Eigen::VectorXd & v,
const Eigen::VectorXd & a) const Eigen::VectorXd & a)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
data.a_gf[0] = -model.gravity; data.a_gf[0] = -model.gravity;
...@@ -121,9 +119,7 @@ namespace se3 ...@@ -121,9 +119,7 @@ namespace se3
const container::aligned_vector<Force> & fext) const container::aligned_vector<Force> & fext)
{ {
assert(fext.size() == model.joints.size()); assert(fext.size() == model.joints.size());
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero(); data.v[0].setZero();
data.a_gf[0] = -model.gravity; data.a_gf[0] = -model.gravity;
...@@ -207,9 +203,7 @@ namespace se3 ...@@ -207,9 +203,7 @@ namespace se3
const Eigen::VectorXd & q, const Eigen::VectorXd & q,
const Eigen::VectorXd & v) const Eigen::VectorXd & v)
{ {
#ifndef NDEBUG
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
#endif
data.v[0].setZero (); data.v[0].setZero ();
data.a_gf[0] = -model.gravity; data.a_gf[0] = -model.gravity;
......
Supports Markdown
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