Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/center-of-mass-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 19 | 19 | 100.0% |
Branches: | 28 | 76 | 36.8% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2019-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__ | ||
6 | #define __pinocchio_algorithm_center_of_mass_derivatives_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/algorithm/check.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | template< | ||
14 | typename Scalar, | ||
15 | int Options, | ||
16 | template<typename, int> | ||
17 | class JointCollectionTpl, | ||
18 | typename Matrix3xOut> | ||
19 | struct CoMVelocityDerivativesForwardStep | ||
20 | : public fusion::JointUnaryVisitorBase< | ||
21 | CoMVelocityDerivativesForwardStep<Scalar, Options, JointCollectionTpl, Matrix3xOut>> | ||
22 | { | ||
23 | /* Assumes that both computeForwardKinematicsDerivatives and centerOfMass (or | ||
24 | * equivalent methods like computeAllTerms) have been computed beforehand. */ | ||
25 | |||
26 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
27 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
28 | |||
29 | typedef boost::fusion::vector<const Model &, Data &, Matrix3xOut &> ArgsType; | ||
30 | |||
31 | template<typename JointModel> | ||
32 | 116 | static void algo( | |
33 | const JointModelBase<JointModel> & jmodel, | ||
34 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
35 | const Model & model, | ||
36 | Data & data, | ||
37 | const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq) | ||
38 | { | ||
39 | typedef typename Model::JointIndex JointIndex; | ||
40 | typedef typename Data::Motion Motion; | ||
41 | |||
42 | typedef Eigen::Matrix< | ||
43 | Scalar, 6, JointModel::NV, Options, 6, | ||
44 | JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV> | ||
45 | Matrix6NV; | ||
46 | |||
47 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
116 | const JointIndex i = jmodel.id(); |
48 | 116 | const JointIndex parent = model.parents[i]; | |
49 | |||
50 | 116 | Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq); | |
51 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix3x>::Type | ||
52 |
1/2✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
|
116 | dvcom_dqi = jmodel.jointCols(dvcom_dq); |
53 | |||
54 |
6/10✓ Branch 0 taken 56 times.
✓ Branch 1 taken 2 times.
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 56 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
|
116 | Motion vpc = (parent > 0) ? (data.v[i] - (Motion)jdata.v()) : Motion::Zero(); |
55 |
2/4✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 58 times.
✗ Branch 6 not taken.
|
116 | vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ] |
56 | |||
57 |
2/4✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
|
116 | Matrix6NV vxS(6, jmodel.nv()); |
58 |
2/6✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
116 | vxS = vpc.cross(jdata.S()); |
59 | |||
60 |
5/10✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 58 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 58 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 58 times.
✗ Branch 14 not taken.
|
348 | dvcom_dqi.noalias() = |
61 |
2/4✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 58 times.
✗ Branch 8 not taken.
|
116 | (data.mass[i] / data.mass[0]) * data.oMi[i].rotation() |
62 |
2/4✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 58 times.
✗ Branch 6 not taken.
|
232 | * (vxS.template middleRows<3>(Motion::LINEAR) - cross(data.com[i], vxS.template middleRows<3>(Motion::ANGULAR))); |
63 | } | ||
64 | }; | ||
65 | |||
66 | template< | ||
67 | typename Scalar, | ||
68 | int Options, | ||
69 | template<typename, int> | ||
70 | class JointCollectionTpl, | ||
71 | typename Matrix3xOut> | ||
72 | 2 | void getCenterOfMassVelocityDerivatives( | |
73 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
74 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
75 | const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq) | ||
76 | { | ||
77 | EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut, Data::Matrix3x); | ||
78 | |||
79 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE(vcom_partial_dq.cols(), model.nv); |
80 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
81 | |||
82 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
83 | typedef typename Model::JointIndex JointIndex; | ||
84 | |||
85 | 2 | Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq); | |
86 | |||
87 | typedef CoMVelocityDerivativesForwardStep<Scalar, Options, JointCollectionTpl, Matrix3xOut> | ||
88 | Pass1; | ||
89 |
2/2✓ Branch 0 taken 58 times.
✓ Branch 1 taken 2 times.
|
60 | for (JointIndex i = 1; i < (JointIndex)model.njoints; i++) |
90 | { | ||
91 |
1/2✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
|
58 | Pass1::run(model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, dvcom_dq)); |
92 | } | ||
93 | 2 | } | |
94 | |||
95 | } // namespace pinocchio | ||
96 | |||
97 | #endif // ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__ | ||
98 |