Skip to content

GitLab

Unverified Commit 3affc20f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #910 from wxmerkt/wxm-if-throw-exception 

Use if() and throw exception on everything apart from model.check(data)
parents d03ebf31 e8bdc8f3
Hide whitespace changes
Inline Side-by-side
Showing with 183 additions and 182 deletions
src/algorithm/aba-derivatives.hxx
View file @ 3affc20f
......@@ -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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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,15 +339,15 @@ 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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.rows() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -408,16 +408,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(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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(fext.size() == (size_t)model.njoints, "The size of the external forces is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.rows() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
src/algorithm/aba.hxx
View file @ 3affc20f
......@@ -223,9 +223,9 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv, "The joint acceleration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -268,9 +268,9 @@ namespace pinocchio
{
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv, "The joint acceleration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -457,7 +457,7 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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 @ 3affc20f
......@@ -63,7 +63,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x);
assert(vcom_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(vcom_partial_dq.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
src/algorithm/center-of-mass.hxx
View file @ 3affc20f
......@@ -105,7 +105,7 @@ namespace pinocchio
const bool computeSubtreeComs)
{
assert(model.check(data) && "data is not consistent with model.");
assert(LEVEL >= 0);
PINOCCHIO_CHECK_INPUT_ARGUMENT(LEVEL >= 0);
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -385,8 +385,8 @@ namespace pinocchio
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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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)
......@@ -461,8 +461,8 @@ 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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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);
......
src/algorithm/centroidal-derivatives.hxx
View file @ 3affc20f
......@@ -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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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,17 +227,17 @@ 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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(a.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(dh_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dh_dq.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dq.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dv.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_da.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_da.rows() == 6);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -357,12 +357,12 @@ 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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dq.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_dv.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_da.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(dhdot_da.rows() == 6);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
src/algorithm/centroidal.hxx
View file @ 3affc20f
......@@ -122,8 +122,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -167,9 +167,9 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(a.size() == model.nv, "The acceleration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -253,8 +253,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -346,8 +346,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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 @ 3affc20f
......@@ -88,7 +88,7 @@ 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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -144,7 +144,7 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
......@@ -196,7 +196,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
......@@ -250,7 +250,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
const typename Data::MatrixXs & U = data.U;
......@@ -307,8 +307,8 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
assert(vin.size() == model.nv);
assert(vout.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(vin.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(vout.size() == model.nv);
MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout);
......@@ -373,7 +373,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat)
assert(model.check(data) && "data is not consistent with model.");
assert(v.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -456,8 +456,8 @@ namespace pinocchio
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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -491,8 +491,8 @@ namespace pinocchio
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<Mat> & Minv)
{
assert(Minv.rows() == model.nv);
assert(Minv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Minv.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Minv.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
......
src/algorithm/compute-all-terms.hpp
View file @ 3affc20f
......@@ -206,8 +206,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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 @ 3affc20f
......@@ -25,9 +25,9 @@ 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());
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == gamma.size());
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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);
PINOCCHIO_CHECK_INPUT_ARGUMENT(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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(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);
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);
// Compute the mass matrix
crba(model, data, q);
......@@ -141,8 +141,8 @@ namespace pinocchio
const Scalar r_coeff,
const Scalar inv_damping)
{
assert(v_before.size() == model.nv);
assert(J.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_before.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
src/algorithm/copy.hpp
View file @ 3affc20f
......@@ -49,7 +49,7 @@ namespace pinocchio
for(JointIndex jid=1; jid<(JointIndex)model.njoints; ++jid)
{
assert(LEVEL>=0);
PINOCCHIO_CHECK_INPUT_ARGUMENT(LEVEL>=0);
dest.oMi[jid] = origin.oMi [jid];
if(LEVEL>=1)
......
src/algorithm/crba.hxx
View file @ 3affc20f
......@@ -178,7 +178,7 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -206,7 +206,7 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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 @ 3affc20f
......@@ -71,8 +71,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -98,7 +98,7 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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 @ 3affc20f
......@@ -107,8 +107,9 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & J)
{
assert(J.rows() == 6);
assert(J.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -157,7 +158,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6Like> & J)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Frame Frame;
......@@ -191,8 +192,8 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
{
assert( dJ.rows() == data.dJ.rows() );
assert( dJ.cols() == data.dJ.cols() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( dJ.rows() == data.dJ.rows() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( dJ.cols() == data.dJ.cols() );
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
src/algorithm/geometry.hxx
View file @ 3affc20f
......@@ -54,12 +54,12 @@ namespace pinocchio
GeometryData & geomData,
const PairIndex & pairId)
{
assert( pairId < geomModel.collisionPairs.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
assert( pairId < geomData.collisionResults.size() );
assert( pair.first < geomData.collisionObjects.size() );
assert( pair.second < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomData.collisionResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geomData.collisionObjects.size() );
fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
collisionResult.clear();
......@@ -115,12 +115,12 @@ namespace pinocchio
GeometryData & geomData,
const PairIndex & pairId)
{
assert( pairId < geomModel.collisionPairs.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomModel.collisionPairs.size() );
const CollisionPair & pair = geomModel.collisionPairs[pairId];
assert( pairId < geomData.distanceResults.size() );
assert( pair.first < geomData.collisionObjects.size() );
assert( pair.second < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pairId < geomData.distanceResults.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geomData.collisionObjects.size() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geomData.collisionObjects.size() );
geomData.distanceResults[pairId].clear();
fcl::distance ( &geomData.collisionObjects[pair.first],
......
src/algorithm/jacobian.hxx
View file @ 3affc20f
......@@ -54,7 +54,7 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -121,11 +121,11 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
{
assert(Jin.rows() == 6);
assert(Jin.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == model.nv);
assert(Jout.rows() == 6);
assert(Jout.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == model.nv);
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -177,7 +177,7 @@ namespace pinocchio
break;
}
default:
assert(false && "must never happened");
PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened");
break;
}
}
......@@ -243,7 +243,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike> & J)
{
assert(model.check(data) && "data is not consistent with model.");
assert(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -324,8 +324,8 @@ namespace pinocchio
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");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(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/joint-configuration.hxx
View file @ 3affc20f
......@@ -26,9 +26,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<ReturnType> & qout)
{
assert(q.size() == model.nq && "The configuration vector is not of the right size");
assert(v.size() == model.nv && "The joint velocity vector is not of the right size");
assert(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");