Commit 54503f81 authored by Wolfgang Merkt's avatar Wolfgang Merkt
Browse files

Rename TEST_NEW_ASSERT to PINOCCHIO_ASSERT_THROW_AT_RUNTIME

parent c0a0fa72
...@@ -309,7 +309,7 @@ namespace pinocchio ...@@ -309,7 +309,7 @@ namespace pinocchio
} }
// Restore the status of dAdq_cols (remove gravity) // Restore the status of dAdq_cols (remove gravity)
TEST_NEW_ASSERT(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k) for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k)
{ {
MotionRef<typename ColsBlock::ColXpr> min(J_cols.col(k)); MotionRef<typename ColsBlock::ColXpr> min(J_cols.col(k));
...@@ -339,15 +339,15 @@ namespace pinocchio ...@@ -339,15 +339,15 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau) const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{ {
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
TEST_NEW_ASSERT(aba_partial_dq.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dq.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dtau.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dtau.rows() == model.nv);
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 typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
...@@ -408,16 +408,16 @@ namespace pinocchio ...@@ -408,16 +408,16 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv, const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau) const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{ {
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dq.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dv.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.rows() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dtau.cols() == model.nv);
TEST_NEW_ASSERT(aba_partial_dtau.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dtau.rows() == model.nv);
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 typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
...@@ -223,9 +223,9 @@ namespace pinocchio ...@@ -223,9 +223,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & tau) const Eigen::MatrixBase<TangentVectorType2> & tau)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
...@@ -268,9 +268,9 @@ namespace pinocchio ...@@ -268,9 +268,9 @@ namespace pinocchio
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
...@@ -457,7 +457,7 @@ namespace pinocchio ...@@ -457,7 +457,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q) const Eigen::MatrixBase<ConfigVectorType> & q)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
data.Minv.template triangularView<Eigen::Upper>().setZero(); data.Minv.template triangularView<Eigen::Upper>().setZero();
......
...@@ -63,7 +63,7 @@ namespace pinocchio ...@@ -63,7 +63,7 @@ namespace pinocchio
{ {
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x); EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x);
TEST_NEW_ASSERT(vcom_partial_dq.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(vcom_partial_dq.cols() == model.nv);
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 ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
...@@ -105,7 +105,7 @@ namespace pinocchio ...@@ -105,7 +105,7 @@ namespace pinocchio
const bool computeSubtreeComs) const bool computeSubtreeComs)
{ {
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(LEVEL >= 0);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -385,8 +385,8 @@ namespace pinocchio ...@@ -385,8 +385,8 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
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."); PINOCCHIO_ASSERT_THROW_AT_RUNTIME((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."); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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); Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
...@@ -423,7 +423,7 @@ namespace pinocchio ...@@ -423,7 +423,7 @@ namespace pinocchio
typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs)); typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs));
} }
TEST_NEW_ASSERT(data.mass[rootSubtreeId] > 0. && "The mass of the subtree is not positive."); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(data.mass[rootSubtreeId] > 0. && "The mass of the subtree is not positive.");
const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId]; const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId];
typename Data::Vector3 & com_subtree = data.com[rootSubtreeId]; typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
if(not computeSubtreeComs) if(not computeSubtreeComs)
...@@ -461,8 +461,8 @@ namespace pinocchio ...@@ -461,8 +461,8 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
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."); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(((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."); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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); Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
......
...@@ -199,7 +199,7 @@ namespace pinocchio ...@@ -199,7 +199,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3) EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut,6,Eigen::Dynamic) EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut,6,Eigen::Dynamic)
TEST_NEW_ASSERT(Fin.cols() == Fout.cols() && "Fin and Fout do not have the same number of columns"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(Fin.cols() == Fout.cols() && "Fin and Fout do not have the same number of columns");
for(Eigen::DenseIndex k = 0; k < Fin.cols(); ++k) for(Eigen::DenseIndex k = 0; k < Fin.cols(); ++k)
{ {
...@@ -227,17 +227,17 @@ namespace pinocchio ...@@ -227,17 +227,17 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{ {
TEST_NEW_ASSERT(q.size() == model.nq && "The joint configuration vector is not of right size"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The joint acceleration vector is not of right size");
TEST_NEW_ASSERT(dh_dq.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dh_dq.cols() == model.nv);
TEST_NEW_ASSERT(dh_dq.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dh_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dq.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dq.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dq.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dv.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dv.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dv.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dv.rows() == 6);
TEST_NEW_ASSERT(dhdot_da.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_da.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_da.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_da.rows() == 6);
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 ModelTpl<Scalar,Options,JointCollectionTpl> Model;
...@@ -357,12 +357,12 @@ namespace pinocchio ...@@ -357,12 +357,12 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{ {
TEST_NEW_ASSERT(dhdot_dq.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dq.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dq.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dq.rows() == 6);
TEST_NEW_ASSERT(dhdot_dv.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dv.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_dv.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dv.rows() == 6);
TEST_NEW_ASSERT(dhdot_da.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_da.cols() == model.nv);
TEST_NEW_ASSERT(dhdot_da.rows() == 6); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_da.rows() == 6);
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 ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
...@@ -122,8 +122,8 @@ namespace pinocchio ...@@ -122,8 +122,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v) const Eigen::MatrixBase<TangentVectorType> & v)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -167,9 +167,9 @@ namespace pinocchio ...@@ -167,9 +167,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & a) const Eigen::MatrixBase<TangentVectorType2> & a)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -253,8 +253,8 @@ namespace pinocchio ...@@ -253,8 +253,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v) const Eigen::MatrixBase<TangentVectorType> & v)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -346,8 +346,8 @@ namespace pinocchio ...@@ -346,8 +346,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v) const Eigen::MatrixBase<TangentVectorType> & v)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
...@@ -88,7 +88,7 @@ namespace pinocchio ...@@ -88,7 +88,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
PINOCCHIO_UNUSED_VARIABLE(model); PINOCCHIO_UNUSED_VARIABLE(model);
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
...@@ -144,7 +144,7 @@ namespace pinocchio ...@@ -144,7 +144,7 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model); PINOCCHIO_UNUSED_VARIABLE(model);
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U; const typename Data::MatrixXs & U = data.U;
...@@ -196,7 +196,7 @@ namespace pinocchio ...@@ -196,7 +196,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U; const typename Data::MatrixXs & U = data.U;
...@@ -250,7 +250,7 @@ namespace pinocchio ...@@ -250,7 +250,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U; const typename Data::MatrixXs & U = data.U;
...@@ -307,8 +307,8 @@ namespace pinocchio ...@@ -307,8 +307,8 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model); PINOCCHIO_UNUSED_VARIABLE(model);
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(vin.size() == model.nv);
TEST_NEW_ASSERT(vout.size() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(vout.size() == model.nv);
MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout); MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);
...@@ -373,7 +373,7 @@ namespace pinocchio ...@@ -373,7 +373,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
...@@ -456,8 +456,8 @@ namespace pinocchio ...@@ -456,8 +456,8 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model); PINOCCHIO_UNUSED_VARIABLE(model);
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); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(col < model.nv && col >= 0);
TEST_NEW_ASSERT(v.size() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -491,8 +491,8 @@ namespace pinocchio ...@@ -491,8 +491,8 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data, const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & Minv) const Eigen::MatrixBase<Mat> & Minv)
{ {
TEST_NEW_ASSERT(Minv.rows() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(Minv.rows() == model.nv);
TEST_NEW_ASSERT(Minv.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(Minv.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model."); assert(model.check(data) && "data is not consistent with model.");
......
...@@ -206,8 +206,8 @@ namespace pinocchio ...@@ -206,8 +206,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v) const Eigen::MatrixBase<TangentVectorType> & v)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex; typedef typename Model::JointIndex JointIndex;
......
...@@ -25,9 +25,9 @@ namespace pinocchio ...@@ -25,9 +25,9 @@ namespace pinocchio
const Eigen::MatrixBase<DriftVectorType> & gamma, const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping) const Scalar inv_damping)
{ {
TEST_NEW_ASSERT(tau.size() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv);
TEST_NEW_ASSERT(J.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(J.cols() == model.nv);
TEST_NEW_ASSERT(J.rows() == gamma.size()); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(J.rows() == gamma.size());
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 DataTpl<Scalar,Options,JointCollectionTpl> Data;
...@@ -78,8 +78,8 @@ namespace pinocchio ...@@ -78,8 +78,8 @@ namespace pinocchio
const Eigen::MatrixBase<DriftVectorType> & gamma, const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping) const Scalar inv_damping)
{ {
TEST_NEW_ASSERT(q.size() == model.nq); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq);
TEST_NEW_ASSERT(v.size() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv);
computeAllTerms(model, data, q, v); computeAllTerms(model, data, q, v);
...@@ -94,8 +94,8 @@ namespace pinocchio ...@@ -94,8 +94,8 @@ namespace pinocchio
const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv) const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv)
{ {
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
TEST_NEW_ASSERT(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
TEST_NEW_ASSERT(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv);
const typename Data::MatrixXs::Index& nc = data.JMinvJt.cols(); const typename Data::MatrixXs::Index& nc = data.JMinvJt.cols();
KKTMatrixType& MJtJ_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,MJtJ_inv); KKTMatrixType& MJtJ_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,MJtJ_inv);
...@@ -124,7 +124,7 @@ namespace pinocchio ...@@ -124,7 +124,7 @@ namespace pinocchio
const Scalar r_coeff, const Scalar r_coeff,
const Scalar inv_damping) const Scalar inv_damping)
{ {
TEST_NEW_ASSERT(q.size() == model.nq); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq);
// Compute the mass matrix // Compute the mass matrix
crba(model, data, q); crba(model, data, q);
...@@ -141,8 +141,8 @@ namespace pinocchio ...@@ -141,8 +141,8 @@ namespace pinocchio
const Scalar r_coeff, const Scalar r_coeff,
const Scalar inv_damping) const Scalar inv_damping)
{ {
TEST_NEW_ASSERT(v_before.size() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v_before.size() == model.nv);
TEST_NEW_ASSERT(J.cols() == model.nv); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(J.cols() == model.nv);
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 DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
...@@ -49,7 +49,7 @@ namespace pinocchio ...@@ -49,7 +49,7 @@ namespace pinocchio
for(JointIndex jid=1; jid<(JointIndex)model.njoints; ++jid) for(JointIndex jid=1; jid<(JointIndex)model.njoints; ++jid)
{ {
TEST_NEW_ASSERT(LEVEL>=0); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(LEVEL>=0);
dest.oMi[jid] = origin.oMi [jid]; dest.oMi[jid] = origin.oMi [jid];
if(LEVEL>=1) if(LEVEL>=1)
......
...@@ -178,7 +178,7 @@ namespace pinocchio ...@@ -178,7 +178,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q) const Eigen::MatrixBase<ConfigVectorType> & q)
{ {
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"); PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
...@@ -206,7 +206,7 @@ namespace pinocchio ...@@ -206,7 +206,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q) const Eigen::MatrixBase<ConfigVectorType> & q)
{ {
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");