Skip to content

GitLab

Verified Commit be40bbd6 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

all: use new PINOCCHIO_CHECK_ARGUMENT_SIZE

parent dc43add2
Pipeline #10803 passed with stage
in 127 minutes and 35 seconds
Hide whitespace changes
Inline Side-by-side
src/algorithm/aba-derivatives.hxx
View file @ be40bbd6
//
// Copyright (c) 2018 CNRS, INRIA
// Copyright (c) 2018-2020 CNRS INRIA
//
#ifndef __pinocchio_aba_derivatives_hxx__
#define __pinocchio_aba_derivatives_hxx__
#ifndef __pinocchio_algorithm_aba_derivatives_hxx__
#define __pinocchio_algorithm_aba_derivatives_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/check.hpp"
......@@ -340,15 +340,15 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
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 torque 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);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -409,16 +409,16 @@ namespace pinocchio
const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
{
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 torque vector is not of right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(fext.size() == (size_t)model.njoints, "The external forces 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);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), (size_t)model.njoints, "The external forces vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -472,5 +472,4 @@ namespace pinocchio
} // namespace pinocchio
#endif // ifndef __pinocchio_aba_derivatives_hxx__
#endif // ifndef __pinocchio_algorithm_aba_derivatives_hxx__
src/algorithm/aba.hxx
View file @ be40bbd6
......@@ -195,9 +195,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType2> & tau)
{
assert(model.check(data) && "data is not consistent with model.");
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 torque vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -240,9 +240,9 @@ namespace pinocchio
{
assert(model.check(data) && "data is not consistent with model.");
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 torque vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......@@ -429,7 +429,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -63,7 +63,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(vcom_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -371,7 +371,8 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
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.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), model.nv, "the resulting matrix does not have the right size.");
Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
......@@ -447,7 +448,8 @@ namespace pinocchio
assert(model.check(data) && "data is not consistent with model.");
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.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -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_CHECK_INPUT_ARGUMENT(Fin.cols() == Fout.cols(), "Fin and Fout do not have the same number of columns");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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)
{
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);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The joint acceleration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -354,12 +354,12 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
{
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);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -144,8 +144,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -188,7 +188,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -274,8 +274,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -330,8 +330,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(vin.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(vout.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v);
......@@ -457,7 +457,7 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE(model);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(Minv.rows() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Minv.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
......
src/algorithm/compute-all-terms.hpp
View file @ be40bbd6
......@@ -207,8 +207,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -25,9 +25,9 @@ namespace pinocchio
const Eigen::MatrixBase<DriftVectorType> & gamma,
const Scalar inv_damping)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(tau.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == gamma.size());
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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;
PINOCCHIO_CHECK_INPUT_ARGUMENT(MJtJ_inv.cols() == data.JMinvJt.cols() + model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(MJtJ_inv.rows() == data.JMinvJt.rows() + model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(MJtJ_inv.cols(), data.JMinvJt.cols() + model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq);
PINOCCHIO_CHECK_ARGUMENT_SIZE(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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_before.size() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
src/algorithm/crba.hxx
View file @ be40bbd6
......@@ -178,7 +178,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
......
src/algorithm/frames-derivatives.hxx
View file @ be40bbd6
......@@ -28,8 +28,8 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames");
......@@ -108,10 +108,10 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Matrix6x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_da.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames");
......
src/algorithm/frames.hxx
View file @ be40bbd6
......@@ -145,8 +145,8 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike> & J)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
......@@ -173,7 +173,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike> & J)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......
src/algorithm/jacobian.hxx
View file @ be40bbd6
......@@ -57,7 +57,7 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
......@@ -125,9 +125,9 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == Jout.cols());
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
......@@ -158,11 +158,11 @@ namespace pinocchio
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == model.nv);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
......@@ -290,7 +290,7 @@ namespace pinocchio
const Eigen::MatrixBase<Matrix6xLike> & J)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -371,8 +371,8 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v)
{
assert(model.check(data) && "data is not consistent with model.");
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_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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 @ be40bbd6
......@@ -21,9 +21,9 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<ReturnType> & qout)
{
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");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -45,9 +45,9 @@ namespace pinocchio
const Scalar & u,
const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -68,9 +68,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & q1,
const Eigen::MatrixBase<ReturnType> & dvout)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(dvout.size() == model.nv, "The output argument is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(dvout.size(), model.nv, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -91,9 +91,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & q1,
const Eigen::MatrixBase<ReturnType> & out)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(out.size() == (model.njoints-1), "The output argument is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q0.size(), model.nq, "The first configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(q1.size(), model.nq, "The second configuration vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(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;
......@@ -114,9 +114,9 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
const Eigen::MatrixBase<ReturnType> & qout)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(lowerLimits.size() == model.nq, "The lower limits vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(upperLimits.size() == model.nq, "The upper limits vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(qout.size() == model.nq, "The output argument is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(lowerLimits.size(), model.nq, "The lower limits vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(upperLimits.size(), model.nq, "The upper limits vector is not of the right size");
PINOCCHIO_CHECK_ARGUMENT_SIZE(qout.size(), model.nq, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
......@@ -134,7 +134,7 @@ namespace pinocchio
void
neutral(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, const Eigen::MatrixBase<ReturnType> & qout)
{