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