Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba-derivatives.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/aba.hxx
View file @ fb3505ad
......@@ -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();
......
src/algorithm/center-of-mass-derivatives.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/center-of-mass.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/centroidal-derivatives.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/centroidal.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/cholesky.hxx
View file @ fb3505ad
......@@ -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);
......
src/algorithm/compute-all-terms.hpp
View file @ fb3505ad
......@@ -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;
......
src/algorithm/contact-dynamics.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/copy.hpp
View file @ fb3505ad
......@@ -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)
......
src/algorithm/crba.hxx
View file @ fb3505ad
......@@ -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;
......
src/algorithm/energy.hpp
View file @ fb3505ad
......@@ -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;
......
src/algorithm/frames.hxx
View file @ fb3505ad
......@@ -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,