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
5a1cbda5
Verified
Commit
5a1cbda5
authored
Apr 18, 2019
by
Justin Carpentier
Browse files
algo/center-of-mass-derivatives: improve efficiency
Number of operations are reduced and it also avoid memory allocations
parent
98335a94
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/algorithm/center-of-mass-derivatives.hxx
View file @
5a1cbda5
//
// Copyright (c) 2019 CNRS
// Copyright (c) 2019 CNRS
INRIA
//
#ifndef __pinocchio_center_of_mass_derivatives_hxx__
...
...
@@ -37,17 +37,20 @@ namespace pinocchio
typedef
typename
Data
::
Motion
Motion
;
const
JointIndex
&
i
=
jmodel
.
id
();
const
JointIndex
&
parent
=
model
.
parents
[
i
];
Matrix3xOut
&
dvcom_dq
=
PINOCCHIO_EIGEN_CONST_CAST
(
Matrix3xOut
,
vcom_partial_dq
);
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
typename
Data
::
Matrix3x
>
::
Type
dvcom_dqi
=
jmodel
.
jointCols
(
dvcom_dq
);
Motion
&
vpc
=
data
.
v
[
0
]
=
(
model
.
parent
s
[
i
]
>
0
)
?
(
data
.
v
[
i
]
-
(
Motion
)
jdata
.
v
())
:
Motion
::
Zero
();
Motion
&
vpc
=
data
.
v
[
0
]
=
(
parent
>
0
)
?
(
data
.
v
[
i
]
-
(
Motion
)
jdata
.
v
())
:
Motion
::
Zero
();
vpc
.
linear
()
-=
data
.
vcom
[
i
];
// vpc = v_{parent+c} = [ v_parent+vc; w_parent ]
const
Eigen
::
Matrix
<
Scalar
,
6
,
JointModel
::
NV
,
Options
>
&
vxS
=
vpc
.
cross
(
jdata
.
S
());
dvcom_dqi
=
data
.
mass
[
i
]
*
data
.
oMi
[
i
].
rotation
()
//*( vxS.template topRows<3>() - cross( data.com[i], vxS.template bottomRows<3>()) );
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
typename
Data
::
Matrix6
>
::
Type
vxS
=
data
.
M6tmp
.
template
leftCols
<
JointModel
::
NV
>(
jmodel
.
nv
());
vxS
=
vpc
.
cross
(
jdata
.
S
());
dvcom_dqi
.
noalias
()
=
(
data
.
mass
[
i
]
/
data
.
mass
[
0
])
*
data
.
oMi
[
i
].
rotation
()
*
(
vxS
.
template
middleRows
<
3
>(
Motion
::
LINEAR
)
-
cross
(
data
.
com
[
i
],
vxS
.
template
middleRows
<
3
>(
Motion
::
ANGULAR
))
);
}
};
...
...
@@ -70,11 +73,11 @@ namespace pinocchio
typedef
CoMVelocityDerivativesForwardStep
<
Scalar
,
Options
,
JointCollectionTpl
,
Matrix3xOut
>
Pass1
;
for
(
JointIndex
i
=
1
;
i
<
(
JointIndex
)
model
.
njoints
;
i
++
)
{
Pass1
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
typename
Pass1
::
ArgsType
(
model
,
data
,
dvcom_dq
));
}
dvcom_dq
/=
data
.
mass
[
0
];
{
Pass1
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
typename
Pass1
::
ArgsType
(
model
,
data
,
dvcom_dq
));
}
data
.
v
[
0
].
setZero
();
}
...
...
src/multibody/data.hpp
View file @
5a1cbda5
...
...
@@ -67,6 +67,7 @@ namespace pinocchio
/// \brief The 3d jacobian type (temporary)
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
Eigen
::
Dynamic
,
Options
>
Matrix3x
;
typedef
Eigen
::
Matrix
<
Scalar
,
6
,
6
,
Options
>
Matrix6
;
typedef
Eigen
::
Matrix
<
Scalar
,
6
,
6
,
Eigen
::
RowMajor
|
Options
>
RowMatrix6
;
typedef
Eigen
::
Matrix
<
Scalar
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
RowMajor
|
Options
>
RowMatrixXs
;
...
...
@@ -181,6 +182,7 @@ namespace pinocchio
typename
Inertia
::
Matrix6
Itmp
;
/// \brief Temporary for derivative algorithms
Matrix6
M6tmp
;
RowMatrix6
M6tmpR
;
RowMatrix6
M6tmpR2
;
...
...
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