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
715867fe
Commit
715867fe
authored
Apr 12, 2019
by
Rohan Budhiraja
Browse files
[algorithm][dhg/dq] add computation of dhg/dq inside computecentroidaldynamicsderivatives
parent
14c5e657
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/algorithm/centroidal-derivatives.hpp
View file @
715867fe
...
...
@@ -34,6 +34,7 @@ namespace pinocchio
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration vector (dim model.nv).
/// \param[out] dh_dq The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv).
/// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).
/// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).
/// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).
...
...
@@ -43,13 +44,14 @@ namespace pinocchio
///
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
Matrix6xLike1
,
typename
Matrix6xLike2
,
typename
Matrix6xLike3
>
typename
Matrix6xLike0
,
typename
Matrix6xLike1
,
typename
Matrix6xLike2
,
typename
Matrix6xLike3
>
inline
void
computeCentroidalDynamicsDerivatives
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
a
,
const
Eigen
::
MatrixBase
<
Matrix6xLike0
>
&
dh_dq
,
const
Eigen
::
MatrixBase
<
Matrix6xLike1
>
&
dhdot_dq
,
const
Eigen
::
MatrixBase
<
Matrix6xLike2
>
&
dhdot_dv
,
const
Eigen
::
MatrixBase
<
Matrix6xLike3
>
&
dhdot_da
);
...
...
src/algorithm/centroidal-derivatives.hxx
View file @
715867fe
...
...
@@ -142,6 +142,7 @@ namespace pinocchio
ColsBlock
dVdq_cols
=
jmodel
.
jointCols
(
data
.
dVdq
);
ColsBlock
dAdq_cols
=
jmodel
.
jointCols
(
data
.
dAdq
);
ColsBlock
dAdv_cols
=
jmodel
.
jointCols
(
data
.
dAdv
);
ColsBlock
dHdq_cols
=
jmodel
.
jointCols
(
data
.
dHdq
);
ColsBlock
dFdq_cols
=
jmodel
.
jointCols
(
data
.
dFdq
);
ColsBlock
dFdv_cols
=
jmodel
.
jointCols
(
data
.
dFdv
);
ColsBlock
dFda_cols
=
jmodel
.
jointCols
(
data
.
dFda
);
...
...
@@ -171,6 +172,9 @@ namespace pinocchio
data
.
doYcrb
[
parent
]
+=
data
.
doYcrb
[
i
];
data
.
oh
[
parent
]
+=
data
.
oh
[
i
];
data
.
of
[
parent
]
+=
data
.
of
[
i
];
forceSet
::
motionActions
(
J_cols
,
data
.
oh
[
i
],
dHdq_cols
);
motionSet
::
inertiaAction
<
ADDTO
>
(
data
.
oYcrb
[
i
],
dVdq_cols
,
dHdq_cols
);
}
template
<
typename
Min
,
typename
Mout
>
...
...
@@ -211,13 +215,14 @@ namespace pinocchio
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
Matrix6xLike1
,
typename
Matrix6xLike2
,
typename
Matrix6xLike3
>
typename
Matrix6xLike0
,
typename
Matrix6xLike1
,
typename
Matrix6xLike2
,
typename
Matrix6xLike3
>
inline
void
computeCentroidalDynamicsDerivatives
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
a
,
const
Eigen
::
MatrixBase
<
Matrix6xLike0
>
&
dh_dq
,
const
Eigen
::
MatrixBase
<
Matrix6xLike1
>
&
dhdot_dq
,
const
Eigen
::
MatrixBase
<
Matrix6xLike2
>
&
dhdot_dv
,
const
Eigen
::
MatrixBase
<
Matrix6xLike3
>
&
dhdot_da
)
...
...
@@ -225,6 +230,8 @@ namespace pinocchio
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
);
...
...
@@ -278,6 +285,7 @@ namespace pinocchio
data
.
Ig
.
inertia
()
=
Ytot
.
inertia
();
// Compute the partial derivatives
translateForceSet
(
data
.
dHdq
,
com
,
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix6xLike0
,
dh_dq
));
translateForceSet
(
data
.
dFdq
,
com
,
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix6xLike1
,
dhdot_dq
));
translateForceSet
(
data
.
dFdv
,
com
,
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix6xLike2
,
dhdot_dv
));
translateForceSet
(
data
.
dFda
,
com
,
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix6xLike3
,
dhdot_da
));
...
...
src/multibody/data.hpp
View file @
715867fe
...
...
@@ -143,6 +143,9 @@ namespace pinocchio
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
MatrixXs
C
;
/// \brief Variation of the spatial momenta with respect to the joint configuration.
Matrix6x
dHdq
;
/// \brief Variation of the forceset with respect to the joint configuration.
Matrix6x
dFdq
;
...
...
src/multibody/data.hxx
View file @
715867fe
...
...
@@ -44,6 +44,7 @@ namespace pinocchio
,
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
)
...
...
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