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
08fb287d
Verified
Commit
08fb287d
authored
Dec 20, 2019
by
Justin Carpentier
Browse files
data: init all the quantities
This will make easy the serialization of the Data struct
parent
c8ada6ea
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/multibody/data.hpp
View file @
08fb287d
...
...
@@ -142,7 +142,7 @@ namespace pinocchio
container
::
aligned_vector
<
Inertia
>
Ycrb
;
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
container
::
aligned_vector
<
typename
Inertia
::
Matrix6
>
dYcrb
;
// TODO: change with dense symmetric matrix6
container
::
aligned_vector
<
Matrix6
>
dYcrb
;
// TODO: change with dense symmetric matrix6
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs
M
;
...
...
@@ -175,19 +175,19 @@ namespace pinocchio
Matrix6x
IS
;
/// \brief Right variation of the inertia matrix
container
::
aligned_vector
<
typename
Inertia
::
Matrix6
>
vxI
;
container
::
aligned_vector
<
Matrix6
>
vxI
;
/// \brief Left variation of the inertia matrix
container
::
aligned_vector
<
typename
Inertia
::
Matrix6
>
Ivx
;
container
::
aligned_vector
<
Matrix6
>
Ivx
;
/// \brief Inertia quantities expressed in the world frame
container
::
aligned_vector
<
Inertia
>
oYcrb
;
/// \brief Time variation of the inertia quantities expressed in the world frame
container
::
aligned_vector
<
typename
Inertia
::
Matrix6
>
doYcrb
;
container
::
aligned_vector
<
Matrix6
>
doYcrb
;
/// \brief Temporary for derivative algorithms
typename
Inertia
::
Matrix6
Itmp
;
Matrix6
Itmp
;
/// \brief Temporary for derivative algorithms
Matrix6
M6tmp
;
...
...
@@ -199,7 +199,7 @@ namespace pinocchio
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container
::
aligned_vector
<
typename
Inertia
::
Matrix6
>
Yaba
;
// TODO: change with dense symmetric matrix6
container
::
aligned_vector
<
Matrix6
>
Yaba
;
// TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
TangentVectorType
u
;
// Joint Inertia
...
...
src/multibody/data.hxx
View file @
08fb287d
...
...
@@ -23,79 +23,79 @@ namespace pinocchio
inline
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
DataTpl
(
const
Model
&
model
)
:
joints
(
0
)
,
a
((
std
::
size_t
)
model
.
njoints
)
,
oa
((
std
::
size_t
)
model
.
njoints
)
,
a_gf
((
std
::
size_t
)
model
.
njoints
)
,
oa_gf
((
std
::
size_t
)
model
.
njoints
)
,
v
((
std
::
size_t
)
model
.
njoints
)
,
ov
((
std
::
size_t
)
model
.
njoints
)
,
f
((
std
::
size_t
)
model
.
njoints
)
,
of
((
std
::
size_t
)
model
.
njoints
)
,
h
((
std
::
size_t
)
model
.
njoints
)
,
oh
((
std
::
size_t
)
model
.
njoints
)
,
oMi
((
std
::
size_t
)
model
.
njoints
)
,
liMi
((
std
::
size_t
)
model
.
njoints
)
,
tau
(
model
.
nv
)
,
nle
(
model
.
nv
)
,
g
(
model
.
nv
)
,
oMf
((
std
::
size_t
)
model
.
nframes
)
,
Ycrb
((
std
::
size_t
)
model
.
njoints
)
,
dYcrb
((
std
::
size_t
)
model
.
njoints
)
,
M
(
model
.
nv
,
model
.
nv
)
,
Minv
(
model
.
nv
,
model
.
nv
)
,
C
(
model
.
nv
,
model
.
nv
)
,
dHdq
(
6
,
model
.
nv
)
,
dFdq
(
6
,
model
.
nv
)
,
dFdv
(
6
,
model
.
nv
)
,
dFda
(
6
,
model
.
nv
)
,
SDinv
(
6
,
model
.
nv
)
,
UDinv
(
6
,
model
.
nv
)
,
IS
(
6
,
model
.
nv
)
,
vxI
((
std
::
size_t
)
model
.
njoints
)
,
Ivx
((
std
::
size_t
)
model
.
njoints
)
,
oYcrb
((
std
::
size_t
)
model
.
njoints
)
,
doYcrb
((
std
::
size_t
)
model
.
njoints
)
,
ddq
(
model
.
nv
)
,
Yaba
((
std
::
size_t
)
model
.
njoints
)
,
u
(
model
.
nv
)
,
Ag
(
6
,
model
.
nv
)
,
dAg
(
6
,
model
.
nv
)
,
Fcrb
((
std
::
size_t
)
model
.
njoints
)
,
a
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
oa
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
a_gf
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
oa_gf
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
v
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
ov
((
std
::
size_t
)
model
.
njoints
,
Motion
::
Zero
()
)
,
f
((
std
::
size_t
)
model
.
njoints
,
Force
::
Zero
()
)
,
of
((
std
::
size_t
)
model
.
njoints
,
Force
::
Zero
()
)
,
h
((
std
::
size_t
)
model
.
njoints
,
Force
::
Zero
()
)
,
oh
((
std
::
size_t
)
model
.
njoints
,
Force
::
Zero
()
)
,
oMi
((
std
::
size_t
)
model
.
njoints
,
SE3
::
Identity
()
)
,
liMi
((
std
::
size_t
)
model
.
njoints
,
SE3
::
Identity
()
)
,
tau
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
nle
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
g
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
oMf
((
std
::
size_t
)
model
.
nframes
,
SE3
::
Identity
()
)
,
Ycrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Zero
()
)
,
dYcrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Zero
()
)
,
M
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
)
)
,
Minv
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
)
)
,
C
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
)
)
,
dHdq
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dFdq
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dFdv
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dFda
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
SDinv
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
UDinv
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
IS
(
MatrixXs
::
Zero
(
6
,
model
.
nv
)
)
,
vxI
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
()
)
,
Ivx
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
()
)
,
oYcrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Zero
()
)
,
doYcrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
()
)
,
ddq
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
Yaba
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
()
)
,
u
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
Ag
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dAg
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
Fcrb
((
std
::
size_t
)
model
.
njoints
,
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
lastChild
((
std
::
size_t
)
model
.
njoints
)
,
nvSubtree
((
std
::
size_t
)
model
.
njoints
)
,
start_idx_v_fromRow
((
std
::
size_t
)
model
.
nv
)
,
end_idx_v_fromRow
((
std
::
size_t
)
model
.
nv
)
,
U
(
model
.
nv
,
model
.
nv
)
,
D
(
model
.
nv
)
,
Dinv
(
model
.
nv
)
,
tmp
(
model
.
nv
)
,
U
(
MatrixXs
::
Identity
(
model
.
nv
,
model
.
nv
)
)
,
D
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
Dinv
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
tmp
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
parents_fromRow
((
std
::
size_t
)
model
.
nv
)
,
supports_fromRow
((
std
::
size_t
)
model
.
nv
)
,
nvSubtree_fromRow
((
std
::
size_t
)
model
.
nv
)
,
J
(
6
,
model
.
nv
)
,
dJ
(
6
,
model
.
nv
)
,
dVdq
(
6
,
model
.
nv
)
,
dAdq
(
6
,
model
.
nv
)
,
dAdv
(
6
,
model
.
nv
)
,
J
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dJ
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dVdq
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dAdq
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dAdv
(
Matrix6x
::
Zero
(
6
,
model
.
nv
)
)
,
dtau_dq
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
))
,
dtau_dv
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
))
,
ddq_dq
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
))
,
ddq_dv
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
))
,
iMf
((
std
::
size_t
)
model
.
njoints
)
,
com
((
std
::
size_t
)
model
.
njoints
)
,
vcom
((
std
::
size_t
)
model
.
njoints
)
,
acom
((
std
::
size_t
)
model
.
njoints
)
,
mass
((
std
::
size_t
)
model
.
njoints
)
,
Jcom
(
3
,
model
.
nv
)
,
iMf
((
std
::
size_t
)
model
.
njoints
,
SE3
::
Identity
()
)
,
com
((
std
::
size_t
)
model
.
njoints
,
Vector3
::
Zero
()
)
,
vcom
((
std
::
size_t
)
model
.
njoints
,
Vector3
::
Zero
()
)
,
acom
((
std
::
size_t
)
model
.
njoints
,
Vector3
::
Zero
()
)
,
mass
((
std
::
size_t
)
model
.
njoints
,(
Scalar
)(
-
1
)
)
,
Jcom
(
Matrix3x
::
Zero
(
3
,
model
.
nv
)
)
,
JMinvJt
()
,
llt_JMinvJt
()
,
lambda_c
()
,
sDUiJt
(
model
.
nv
,
model
.
nv
)
,
torque_residual
(
model
.
nv
)
,
dq_after
(
model
.
nv
)
,
sDUiJt
(
MatrixXs
::
Zero
(
model
.
nv
,
model
.
nv
)
)
,
torque_residual
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
dq_after
(
VectorXs
::
Zero
(
model
.
nv
)
)
,
impulse_c
()
,
staticRegressor
(
3
,
4
*
(
model
.
njoints
-
1
))
,
jointTorqueRegressor
(
model
.
nv
,
10
*
(
model
.
njoints
-
1
))
,
staticRegressor
(
Matrix3x
::
Zero
(
3
,
4
*
(
model
.
njoints
-
1
))
)
,
jointTorqueRegressor
(
MatrixXs
::
Zero
(
model
.
nv
,
10
*
(
model
.
njoints
-
1
))
)
#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
,
kinematic_hessians
(
6
,
std
::
max
(
1
,
model
.
nv
),
std
::
max
(
1
,
model
.
nv
))
// the minimum size should be 1 for compatibility reasons
#else
...
...
@@ -116,32 +116,13 @@ namespace pinocchio
computeLastChild
(
model
);
/* Init for Coriolis */
C
.
setZero
();
/* Init for Cholesky */
U
.
setIdentity
();
computeParents_fromRow
(
model
);
computeSupports_fromRow
(
model
);
/* Init Jacobian */
J
.
setZero
();
Ag
.
setZero
();
/* Init universe states relatively to itself */
a
[
0
].
setZero
();
oa
[
0
].
setZero
();
v
[
0
].
setZero
();
ov
[
0
].
setZero
();
a_gf
[
0
]
=
-
model
.
gravity
;
f
[
0
].
setZero
();
h
[
0
].
setZero
();
oMi
[
0
].
setIdentity
();
liMi
[
0
].
setIdentity
();
oMf
[
0
].
setIdentity
();
Yaba
[
0
].
setZero
();
Ycrb
[
0
].
setZero
();
kinematic_hessians
.
setZero
();
}
...
...
@@ -225,6 +206,15 @@ namespace pinocchio
}
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
bool
operator
==
(
const
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data1
,
const
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data2
)
{
return
data1
.
joints
==
data2
.
joints
;
}
}
// namespace pinocchio
/// @endcond
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment