Commit c0a0fa72 authored by Wolfgang Merkt's avatar Wolfgang Merkt
Browse files

Change model.check(data) back to assert

parent fb3505ad
......@@ -348,7 +348,7 @@ namespace pinocchio
TEST_NEW_ASSERT(aba_partial_dv.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.rows() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -418,7 +418,7 @@ namespace pinocchio
TEST_NEW_ASSERT(aba_partial_dv.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.rows() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -222,7 +222,7 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The joint velocity vector is not of right size");
TEST_NEW_ASSERT(tau.size() == model.nv && "The joint acceleration vector is not of right size");
......@@ -267,7 +267,7 @@ namespace pinocchio
const container::aligned_vector<ForceDerived> & fext)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The joint velocity vector is not of right size");
TEST_NEW_ASSERT(tau.size() == model.nv && "The joint acceleration vector is not of right size");
......@@ -456,7 +456,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -64,7 +64,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x);
TEST_NEW_ASSERT(vcom_partial_dq.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
......@@ -104,7 +104,7 @@ namespace pinocchio
const int LEVEL,
const bool computeSubtreeComs)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(LEVEL >= 0);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -194,7 +194,7 @@ namespace pinocchio
{
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
}
......@@ -282,7 +282,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const bool computeSubtreeComs)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -384,7 +384,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT((int)rootSubtreeId < model.njoints && "Invalid joint id.");
TEST_NEW_ASSERT(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
......@@ -460,7 +460,7 @@ namespace pinocchio
{
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(((int)rootSubtreeId < model.njoints) && "Invalid joint id.");
TEST_NEW_ASSERT(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
......@@ -496,7 +496,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::SE3 SE3;
......
......@@ -238,7 +238,7 @@ namespace pinocchio
TEST_NEW_ASSERT(dhdot_dv.rows() == 6);
TEST_NEW_ASSERT(dhdot_da.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_da.rows() == 6);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -363,7 +363,7 @@ namespace pinocchio
TEST_NEW_ASSERT(dhdot_dv.rows() == 6);
TEST_NEW_ASSERT(dhdot_da.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_da.rows() == 6);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
......@@ -121,7 +121,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......@@ -166,7 +166,7 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
TEST_NEW_ASSERT(a.size() == model.nv && "The acceleration vector is not of right size");
......@@ -252,7 +252,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......@@ -345,7 +345,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......
......@@ -33,7 +33,7 @@ namespace pinocchio
* end
*/
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -87,7 +87,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -143,7 +143,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -195,7 +195,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -249,7 +249,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -306,7 +306,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes)
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(vin.size() == model.nv);
TEST_NEW_ASSERT(vout.size() == model.nv);
......@@ -372,7 +372,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -422,7 +422,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -455,7 +455,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(col < model.nv && col >= 0);
TEST_NEW_ASSERT(v.size() == model.nv);
......@@ -494,7 +494,7 @@ namespace pinocchio
TEST_NEW_ASSERT(Minv.rows() == model.nv);
TEST_NEW_ASSERT(Minv.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv);
......
......@@ -205,7 +205,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......
......@@ -28,7 +28,7 @@ namespace pinocchio
TEST_NEW_ASSERT(tau.size() == model.nv);
TEST_NEW_ASSERT(J.cols() == model.nv);
TEST_NEW_ASSERT(J.rows() == gamma.size());
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -143,7 +143,7 @@ namespace pinocchio
{
TEST_NEW_ASSERT(v_before.size() == model.nv);
TEST_NEW_ASSERT(J.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
......@@ -177,7 +177,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -205,7 +205,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -70,7 +70,7 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v,
const bool update_kinematics)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......@@ -97,7 +97,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool update_kinematics)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
......@@ -17,7 +17,7 @@ namespace pinocchio
inline void updateFramePlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Frame Frame;
......@@ -40,7 +40,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
const typename Model::Frame & frame = model.frames[frame_id];
......@@ -64,7 +64,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
updateFramePlacements(model, data);
......@@ -76,7 +76,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -91,7 +91,7 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -110,7 +110,7 @@ namespace pinocchio
TEST_NEW_ASSERT(J.rows() == 6);
TEST_NEW_ASSERT(J.cols() == model.nv);
TEST_NEW_ASSERT(data.J.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -156,7 +156,7 @@ namespace pinocchio
const FrameIndex frameId,
const Eigen::MatrixBase<Matrix6Like> & J)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -193,7 +193,7 @@ namespace pinocchio
{
TEST_NEW_ASSERT( dJ.rows() == data.dJ.rows() );
TEST_NEW_ASSERT( dJ.cols() == data.dJ.cols() );
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Frame Frame;
......
......@@ -19,7 +19,7 @@ namespace pinocchio
GeometryData & geomData,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
forwardKinematics(model, data, q);
updateGeometryPlacements(model, data, geomModel, geomData);
......@@ -32,7 +32,7 @@ namespace pinocchio
GeometryData & geomData)
{
PINOCCHIO_UNUSED_VARIABLE(model);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
for (GeomIndex i=0; i < (GeomIndex) geomModel.ngeoms; ++i)
{
......@@ -100,7 +100,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool stopAtFirstCollision)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model, data, geomModel, geomData, q);
......@@ -180,7 +180,7 @@ namespace pinocchio
const GeometryModel & geomModel,
GeometryData & geomData)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model,data,geomModel,geomData);
return computeDistances(geomModel,geomData);
}
......@@ -192,7 +192,7 @@ namespace pinocchio
GeometryData & geomData,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements(model, data, geomModel, geomData, q);
return computeDistances(geomModel,geomData);
}
......@@ -206,7 +206,7 @@ namespace pinocchio
const Eigen::VectorXd & q
)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
updateGeometryPlacements (model, data, geomModel, geomData, q);
return computeDistances<ComputeShortest>(geomModel,geomData);
}
......
......@@ -53,7 +53,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -96,7 +96,7 @@ namespace pinocchio
computeJointJacobians(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -193,7 +193,7 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & J)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
details::translateJointJacobian(model,data,jointId,rf,
data.J,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
......@@ -242,7 +242,7 @@ namespace pinocchio
const JointIndex jointId,
const Eigen::MatrixBase<Matrix6xLike> & J)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -323,7 +323,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
......@@ -347,7 +347,7 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
details::translateJointJacobian(model,data,jointId,rf,
data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
......
......@@ -85,7 +85,7 @@ namespace pinocchio
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
TEST_NEW_ASSERT(a.size() == model.nv && "The acceleration vector is not of right size");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -188,7 +188,7 @@ namespace pinocchio
TEST_NEW_ASSERT(v_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(v_partial_dv.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -350,7 +350,7 @@ namespace pinocchio
TEST_NEW_ASSERT(a_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(a_partial_dv.cols() == model.nv);
TEST_NEW_ASSERT(a_partial_da.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
......@@ -33,7 +33,7 @@ namespace pinocchio
inline void emptyForwardPass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
typedef EmptyForwardStep<Scalar,Options,JointCollectionTpl> Algo;
......@@ -50,7 +50,7 @@ namespace pinocchio
inline void updateGlobalPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -106,7 +106,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -169,7 +169,7 @@ namespace pinocchio
{
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -239,7 +239,7 @@ namespace pinocchio
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(v.size() == model.nv && "The velocity vector is not of right size");
TEST_NEW_ASSERT(a.size() == model.nv && "The acceleration vector is not of right size");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -19,7 +19,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -145,7 +145,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
JointIndex jointId)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_UNUSED_VARIABLE(model);
......@@ -159,7 +159,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
FrameIndex frameId)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Frame Frame;
......@@ -258,7 +258,7 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a)
{
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(q.size() == model.nq);
TEST_NEW_ASSERT(v.size() == model.nv);
TEST_NEW_ASSERT(a.size() == model.nv);
......
......@@ -130,7 +130,7 @@ namespace pinocchio
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
TEST_NEW_ASSERT(gravity_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT</