Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
54503f81
Commit
54503f81
authored
Oct 16, 2019
by
Wolfgang Merkt
Browse files
Rename TEST_NEW_ASSERT to PINOCCHIO_ASSERT_THROW_AT_RUNTIME
parent
c0a0fa72
Changes
66
Hide whitespace changes
Inline
Side-by-side
src/algorithm/aba-derivatives.hxx
View file @
54503f81
...
...
@@ -309,7 +309,7 @@ namespace pinocchio
}
// Restore the status of dAdq_cols (remove gravity)
TEST_NEW_ASSERT
(
model
.
gravity
.
angular
().
isZero
()
&&
"The gravity must be a pure force vector, no angular part"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
model
.
gravity
.
angular
().
isZero
()
&&
"The gravity must be a pure force vector, no angular part"
);
for
(
Eigen
::
DenseIndex
k
=
0
;
k
<
jmodel
.
nv
();
++
k
)
{
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
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
TEST_NEW_ASSERT
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
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
(
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
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
TEST_NEW_ASSERT
(
fext
.
size
()
==
(
size_t
)
model
.
njoints
&&
"The size of the external forces is not of right size"
);
TEST_NEW_ASSERT
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
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
(
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
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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 @
54503f81
...
...
@@ -223,9 +223,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
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"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -268,9 +268,9 @@ namespace pinocchio
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
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"
);
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
data
.
Minv
.
template
triangularView
<
Eigen
::
Upper
>().
setZero
();
...
...
src/algorithm/center-of-mass-derivatives.hxx
View file @
54503f81
...
...
@@ -63,7 +63,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE
(
Matrix3xOut
,
Data
::
Matrix3x
);
TEST_NEW_ASSERT
(
vcom_partial_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
vcom_partial_dq
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
src/algorithm/center-of-mass.hxx
View file @
54503f81
...
...
@@ -105,7 +105,7 @@ namespace pinocchio
const
bool
computeSubtreeComs
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
LEVEL
>=
0
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
LEVEL
>=
0
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
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."
);
TEST_NEW_ASSERT
((
int
)
rootSubtreeId
<
model
.
njoints
&&
"Invalid joint id."
);
TEST_NEW_ASSERT
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
&&
"the resulting matrix does not have the right size."
);
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."
);
Matrix3xLike
&
Jcom_subtree
=
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix3xLike
,
res
);
...
...
@@ -423,7 +423,7 @@ namespace pinocchio
typename
Pass2
::
ArgsType
(
model
,
data
,
Jcom_subtree
,
computeSubtreeComs
));
}
TEST_NEW_ASSERT
(
data
.
mass
[
rootSubtreeId
]
>
0.
&&
"The mass of the subtree is not positive."
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
data
.
mass
[
rootSubtreeId
]
>
0.
&&
"The mass of the subtree is not positive."
);
const
Scalar
mass_inv_subtree
=
Scalar
(
1
)
/
data
.
mass
[
rootSubtreeId
];
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."
);
TEST_NEW_ASSERT
(((
int
)
rootSubtreeId
<
model
.
njoints
)
&&
"Invalid joint id."
);
TEST_NEW_ASSERT
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
&&
"the resulting matrix does not have the right size."
);
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."
);
Matrix3xLike
&
Jcom_subtree
=
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix3xLike
,
res
);
...
...
src/algorithm/centroidal-derivatives.hxx
View file @
54503f81
...
...
@@ -199,7 +199,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector3Like
,
3
)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
(
Matrix6xLikeOut
,
6
,
Eigen
::
Dynamic
)
TEST_NEW_ASSERT
(
Fin
.
cols
()
==
Fout
.
cols
()
&&
"Fin and Fout do not have the same number of columns"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Fin
.
cols
()
==
Fout
.
cols
()
&&
"Fin and Fout do not have the same number of columns"
);
for
(
Eigen
::
DenseIndex
k
=
0
;
k
<
Fin
.
cols
();
++
k
)
{
...
...
@@ -227,17 +227,17 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6xLike2
>
&
dhdot_dv
,
const
Eigen
::
MatrixBase
<
Matrix6xLike3
>
&
dhdot_da
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
a
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
TEST_NEW_ASSERT
(
dh_dq
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dh_dq
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
dhdot_dq
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_dq
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
dhdot_dv
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_dv
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
dhdot_da
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_da
.
rows
()
==
6
);
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
(
dh_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dh_dq
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dq
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dv
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_da
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
dhdot_dq
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_dq
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
dhdot_dv
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_dv
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
dhdot_da
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
dhdot_da
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dq
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_dv
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dhdot_da
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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 @
54503f81
...
...
@@ -122,8 +122,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
TEST_NEW_ASSERT
(
a
.
size
()
==
model
.
nv
&&
"The acceleration vector is not of right size"
);
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"
);
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
src/algorithm/cholesky.hxx
View file @
54503f81
...
...
@@ -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."
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
vin
.
size
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
vout
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
vin
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
col
<
model
.
nv
&&
col
>=
0
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
col
<
model
.
nv
&&
col
>=
0
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
Minv
.
rows
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
Minv
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Minv
.
rows
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Minv
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
...
...
src/algorithm/compute-all-terms.hpp
View file @
54503f81
...
...
@@ -206,8 +206,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/contact-dynamics.hxx
View file @
54503f81
...
...
@@ -25,9 +25,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
DriftVectorType
>
&
gamma
,
const
Scalar
inv_damping
)
{
TEST_NEW_ASSERT
(
tau
.
size
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
J
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
J
.
rows
()
==
gamma
.
size
());
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
tau
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
;
TEST_NEW_ASSERT
(
MJtJ_inv
.
cols
()
==
data
.
JMinvJt
.
cols
()
+
model
.
nv
);
TEST_NEW_ASSERT
(
MJtJ_inv
.
rows
()
==
data
.
JMinvJt
.
rows
()
+
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
MJtJ_inv
.
cols
()
==
data
.
JMinvJt
.
cols
()
+
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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
)
{
TEST_NEW_ASSERT
(
v_before
.
size
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
v_before
.
size
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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 @
54503f81
...
...
@@ -49,7 +49,7 @@ namespace pinocchio
for
(
JointIndex
jid
=
1
;
jid
<
(
JointIndex
)
model
.
njoints
;
++
jid
)
{
TEST_NEW_ASSERT
(
LEVEL
>=
0
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
LEVEL
>=
0
);
dest
.
oMi
[
jid
]
=
origin
.
oMi
[
jid
];
if
(
LEVEL
>=
1
)
...
...
src/algorithm/crba.hxx
View file @
54503f81
...
...
@@ -178,7 +178,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -206,7 +206,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
src/algorithm/energy.hpp
View file @
54503f81
...
...
@@ -71,8 +71,8 @@ namespace pinocchio
const
bool
update_kinematics
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/frames.hxx
View file @
54503f81
...
...
@@ -107,9 +107,9 @@ namespace pinocchio
const
ReferenceFrame
rf
,
const
Eigen
::
MatrixBase
<
Matrix6xLike
>
&
J
)
{
TEST_NEW_ASSERT
(
J
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
J
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
data
.
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
J
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
data
.
J
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
@@ -157,7 +157,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6Like
>
&
J
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
Frame
Frame
;
...
...
@@ -191,8 +191,8 @@ namespace pinocchio
const
ReferenceFrame
rf
,
const
Eigen
::
MatrixBase
<
Matrix6xLike
>
&
dJ
)
{
TEST_NEW_ASSERT
(
dJ
.
rows
()
==
data
.
dJ
.
rows
()
);
TEST_NEW_ASSERT
(
dJ
.
cols
()
==
data
.
dJ
.
cols
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
dJ
.
rows
()
==
data
.
dJ
.
rows
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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 @
54503f81
...
...
@@ -54,12 +54,12 @@ namespace pinocchio
GeometryData
&
geomData
,
const
PairIndex
&
pairId
)
{
TEST_NEW_ASSERT
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
const
CollisionPair
&
pair
=
geomModel
.
collisionPairs
[
pairId
];
TEST_NEW_ASSERT
(
pairId
<
geomData
.
collisionResults
.
size
()
);
TEST_NEW_ASSERT
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
TEST_NEW_ASSERT
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
geomData
.
collisionResults
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
fcl
::
CollisionResult
&
collisionResult
=
geomData
.
collisionResults
[
pairId
];
collisionResult
.
clear
();
...
...
@@ -115,12 +115,12 @@ namespace pinocchio
GeometryData
&
geomData
,
const
PairIndex
&
pairId
)
{
TEST_NEW_ASSERT
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
const
CollisionPair
&
pair
=
geomModel
.
collisionPairs
[
pairId
];
TEST_NEW_ASSERT
(
pairId
<
geomData
.
distanceResults
.
size
()
);
TEST_NEW_ASSERT
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
TEST_NEW_ASSERT
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
geomData
.
distanceResults
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
geomData
.
distanceResults
[
pairId
].
clear
();
fcl
::
distance
(
&
geomData
.
collisionObjects
[
pair
.
first
],
...
...
src/algorithm/jacobian.hxx
View file @
54503f81
...
...
@@ -54,7 +54,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
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
)
{
TEST_NEW_ASSERT
(
Jin
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
Jin
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Jin
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Jin
.
cols
()
==
model
.
nv
);
TEST_NEW_ASSERT
(
Jout
.
rows
()
==
6
);
TEST_NEW_ASSERT
(
Jout
.
cols
()
==
model
.
nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Jout
.
rows
()
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
Jout
.
cols
()
==
model
.
nv
);
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -177,7 +177,7 @@ namespace pinocchio
break
;
}
default:
TEST_NEW_ASSERT
(
false
&&
"must never happened"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
typedef
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."
);
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
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"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/joint-configuration.hxx
View file @
54503f81
...
...
@@ -26,9 +26,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
,
const
Eigen
::
MatrixBase
<
ReturnType
>
&
qout
)
{
TEST_NEW_ASSERT
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of the right size"
);
TEST_NEW_ASSERT
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of the right size"
);
TEST_NEW_ASSERT
(
qout
.
size
()
==
model
.
nq
&&
"The output argument is not of the right size"
);
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"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;