Skip to content

GitLab

Commit 711c25ab authored by Wolfgang Merkt's avatar Wolfgang Merkt
Browse files

Implement assert-like macro with message

parent 98735e92
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba-derivatives.hxx
View file @ 711c25ab
......@@ -309,7 +309,7 @@ namespace pinocchio
}
// Restore the status of dAdq_cols (remove gravity)
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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,9 +339,9 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(tau.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.rows() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.cols() == model.nv);
......@@ -408,10 +408,10 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector 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");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(tau.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(fext.size() == (size_t)model.njoints, "The size of the external forces is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dq.rows() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(aba_partial_dv.cols() == model.nv);
......
src/algorithm/aba.hxx
View file @ 711c25ab
......@@ -223,9 +223,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & tau)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(tau.size() == model.nv && "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.hxx
View file @ 711c25ab
......@@ -385,8 +385,8 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME((int)rootSubtreeId < model.njoints && "Invalid joint id.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE((int)rootSubtreeId < model.njoints, "Invalid joint id.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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));
}
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(data.mass[rootSubtreeId] > 0. && "The mass of the subtree is not positive.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(((int)rootSubtreeId < model.njoints) && "Invalid joint id.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(res.rows() == 3 && res.cols() == model.nv && "the resulting matrix does not have the right size.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(((int)rootSubtreeId < model.njoints), "Invalid joint id.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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 @ 711c25ab
......@@ -199,7 +199,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut,6,Eigen::Dynamic)
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(Fin.cols() == Fout.cols() && "Fin and Fout do not have the same number of columns");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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,9 +227,9 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dh_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dh_dq.rows() == 6);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dhdot_dq.cols() == model.nv);
......
src/algorithm/centroidal.hxx
View file @ 711c25ab
......@@ -122,8 +122,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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/compute-all-terms.hpp
View file @ 711c25ab
......@@ -206,8 +206,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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/crba.hxx
View file @ 711c25ab
......@@ -178,7 +178,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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 @ 711c25ab
......@@ -71,8 +71,8 @@ namespace pinocchio
const bool update_kinematics)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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 @ 711c25ab
......@@ -157,7 +157,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6Like> & J)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::Frame Frame;
......
src/algorithm/jacobian.hxx
View file @ 711c25ab
......@@ -54,7 +54,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -177,7 +177,7 @@ namespace pinocchio
break;
}
default:
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(false && "must never happened");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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 @ 711c25ab
......@@ -26,9 +26,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(qout.size() == model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -50,9 +50,9 @@ namespace pinocchio
const Scalar & u,
const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q0.size() == model.nq && "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q1.size() == model.nq && "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(qout.size() == model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -73,9 +73,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & q1,
const Eigen::MatrixBase<ReturnType> & dvout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q0.size() == model.nq && "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q1.size() == model.nq && "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(dvout.size() == model.nv && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(dvout.size() == model.nv, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -96,9 +96,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & q1,
const Eigen::MatrixBase<ReturnType> & out)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q0.size() == model.nq && "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q1.size() == model.nq && "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(out.size() == (model.njoints-1) && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(out.size() == (model.njoints-1), "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -119,9 +119,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(lowerLimits.size() == model.nq && "The lower limits vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(upperLimits.size() == model.nq && "The upper limits vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(lowerLimits.size() == model.nq, "The lower limits vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(upperLimits.size() == model.nq, "The upper limits vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(qout.size() == model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -139,7 +139,7 @@ namespace pinocchio
void
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(qout.size() == model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -159,10 +159,10 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(J.rows() == model.nv && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(J.cols() == model.nv && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(J.rows() == model.nv, "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(J.cols() == model.nv, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -181,8 +181,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn1> & q0,
const Eigen::MatrixBase<ConfigVectorIn2> & q1)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q0.size() == model.nq && "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q1.size() == model.nq && "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q1.size() == model.nq, "The second configuration vector is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -212,7 +212,7 @@ namespace pinocchio
inline void normalize(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & qout)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(qout.size() == model.nq && "The output argument is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(qout.size() == model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -232,9 +232,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & q2,
const Scalar & prec)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q1.size() == model.nq && "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q2.size() == model.nq && "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(prec >= 0 && "The precision is negative");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q1.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q2.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(prec >= 0, "The precision is negative");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -258,7 +258,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVector> & q,
const Eigen::MatrixBase<JacobianMatrix> & jacobian)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(jacobian.rows() == model.nq && jacobian.cols() == model.nv
&& "The jacobian does not have the right dimension");
......
src/algorithm/kinematics-derivatives.hxx
View file @ 711c25ab
......@@ -82,9 +82,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The acceleration vector is not of right size");
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......
src/algorithm/kinematics.hxx
View file @ 711c25ab
......@@ -105,7 +105,7 @@ namespace pinocchio
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -167,8 +167,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -236,9 +236,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The acceleration vector is not of right size");
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
src/algorithm/rnea-derivatives.hxx
View file @ 711c25ab
......@@ -127,7 +127,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(gravity_partial_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(gravity_partial_dq.rows() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
......@@ -351,7 +351,7 @@ namespace pinocchio
}
// Restore the status of dAdq_cols (remove gravity)
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(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));
......@@ -382,9 +382,9 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dq.rows() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dv.cols() == model.nv);
......@@ -429,10 +429,10 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The joint acceleration vector 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");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(fext.size() == (size_t)model.njoints, "The size of the external forces is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dq.cols() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dq.rows() == model.nv);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(rnea_partial_dv.cols() == model.nv);
......
src/algorithm/rnea.hxx
View file @ 711c25ab
......@@ -101,9 +101,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & a)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The acceleration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -141,9 +141,9 @@ namespace pinocchio
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(fext.size() == model.joints.size());
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(a.size() == model.nv && "The acceleration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(a.size() == model.nv, "The acceleration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -246,8 +246,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(v.size() == model.nv && "The velocity vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(v.size() == model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -340,7 +340,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(q.size() == model.nq && "The configuration vector is not of right size");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(q.size() == model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......
src/math/rotation.hpp
View file @ 711c25ab
......@@ -25,7 +25,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(isUnitary(axis) && "The axis is not unitary.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(isUnitary(axis), "The axis is not unitary.");
Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
Vector3 sin_axis = sin_value * axis;
......
src/multibody/fcl.hxx
View file @ 711c25ab
......@@ -20,7 +20,7 @@ namespace pinocchio
inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
: Base(co1,co2)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME(co1 != co2 && "The index of collision objects must not be equal.");
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE(co1 != co2, "The index of collision objects must not be equal.");
}
inline bool CollisionPair::operator== (const CollisionPair& rhs) const
......
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @ 711c25ab
......@@ -471,7 +471,7 @@ namespace pinocchio
: axis(x,y,z)
{
axis.normalize();