Commit d7b1e3a9 authored by jcarpent's avatar jcarpent
Browse files

[Algo] Remove useless ifndef

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