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

Use if() and throw exception on everything apart from model.check(data)

parent 06995d18
......@@ -309,7 +309,7 @@ namespace pinocchio
}
// Restore the status of dAdq_cols (remove gravity)
assert(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
TEST_NEW_ASSERT(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k)
{
MotionRef<typename ColsBlock::ColXpr> min(J_cols.col(k));
......@@ -339,16 +339,16 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(tau.size() == model.nv && "The joint acceleration vector is not of right size");
assert(aba_partial_dq.cols() == model.nv);
assert(aba_partial_dq.rows() == model.nv);
assert(aba_partial_dv.cols() == model.nv);
assert(aba_partial_dv.rows() == model.nv);
assert(aba_partial_dtau.cols() == model.nv);
assert(aba_partial_dtau.rows() == model.nv);
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");
TEST_NEW_ASSERT(aba_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dq.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.cols() == model.nv);
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.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -408,17 +408,17 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(tau.size() == model.nv && "The joint acceleration vector is not of right size");
assert(fext.size() == (size_t)model.njoints && "The size of the external forces is not of right size");
assert(aba_partial_dq.cols() == model.nv);
assert(aba_partial_dq.rows() == model.nv);
assert(aba_partial_dv.cols() == model.nv);
assert(aba_partial_dv.rows() == model.nv);
assert(aba_partial_dtau.cols() == model.nv);
assert(aba_partial_dtau.rows() == model.nv);
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");
TEST_NEW_ASSERT(fext.size() == (size_t)model.njoints && "The size of the external forces is not of right size");
TEST_NEW_ASSERT(aba_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dq.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.cols() == model.nv);
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.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -222,10 +222,10 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & tau)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(tau.size() == model.nv && "The joint acceleration vector is not of right size");
// TEST_NEW_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");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -267,10 +267,10 @@ namespace pinocchio
const container::aligned_vector<ForceDerived> & fext)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(tau.size() == model.nv && "The joint acceleration vector is not of right size");
// TEST_NEW_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");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -456,8 +456,8 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
// TEST_NEW_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;
data.Minv.template triangularView<Eigen::Upper>().setZero();
......
......@@ -63,8 +63,8 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x);
assert(vcom_partial_dq.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(vcom_partial_dq.cols() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
......@@ -104,8 +104,8 @@ namespace pinocchio
const int LEVEL,
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
assert(LEVEL >= 0);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(LEVEL >= 0);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -194,7 +194,7 @@ namespace pinocchio
{
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_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)
{
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -384,9 +384,9 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
assert(model.check(data) && "data is not consistent with model.");
assert((int)rootSubtreeId < model.njoints && "Invalid joint id.");
assert(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
// TEST_NEW_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.");
Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
......@@ -423,7 +423,7 @@ namespace pinocchio
typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs));
}
assert(data.mass[rootSubtreeId] > 0. && "The mass of the subtree is not positive.");
TEST_NEW_ASSERT(data.mass[rootSubtreeId] > 0. && "The mass of the subtree is not positive.");
const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId];
typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
if(not computeSubtreeComs)
......@@ -460,9 +460,9 @@ namespace pinocchio
{
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
assert(model.check(data) && "data is not consistent with model.");
assert(((int)rootSubtreeId < model.njoints) && "Invalid joint id.");
assert(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
// TEST_NEW_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.");
Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
......@@ -496,7 +496,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::SE3 SE3;
......
......@@ -199,7 +199,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut,6,Eigen::Dynamic)
assert(Fin.cols() == Fout.cols() && "Fin and Fout do not have the same number of columns");
TEST_NEW_ASSERT(Fin.cols() == Fout.cols() && "Fin and Fout do not have the same number of columns");
for(Eigen::DenseIndex k = 0; k < Fin.cols(); ++k)
{
......@@ -227,18 +227,18 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{
assert(q.size() == model.nq && "The joint configuration vector is not of right size");
assert(v.size() == model.nv && "The joint velocity vector is not of right size");
assert(a.size() == model.nv && "The joint acceleration vector is not of right size");
assert(dh_dq.cols() == model.nv);
assert(dh_dq.rows() == 6);
assert(dhdot_dq.cols() == model.nv);
assert(dhdot_dq.rows() == 6);
assert(dhdot_dv.cols() == model.nv);
assert(dhdot_dv.rows() == 6);
assert(dhdot_da.cols() == model.nv);
assert(dhdot_da.rows() == 6);
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(a.size() == model.nv && "The joint acceleration vector is not of right size");
TEST_NEW_ASSERT(dh_dq.cols() == model.nv);
TEST_NEW_ASSERT(dh_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dq.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dv.cols() == model.nv);
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.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -357,13 +357,13 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{
assert(dhdot_dq.cols() == model.nv);
assert(dhdot_dq.rows() == 6);
assert(dhdot_dv.cols() == model.nv);
assert(dhdot_dv.rows() == 6);
assert(dhdot_da.cols() == model.nv);
assert(dhdot_da.rows() == 6);
assert(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(dhdot_dq.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dv.cols() == model.nv);
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.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
......@@ -121,9 +121,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -166,10 +166,10 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -252,9 +252,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -345,9 +345,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
......@@ -33,7 +33,7 @@ namespace pinocchio
* end
*/
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -87,8 +87,8 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
// TEST_NEW_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,8 +143,8 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
// TEST_NEW_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);
const typename Data::MatrixXs & U = data.U;
......@@ -195,8 +195,8 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
// TEST_NEW_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);
const typename Data::MatrixXs & U = data.U;
......@@ -249,8 +249,8 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
// TEST_NEW_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);
const typename Data::MatrixXs & U = data.U;
......@@ -306,9 +306,9 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes)
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(vin.size() == model.nv);
assert(vout.size() == model.nv);
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
TEST_NEW_ASSERT(vin.size() == model.nv);
TEST_NEW_ASSERT(vout.size() == model.nv);
MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);
......@@ -372,8 +372,8 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
// TEST_NEW_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)
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -455,9 +455,9 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(col < model.nv && col >= 0);
assert(v.size() == model.nv);
// TEST_NEW_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);
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -491,10 +491,10 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & Minv)
{
assert(Minv.rows() == model.nv);
assert(Minv.cols() == model.nv);
TEST_NEW_ASSERT(Minv.rows() == model.nv);
TEST_NEW_ASSERT(Minv.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_ASSERT(model.check(data) && "data is not consistent with model.");
Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv);
......
......@@ -205,9 +205,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
......@@ -25,10 +25,10 @@ namespace pinocchio
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping)
{
assert(tau.size() == model.nv);
assert(J.cols() == model.nv);
assert(J.rows() == gamma.size());
assert(model.check(data) && "data is not consistent with model.");
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.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -78,8 +78,8 @@ namespace pinocchio
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping)
{
assert(q.size() == model.nq);
assert(v.size() == model.nv);
TEST_NEW_ASSERT(q.size() == model.nq);
TEST_NEW_ASSERT(v.size() == model.nv);
computeAllTerms(model, data, q, v);
......@@ -94,8 +94,8 @@ namespace pinocchio
const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv)
{
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
assert(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
assert(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv);
TEST_NEW_ASSERT(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
TEST_NEW_ASSERT(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv);
const typename Data::MatrixXs::Index& nc = data.JMinvJt.cols();
KKTMatrixType& MJtJ_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,MJtJ_inv);
......@@ -124,7 +124,7 @@ namespace pinocchio
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(q.size() == model.nq);
TEST_NEW_ASSERT(q.size() == model.nq);
// Compute the mass matrix
crba(model, data, q);
......@@ -141,9 +141,9 @@ namespace pinocchio
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(v_before.size() == model.nv);
assert(J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
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.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
......@@ -49,7 +49,7 @@ namespace pinocchio
for(JointIndex jid=1; jid<(JointIndex)model.njoints; ++jid)
{
assert(LEVEL>=0);
TEST_NEW_ASSERT(LEVEL>=0);
dest.oMi[jid] = origin.oMi [jid];
if(LEVEL>=1)
......
......@@ -177,8 +177,8 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
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.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -205,8 +205,8 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
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.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
......@@ -70,9 +70,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v,
const bool update_kinematics)
{
assert(model.check(data) && "data is not consistent with model.");
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");
// TEST_NEW_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");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -97,8 +97,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const bool update_kinematics)
{
assert(model.check(data) && "data is not consistent with model.");
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.");
TEST_NEW_ASSERT(q.size() == model.nq && "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
......@@ -17,7 +17,7 @@ namespace pinocchio
inline void updateFramePlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data)
{
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_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)
{
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_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)
{
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_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)
{
assert(model.check(data) && "data is not consistent with model.");
// TEST_NEW_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)
{