Skip to content
GitLab
Menu
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
3affc20f
Unverified
Commit
3affc20f
authored
Oct 21, 2019
by
Justin Carpentier
Committed by
GitHub
Oct 21, 2019
Browse files
Merge pull request #910 from wxmerkt/wxm-if-throw-exception
Use if() and throw exception on everything apart from model.check(data)
parents
d03ebf31
e8bdc8f3
Changes
26
Hide whitespace changes
Inline
Side-by-side
src/algorithm/aba-derivatives.hxx
View file @
3affc20f
...
...
@@ -309,7 +309,7 @@ namespace pinocchio
}
// Restore the status of dAdq_cols (remove gravity)
assert
(
model
.
gravity
.
angular
().
isZero
()
&&
"The gravity must be a pure force vector, no angular part"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
model
.
gravity
.
angular
().
isZero
()
,
"The gravity must be a pure force vector, no angular part"
);
for
(
Eigen
::
DenseIndex
k
=
0
;
k
<
jmodel
.
nv
();
++
k
)
{
MotionRef
<
typename
ColsBlock
::
ColXpr
>
min
(
J_cols
.
col
(
k
));
...
...
@@ -339,15 +339,15 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
MatrixType2
>
&
aba_partial_dv
,
const
Eigen
::
MatrixBase
<
MatrixType3
>
&
aba_partial_dtau
)
{
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
assert
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
assert
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
assert
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
tau
.
size
()
==
model
.
nv
,
"The joint acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -408,16 +408,16 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
MatrixType2
>
&
aba_partial_dv
,
const
Eigen
::
MatrixBase
<
MatrixType3
>
&
aba_partial_dtau
)
{
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
assert
(
fext
.
size
()
==
(
size_t
)
model
.
njoints
&&
"The size of the external forces is not of right size"
);
assert
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
assert
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
assert
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
assert
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
tau
.
size
()
==
model
.
nv
,
"The joint acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
fext
.
size
()
==
(
size_t
)
model
.
njoints
,
"The size of the external forces is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dq
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dv
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dtau
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
src/algorithm/aba.hxx
View file @
3affc20f
...
...
@@ -223,9 +223,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
tau
.
size
()
==
model
.
nv
,
"The joint acceleration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -268,9 +268,9 @@ namespace pinocchio
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
tau
.
size
()
==
model
.
nv
,
"The joint acceleration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -457,7 +457,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
data
.
Minv
.
template
triangularView
<
Eigen
::
Upper
>().
setZero
();
...
...
src/algorithm/center-of-mass-derivatives.hxx
View file @
3affc20f
...
...
@@ -63,7 +63,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE
(
Matrix3xOut
,
Data
::
Matrix3x
);
assert
(
vcom_partial_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
vcom_partial_dq
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
src/algorithm/center-of-mass.hxx
View file @
3affc20f
...
...
@@ -105,7 +105,7 @@ namespace pinocchio
const
bool
computeSubtreeComs
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
LEVEL
>=
0
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
LEVEL
>=
0
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -385,8 +385,8 @@ namespace pinocchio
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
((
int
)
rootSubtreeId
<
model
.
njoints
&&
"Invalid joint id."
);
assert
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
&&
"the resulting matrix does not have the right size."
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
((
int
)
rootSubtreeId
<
model
.
njoints
,
"Invalid joint id."
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
,
"the resulting matrix does not have the right size."
);
Matrix3xLike
&
Jcom_subtree
=
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix3xLike
,
res
);
...
...
@@ -423,7 +423,7 @@ namespace pinocchio
typename
Pass2
::
ArgsType
(
model
,
data
,
Jcom_subtree
,
computeSubtreeComs
));
}
assert
(
data
.
mass
[
rootSubtreeId
]
>
0.
&&
"The mass of the subtree is not positive."
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
data
.
mass
[
rootSubtreeId
]
>
0.
,
"The mass of the subtree is not positive."
);
const
Scalar
mass_inv_subtree
=
Scalar
(
1
)
/
data
.
mass
[
rootSubtreeId
];
typename
Data
::
Vector3
&
com_subtree
=
data
.
com
[
rootSubtreeId
];
if
(
not
computeSubtreeComs
)
...
...
@@ -461,8 +461,8 @@ namespace pinocchio
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(((
int
)
rootSubtreeId
<
model
.
njoints
)
&&
"Invalid joint id."
);
assert
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
&&
"the resulting matrix does not have the right size."
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(((
int
)
rootSubtreeId
<
model
.
njoints
)
,
"Invalid joint id."
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
res
.
rows
()
==
3
&&
res
.
cols
()
==
model
.
nv
,
"the resulting matrix does not have the right size."
);
Matrix3xLike
&
Jcom_subtree
=
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix3xLike
,
res
);
...
...
src/algorithm/centroidal-derivatives.hxx
View file @
3affc20f
...
...
@@ -199,7 +199,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector3Like
,
3
)
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
(
Matrix6xLikeOut
,
6
,
Eigen
::
Dynamic
)
assert
(
Fin
.
cols
()
==
Fout
.
cols
()
&&
"Fin and Fout do not have the same number of columns"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Fin
.
cols
()
==
Fout
.
cols
()
,
"Fin and Fout do not have the same number of columns"
);
for
(
Eigen
::
DenseIndex
k
=
0
;
k
<
Fin
.
cols
();
++
k
)
{
...
...
@@ -227,17 +227,17 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6xLike2
>
&
dhdot_dv
,
const
Eigen
::
MatrixBase
<
Matrix6xLike3
>
&
dhdot_da
)
{
assert
(
q
.
size
()
==
model
.
nq
&&
"The joint configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
a
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
assert
(
dh_dq
.
cols
()
==
model
.
nv
);
assert
(
dh_dq
.
rows
()
==
6
);
assert
(
dhdot_dq
.
cols
()
==
model
.
nv
);
assert
(
dhdot_dq
.
rows
()
==
6
);
assert
(
dhdot_dv
.
cols
()
==
model
.
nv
);
assert
(
dhdot_dv
.
rows
()
==
6
);
assert
(
dhdot_da
.
cols
()
==
model
.
nv
);
assert
(
dhdot_da
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The joint configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
a
.
size
()
==
model
.
nv
,
"The joint acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dh_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dh_dq
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dq
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dv
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_da
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_da
.
rows
()
==
6
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
@@ -357,12 +357,12 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6xLike2
>
&
dhdot_dv
,
const
Eigen
::
MatrixBase
<
Matrix6xLike3
>
&
dhdot_da
)
{
assert
(
dhdot_dq
.
cols
()
==
model
.
nv
);
assert
(
dhdot_dq
.
rows
()
==
6
);
assert
(
dhdot_dv
.
cols
()
==
model
.
nv
);
assert
(
dhdot_dv
.
rows
()
==
6
);
assert
(
dhdot_da
.
cols
()
==
model
.
nv
);
assert
(
dhdot_da
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dq
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dq
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dv
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_dv
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_da
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dhdot_da
.
rows
()
==
6
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
src/algorithm/centroidal.hxx
View file @
3affc20f
...
...
@@ -122,8 +122,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -167,9 +167,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
a
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
assert
(
a
.
size
()
==
model
.
nv
&&
"The acceleration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
a
.
size
()
==
model
.
nv
,
"The acceleration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -253,8 +253,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -346,8 +346,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
src/algorithm/cholesky.hxx
View file @
3affc20f
...
...
@@ -88,7 +88,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
)
PINOCCHIO_UNUSED_VARIABLE
(
model
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
Mat
&
v_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Mat
,
v
);
...
...
@@ -144,7 +144,7 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE
(
model
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
Mat
&
v_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Mat
,
v
);
const
typename
Data
::
MatrixXs
&
U
=
data
.
U
;
...
...
@@ -196,7 +196,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
)
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
Mat
&
v_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Mat
,
v
);
const
typename
Data
::
MatrixXs
&
U
=
data
.
U
;
...
...
@@ -250,7 +250,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
)
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
Mat
&
v_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Mat
,
v
);
const
typename
Data
::
MatrixXs
&
U
=
data
.
U
;
...
...
@@ -307,8 +307,8 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE
(
model
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
vin
.
size
()
==
model
.
nv
);
assert
(
vout
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
vin
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
vout
.
size
()
==
model
.
nv
);
MatRes
&
vout_
=
PINOCCHIO_EIGEN_CONST_CAST
(
MatRes
,
vout
);
...
...
@@ -373,7 +373,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Mat
)
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
Mat
&
v_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Mat
,
v
);
...
...
@@ -456,8 +456,8 @@ namespace pinocchio
PINOCCHIO_UNUSED_VARIABLE
(
model
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
col
<
model
.
nv
&&
col
>=
0
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
col
<
model
.
nv
&&
col
>=
0
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -491,8 +491,8 @@ namespace pinocchio
const
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data
,
const
Eigen
::
MatrixBase
<
Mat
>
&
Minv
)
{
assert
(
Minv
.
rows
()
==
model
.
nv
);
assert
(
Minv
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Minv
.
rows
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Minv
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
...
...
src/algorithm/compute-all-terms.hpp
View file @
3affc20f
...
...
@@ -206,8 +206,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/contact-dynamics.hxx
View file @
3affc20f
...
...
@@ -25,9 +25,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
DriftVectorType
>
&
gamma
,
const
Scalar
inv_damping
)
{
assert
(
tau
.
size
()
==
model
.
nv
);
assert
(
J
.
cols
()
==
model
.
nv
);
assert
(
J
.
rows
()
==
gamma
.
size
());
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
tau
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
J
.
rows
()
==
gamma
.
size
());
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -78,8 +78,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
DriftVectorType
>
&
gamma
,
const
Scalar
inv_damping
)
{
assert
(
q
.
size
()
==
model
.
nq
);
assert
(
v
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
);
computeAllTerms
(
model
,
data
,
q
,
v
);
...
...
@@ -94,8 +94,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
KKTMatrixType
>
&
MJtJ_inv
)
{
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
assert
(
MJtJ_inv
.
cols
()
==
data
.
JMinvJt
.
cols
()
+
model
.
nv
);
assert
(
MJtJ_inv
.
rows
()
==
data
.
JMinvJt
.
rows
()
+
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
MJtJ_inv
.
cols
()
==
data
.
JMinvJt
.
cols
()
+
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
MJtJ_inv
.
rows
()
==
data
.
JMinvJt
.
rows
()
+
model
.
nv
);
const
typename
Data
::
MatrixXs
::
Index
&
nc
=
data
.
JMinvJt
.
cols
();
KKTMatrixType
&
MJtJ_inv_
=
PINOCCHIO_EIGEN_CONST_CAST
(
KKTMatrixType
,
MJtJ_inv
);
...
...
@@ -124,7 +124,7 @@ namespace pinocchio
const
Scalar
r_coeff
,
const
Scalar
inv_damping
)
{
assert
(
q
.
size
()
==
model
.
nq
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
);
// Compute the mass matrix
crba
(
model
,
data
,
q
);
...
...
@@ -141,8 +141,8 @@ namespace pinocchio
const
Scalar
r_coeff
,
const
Scalar
inv_damping
)
{
assert
(
v_before
.
size
()
==
model
.
nv
);
assert
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v_before
.
size
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
J
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
src/algorithm/copy.hpp
View file @
3affc20f
...
...
@@ -49,7 +49,7 @@ namespace pinocchio
for
(
JointIndex
jid
=
1
;
jid
<
(
JointIndex
)
model
.
njoints
;
++
jid
)
{
assert
(
LEVEL
>=
0
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
LEVEL
>=
0
);
dest
.
oMi
[
jid
]
=
origin
.
oMi
[
jid
];
if
(
LEVEL
>=
1
)
...
...
src/algorithm/crba.hxx
View file @
3affc20f
...
...
@@ -178,7 +178,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
@@ -206,7 +206,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
JointIndex
JointIndex
;
...
...
src/algorithm/energy.hpp
View file @
3affc20f
...
...
@@ -71,8 +71,8 @@ namespace pinocchio
const
bool
update_kinematics
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
@@ -98,7 +98,7 @@ namespace pinocchio
const
bool
update_kinematics
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/frames.hxx
View file @
3affc20f
...
...
@@ -107,8 +107,9 @@ namespace pinocchio
const
ReferenceFrame
rf
,
const
Eigen
::
MatrixBase
<
Matrix6xLike
>
&
J
)
{
assert
(
J
.
rows
()
==
6
);
assert
(
J
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
J
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
J
.
cols
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
@@ -157,7 +158,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6Like
>
&
J
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
Frame
Frame
;
...
...
@@ -191,8 +192,8 @@ namespace pinocchio
const
ReferenceFrame
rf
,
const
Eigen
::
MatrixBase
<
Matrix6xLike
>
&
dJ
)
{
assert
(
dJ
.
rows
()
==
data
.
dJ
.
rows
()
);
assert
(
dJ
.
cols
()
==
data
.
dJ
.
cols
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dJ
.
rows
()
==
data
.
dJ
.
rows
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
dJ
.
cols
()
==
data
.
dJ
.
cols
()
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
...
...
src/algorithm/geometry.hxx
View file @
3affc20f
...
...
@@ -54,12 +54,12 @@ namespace pinocchio
GeometryData
&
geomData
,
const
PairIndex
&
pairId
)
{
assert
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
const
CollisionPair
&
pair
=
geomModel
.
collisionPairs
[
pairId
];
assert
(
pairId
<
geomData
.
collisionResults
.
size
()
);
assert
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
assert
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pairId
<
geomData
.
collisionResults
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
fcl
::
CollisionResult
&
collisionResult
=
geomData
.
collisionResults
[
pairId
];
collisionResult
.
clear
();
...
...
@@ -115,12 +115,12 @@ namespace pinocchio
GeometryData
&
geomData
,
const
PairIndex
&
pairId
)
{
assert
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pairId
<
geomModel
.
collisionPairs
.
size
()
);
const
CollisionPair
&
pair
=
geomModel
.
collisionPairs
[
pairId
];
assert
(
pairId
<
geomData
.
distanceResults
.
size
()
);
assert
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
assert
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pairId
<
geomData
.
distanceResults
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pair
.
first
<
geomData
.
collisionObjects
.
size
()
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
pair
.
second
<
geomData
.
collisionObjects
.
size
()
);
geomData
.
distanceResults
[
pairId
].
clear
();
fcl
::
distance
(
&
geomData
.
collisionObjects
[
pair
.
first
],
...
...
src/algorithm/jacobian.hxx
View file @
3affc20f
...
...
@@ -54,7 +54,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
@@ -121,11 +121,11 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6xLikeIn
>
&
Jin
,
const
Eigen
::
MatrixBase
<
Matrix6xLikeOut
>
&
Jout
)
{
assert
(
Jin
.
rows
()
==
6
);
assert
(
Jin
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Jin
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Jin
.
cols
()
==
model
.
nv
);
assert
(
Jout
.
rows
()
==
6
);
assert
(
Jout
.
cols
()
==
model
.
nv
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Jout
.
rows
()
==
6
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
Jout
.
cols
()
==
model
.
nv
);
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
...
...
@@ -177,7 +177,7 @@ namespace pinocchio
break
;
}
default:
assert
(
false
&&
"must never happened"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
false
,
"must never happened"
);
break
;
}
}
...
...
@@ -243,7 +243,7 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Matrix6xLike
>
&
J
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
@@ -324,8 +324,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The velocity vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The velocity vector is not of right size"
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
...
...
src/algorithm/joint-configuration.hxx
View file @
3affc20f
...
...
@@ -26,9 +26,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
,
const
Eigen
::
MatrixBase
<
ReturnType
>
&
qout
)
{
assert
(
q
.
size
()
==
model
.
nq
&&
"The configuration vector is not of the right size"
);
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of the right size"
);
assert
(
qout
.
size
()
==
model
.
nq
&&
"The output argument is not of the right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
q
.
size
()
==
model
.
nq
,
"The configuration vector is not of the right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
v
.
size
()
==
model
.
nv
,
"The joint velocity vector is not of the right size"
);
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
qout
.
size
()
==
model
.
nq
,
"The output argument is not of the right size"
);