| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/regressor.hxx |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 184 | 184 | 100.0% |
| Branches: | 230 | 858 | 26.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2018-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_regressor_hxx__ | ||
| 6 | #define __pinocchio_algorithm_regressor_hxx__ | ||
| 7 | |||
| 8 | #include "pinocchio/algorithm/check.hpp" | ||
| 9 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 10 | #include "pinocchio/spatial/skew.hpp" | ||
| 11 | #include "pinocchio/spatial/symmetric3.hpp" | ||
| 12 | |||
| 13 | namespace pinocchio | ||
| 14 | { | ||
| 15 | |||
| 16 | namespace internal | ||
| 17 | { | ||
| 18 | template< | ||
| 19 | typename Scalar, | ||
| 20 | int Options, | ||
| 21 | template<typename, int> class JointCollectionTpl, | ||
| 22 | typename Matrix6xReturnType> | ||
| 23 | 576 | void computeJointKinematicRegressorGeneric( | |
| 24 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 25 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 26 | const JointIndex joint_id, | ||
| 27 | const ReferenceFrame rf, | ||
| 28 | const SE3Tpl<Scalar, Options> & global_frame_placement, | ||
| 29 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) | ||
| 30 | { | ||
| 31 |
2/4✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 576 times.
✗ Branch 4 not taken.
|
576 | assert(model.check(data) && "data is not consistent with model."); |
| 32 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 576 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.
|
576 | PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6); |
| 33 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 576 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.
|
576 | PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6 * (model.njoints - 1)); |
| 34 | |||
| 35 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 36 | typedef typename Data::SE3 SE3; | ||
| 37 | |||
| 38 | 576 | Matrix6xReturnType & kinematic_regressor_ = kinematic_regressor.const_cast_derived(); | |
| 39 |
1/2✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
|
576 | kinematic_regressor_.setZero(); |
| 40 | |||
| 41 | 576 | const SE3Tpl<Scalar, Options> & oMi = global_frame_placement; | |
| 42 |
1/2✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
|
576 | SE3 oMp; // placement of the frame following the jointPlacement transform |
| 43 |
1/2✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
|
576 | SE3 iMp; // relative placement between the joint frame and the jointPlacement |
| 44 |
2/2✓ Branch 1 taken 2973 times.
✓ Branch 2 taken 576 times.
|
3549 | for (JointIndex i = joint_id; i > 0; i = model.parents[i]) |
| 45 | { | ||
| 46 | 2973 | const JointIndex parent_id = model.parents[i]; | |
| 47 |
2/4✓ Branch 3 taken 2973 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2973 times.
✗ Branch 7 not taken.
|
2973 | oMp = data.oMi[parent_id] * model.jointPlacements[i]; |
| 48 |
3/4✓ Branch 0 taken 1009 times.
✓ Branch 1 taken 982 times.
✓ Branch 2 taken 982 times.
✗ Branch 3 not taken.
|
2973 | switch (rf) |
| 49 | { | ||
| 50 | 1009 | case LOCAL: | |
| 51 |
2/4✓ Branch 1 taken 1009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1009 times.
✗ Branch 5 not taken.
|
1009 | iMp = oMi.actInv(oMp); |
| 52 |
3/6✓ Branch 1 taken 1009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1009 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1009 times.
✗ Branch 8 not taken.
|
1009 | kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) = |
| 53 | iMp.toActionMatrix(); // TODO: we can avoid a copy | ||
| 54 | 1009 | break; | |
| 55 | 982 | case LOCAL_WORLD_ALIGNED: | |
| 56 |
3/6✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
|
982 | iMp.rotation() = oMp.rotation(); |
| 57 |
5/10✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 982 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 982 times.
✗ Branch 14 not taken.
|
982 | iMp.translation() = oMp.translation() - oMi.translation(); |
| 58 |
3/6✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
|
982 | kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) = |
| 59 | iMp.toActionMatrix(); // TODO: we can avoid a copy | ||
| 60 | 982 | break; | |
| 61 | 982 | case WORLD: | |
| 62 |
3/6✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
|
982 | kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) = |
| 63 | oMp.toActionMatrix(); // TODO: we can avoid a copy | ||
| 64 | 982 | break; | |
| 65 | } | ||
| 66 | } | ||
| 67 | 576 | } | |
| 68 | } // namespace internal | ||
| 69 | |||
| 70 | template< | ||
| 71 | typename Scalar, | ||
| 72 | int Options, | ||
| 73 | template<typename, int> class JointCollectionTpl, | ||
| 74 | typename Matrix6xReturnType> | ||
| 75 | 163 | void computeJointKinematicRegressor( | |
| 76 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 77 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 78 | const JointIndex joint_id, | ||
| 79 | const ReferenceFrame rf, | ||
| 80 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) | ||
| 81 | { | ||
| 82 |
2/6✓ Branch 0 taken 163 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 163 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
163 | PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints); |
| 83 | 163 | internal::computeJointKinematicRegressorGeneric( | |
| 84 | 163 | model, data, joint_id, rf, data.oMi[joint_id], kinematic_regressor.const_cast_derived()); | |
| 85 | 163 | } | |
| 86 | |||
| 87 | template< | ||
| 88 | typename Scalar, | ||
| 89 | int Options, | ||
| 90 | template<typename, int> class JointCollectionTpl, | ||
| 91 | typename Matrix6xReturnType> | ||
| 92 | 247 | void computeJointKinematicRegressor( | |
| 93 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 94 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 95 | const JointIndex joint_id, | ||
| 96 | const ReferenceFrame rf, | ||
| 97 | const SE3Tpl<Scalar, Options> & placement, | ||
| 98 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) | ||
| 99 | { | ||
| 100 |
2/6✓ Branch 0 taken 247 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 247 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
247 | PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints); |
| 101 | |||
| 102 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 103 | typedef typename Data::SE3 SE3; | ||
| 104 | |||
| 105 |
1/2✓ Branch 2 taken 247 times.
✗ Branch 3 not taken.
|
247 | const SE3 global_placement = data.oMi[joint_id] * placement; |
| 106 | |||
| 107 |
1/2✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
|
247 | internal::computeJointKinematicRegressorGeneric( |
| 108 | 247 | model, data, joint_id, rf, global_placement, kinematic_regressor.const_cast_derived()); | |
| 109 | 247 | } | |
| 110 | |||
| 111 | template< | ||
| 112 | typename Scalar, | ||
| 113 | int Options, | ||
| 114 | template<typename, int> class JointCollectionTpl, | ||
| 115 | typename Matrix6xReturnType> | ||
| 116 | 166 | void computeFrameKinematicRegressor( | |
| 117 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 118 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 119 | const FrameIndex frame_id, | ||
| 120 | const ReferenceFrame rf, | ||
| 121 | const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) | ||
| 122 | { | ||
| 123 |
2/6✓ Branch 0 taken 166 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 166 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
166 | PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id > 0 && (Eigen::DenseIndex)frame_id < model.nframes); |
| 124 | |||
| 125 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 126 | typedef typename Model::Frame Frame; | ||
| 127 | |||
| 128 | 166 | const Frame & frame = model.frames[frame_id]; | |
| 129 |
1/2✓ Branch 4 taken 166 times.
✗ Branch 5 not taken.
|
166 | data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement; |
| 130 | |||
| 131 | 166 | internal::computeJointKinematicRegressorGeneric( | |
| 132 | 166 | model, data, frame.parentJoint, rf, data.oMf[frame_id], | |
| 133 | 166 | kinematic_regressor.const_cast_derived()); | |
| 134 | 166 | } | |
| 135 | |||
| 136 | template< | ||
| 137 | typename Scalar, | ||
| 138 | int Options, | ||
| 139 | template<typename, int> class JointCollectionTpl, | ||
| 140 | typename ConfigVectorType> | ||
| 141 | 2 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & computeStaticRegressor( | |
| 142 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 143 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 144 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 145 | { | ||
| 146 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
| 147 |
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(q.size(), model.nq); |
| 148 | |||
| 149 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 150 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 151 | typedef typename Model::JointIndex JointIndex; | ||
| 152 | typedef typename Data::SE3 SE3; | ||
| 153 | |||
| 154 | typedef typename Data::Matrix3x Matrix3x; | ||
| 155 | typedef typename SizeDepType<4>::ColsReturn<Matrix3x>::Type ColsBlock; | ||
| 156 | |||
| 157 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | forwardKinematics(model, data, q.derived()); |
| 158 | |||
| 159 | // Computes the total mass of the system | ||
| 160 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | Scalar mass = Scalar(0); |
| 161 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 162 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
54 | mass += model.inertias[(JointIndex)i].mass(); |
| 163 | |||
| 164 |
0/4✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2 | const Scalar mass_inv = Scalar(1) / mass; |
| 165 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 166 | { | ||
| 167 | 54 | const SE3 & oMi = data.oMi[i]; | |
| 168 | ColsBlock sr_cols = | ||
| 169 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | data.staticRegressor.template middleCols<4>((Eigen::DenseIndex)(i - 1) * 4); |
| 170 |
3/6✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
|
54 | sr_cols.col(0) = oMi.translation(); |
| 171 |
3/6✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
|
54 | sr_cols.template rightCols<3>() = oMi.rotation(); |
| 172 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | sr_cols *= mass_inv; |
| 173 | } | ||
| 174 | |||
| 175 | 2 | return data.staticRegressor; | |
| 176 | } | ||
| 177 | |||
| 178 | namespace details | ||
| 179 | { | ||
| 180 | // auxiliary function for bodyRegressor: bigL(omega)*I.toDynamicParameters().tail<6>() = | ||
| 181 | // I.inertia() * omega | ||
| 182 | /* | ||
| 183 | template<typename Vector3Like> | ||
| 184 | inline Eigen::Matrix<typename | ||
| 185 | Vector3Like::Scalar,3,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> bigL(const | ||
| 186 | Eigen::MatrixBase<Vector3Like> & omega) | ||
| 187 | { | ||
| 188 | typedef typename Vector3Like::Scalar Scalar; | ||
| 189 | enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options }; | ||
| 190 | typedef Eigen::Matrix<Scalar,3,6,Options> ReturnType; | ||
| 191 | |||
| 192 | ReturnType L; | ||
| 193 | L << omega[0], omega[1], Scalar(0), omega[2], Scalar(0), Scalar(0), | ||
| 194 | Scalar(0), omega[0], omega[1], Scalar(0), omega[2], Scalar(0), | ||
| 195 | Scalar(0), Scalar(0), Scalar(0), omega[0], omega[1], omega[2]; | ||
| 196 | return L; | ||
| 197 | } | ||
| 198 | */ | ||
| 199 | |||
| 200 | // auxiliary function for bodyRegressor: res += bigL(omega) | ||
| 201 | template<typename Vector3Like, typename OutputType> | ||
| 202 | inline void | ||
| 203 | 60 | addBigL(const Eigen::MatrixBase<Vector3Like> & omega, const Eigen::MatrixBase<OutputType> & out) | |
| 204 | { | ||
| 205 | 60 | OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out); | |
| 206 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(0, 0) += omega[0]; |
| 207 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(0, 1) += omega[1]; |
| 208 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(0, 3) += omega[2]; |
| 209 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(1, 1) += omega[0]; |
| 210 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(1, 2) += omega[1]; |
| 211 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(1, 4) += omega[2]; |
| 212 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(2, 3) += omega[0]; |
| 213 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(2, 4) += omega[1]; |
| 214 |
0/4✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
60 | res(2, 5) += omega[2]; |
| 215 | 60 | } | |
| 216 | |||
| 217 | // auxiliary function for bodyRegressor: res = cross(omega,bigL(omega)) | ||
| 218 | template<typename Vector3Like, typename OutputType> | ||
| 219 | inline void | ||
| 220 | 60 | crossBigL(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<OutputType> & out) | |
| 221 | { | ||
| 222 | typedef typename Vector3Like::Scalar Scalar; | ||
| 223 | 60 | OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out); | |
| 224 | |||
| 225 |
15/69✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 59 not taken.
✗ Branch 60 not taken.
✗ Branch 62 not taken.
✗ Branch 63 not taken.
✗ Branch 65 not taken.
✗ Branch 66 not taken.
✗ Branch 68 not taken.
✗ Branch 69 not taken.
✗ Branch 71 not taken.
✗ Branch 72 not taken.
✗ Branch 74 not taken.
✗ Branch 75 not taken.
✗ Branch 77 not taken.
✗ Branch 78 not taken.
✗ Branch 80 not taken.
✗ Branch 81 not taken.
|
60 | res << Scalar(0), -v[2] * v[0], -v[2] * v[1], v[1] * v[0], v[1] * v[1] - v[2] * v[2], |
| 226 |
18/56✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 60 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 60 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 60 times.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 67 not taken.
✗ Branch 68 not taken.
✗ Branch 70 not taken.
✗ Branch 71 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 76 not taken.
✗ Branch 77 not taken.
✗ Branch 79 not taken.
✗ Branch 80 not taken.
✗ Branch 82 not taken.
✗ Branch 83 not taken.
|
60 | v[2] * v[1], v[2] * v[0], v[2] * v[1], Scalar(0), v[2] * v[2] - v[0] * v[0], -v[1] * v[0], |
| 227 |
17/52✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 60 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 60 times.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 67 not taken.
✗ Branch 68 not taken.
✗ Branch 70 not taken.
✗ Branch 71 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 76 not taken.
✗ Branch 77 not taken.
|
60 | -v[2] * v[0], -v[1] * v[0], v[0] * v[0] - v[1] * v[1], v[1] * v[0], -v[2] * v[1], |
| 228 |
4/8✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
|
60 | v[2] * v[0], Scalar(0); |
| 229 | 60 | } | |
| 230 | } // namespace details | ||
| 231 | |||
| 232 | template<typename MotionVelocity, typename MotionAcceleration, typename OutputType> | ||
| 233 | 60 | inline void bodyRegressor( | |
| 234 | const MotionDense<MotionVelocity> & v, | ||
| 235 | const MotionDense<MotionAcceleration> & a, | ||
| 236 | const Eigen::MatrixBase<OutputType> & regressor) | ||
| 237 | { | ||
| 238 |
2/4✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
|
60 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, regressor, 6, 10); |
| 239 | |||
| 240 | typedef typename MotionVelocity::Scalar Scalar; | ||
| 241 | enum | ||
| 242 | { | ||
| 243 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options | ||
| 244 | }; | ||
| 245 | |||
| 246 | typedef Symmetric3Tpl<Scalar, Options> Symmetric3; | ||
| 247 | typedef typename Symmetric3::SkewSquare SkewSquare; | ||
| 248 | using ::pinocchio::details::addBigL; | ||
| 249 | using ::pinocchio::details::crossBigL; | ||
| 250 | |||
| 251 | 60 | OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, regressor); | |
| 252 | |||
| 253 |
4/8✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
|
120 | res.template block<3, 1>(MotionVelocity::LINEAR, 0) = |
| 254 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
120 | a.linear() + v.angular().cross(v.linear()); |
| 255 | 60 | const Eigen::Block<OutputType, 3, 1> & acc = | |
| 256 |
1/2✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
|
60 | res.template block<3, 1>(MotionVelocity::LINEAR, 0); |
| 257 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
60 | res.template block<3, 3>(MotionVelocity::LINEAR, 1) = |
| 258 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 60 times.
✗ Branch 9 not taken.
|
60 | Symmetric3(SkewSquare(v.angular())).matrix(); |
| 259 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
60 | addSkew(a.angular(), res.template block<3, 3>(MotionVelocity::LINEAR, 1)); |
| 260 | |||
| 261 |
2/4✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
|
60 | res.template block<3, 6>(MotionVelocity::LINEAR, 4).setZero(); |
| 262 | |||
| 263 |
2/4✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
|
60 | res.template block<3, 1>(MotionVelocity::ANGULAR, 0).setZero(); |
| 264 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
60 | skew(-acc, res.template block<3, 3>(MotionVelocity::ANGULAR, 1)); |
| 265 | // res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(), | ||
| 266 | // bigL(v.angular())); | ||
| 267 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
60 | crossBigL(v.angular(), res.template block<3, 6>(MotionVelocity::ANGULAR, 4)); |
| 268 |
3/6✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
|
60 | addBigL(a.angular(), res.template block<3, 6>(MotionVelocity::ANGULAR, 4)); |
| 269 | 60 | } | |
| 270 | |||
| 271 | template<typename MotionVelocity, typename MotionAcceleration> | ||
| 272 | inline Eigen::Matrix< | ||
| 273 | typename MotionVelocity::Scalar, | ||
| 274 | 6, | ||
| 275 | 10, | ||
| 276 | PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options> | ||
| 277 | 2 | bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a) | |
| 278 | { | ||
| 279 | typedef typename MotionVelocity::Scalar Scalar; | ||
| 280 | enum | ||
| 281 | { | ||
| 282 | Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options | ||
| 283 | }; | ||
| 284 | typedef Eigen::Matrix<Scalar, 6, 10, Options> ReturnType; | ||
| 285 | |||
| 286 | 2 | ReturnType res; | |
| 287 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | bodyRegressor(v, a, res); |
| 288 | 2 | return res; | |
| 289 | } | ||
| 290 | |||
| 291 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 292 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType & | ||
| 293 | 56 | jointBodyRegressor( | |
| 294 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 295 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 296 | JointIndex joint_id) | ||
| 297 | { | ||
| 298 |
1/2✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
|
56 | assert(model.check(data) && "data is not consistent with model."); |
| 299 | |||
| 300 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
| 301 | |||
| 302 | 56 | bodyRegressor(data.v[joint_id], data.a_gf[joint_id], data.bodyRegressor); | |
| 303 | 56 | return data.bodyRegressor; | |
| 304 | } | ||
| 305 | |||
| 306 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 307 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType & | ||
| 308 | 2 | frameBodyRegressor( | |
| 309 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 310 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 311 | FrameIndex frame_id) | ||
| 312 | { | ||
| 313 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
| 314 | |||
| 315 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 316 | typedef typename Model::Frame Frame; | ||
| 317 | typedef typename Model::JointIndex JointIndex; | ||
| 318 | typedef typename Model::SE3 SE3; | ||
| 319 | |||
| 320 | 2 | const Frame & frame = model.frames[frame_id]; | |
| 321 | 2 | const JointIndex & parent = frame.parentJoint; | |
| 322 | 2 | const SE3 & placement = frame.placement; | |
| 323 | |||
| 324 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | bodyRegressor( |
| 325 | 4 | placement.actInv(data.v[parent]), placement.actInv(data.a_gf[parent]), data.bodyRegressor); | |
| 326 | 2 | return data.bodyRegressor; | |
| 327 | } | ||
| 328 | |||
| 329 | template< | ||
| 330 | typename Scalar, | ||
| 331 | int Options, | ||
| 332 | template<typename, int> class JointCollectionTpl, | ||
| 333 | typename ConfigVectorType, | ||
| 334 | typename TangentVectorType1, | ||
| 335 | typename TangentVectorType2> | ||
| 336 | struct JointTorqueRegressorForwardStep | ||
| 337 | : public fusion::JointUnaryVisitorBase<JointTorqueRegressorForwardStep< | ||
| 338 | Scalar, | ||
| 339 | Options, | ||
| 340 | JointCollectionTpl, | ||
| 341 | ConfigVectorType, | ||
| 342 | TangentVectorType1, | ||
| 343 | TangentVectorType2>> | ||
| 344 | { | ||
| 345 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 346 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 347 | |||
| 348 | typedef boost::fusion::vector< | ||
| 349 | const Model &, | ||
| 350 | Data &, | ||
| 351 | const ConfigVectorType &, | ||
| 352 | const TangentVectorType1 &, | ||
| 353 | const TangentVectorType2 &> | ||
| 354 | ArgsType; | ||
| 355 | |||
| 356 | template<typename JointModel> | ||
| 357 | 108 | static void algo( | |
| 358 | const JointModelBase<JointModel> & jmodel, | ||
| 359 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 360 | const Model & model, | ||
| 361 | Data & data, | ||
| 362 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 363 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 364 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
| 365 | { | ||
| 366 | typedef typename Model::JointIndex JointIndex; | ||
| 367 | |||
| 368 | 108 | const JointIndex i = jmodel.id(); | |
| 369 | 108 | const JointIndex parent = model.parents[i]; | |
| 370 | |||
| 371 | 108 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); | |
| 372 | |||
| 373 |
3/5✓ Branch 4 taken 52 times.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 52 times.
✗ Branch 9 not taken.
|
108 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 374 | |||
| 375 | 108 | data.v[i] = jdata.v(); | |
| 376 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
|
108 | if (parent > 0) |
| 377 |
1/2✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
|
104 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
| 378 | |||
| 379 |
2/8✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
108 | data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
| 380 |
3/6✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
|
108 | data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); |
| 381 |
1/2✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
|
108 | data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
| 382 | } | ||
| 383 | }; | ||
| 384 | |||
| 385 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 386 | struct JointTorqueRegressorBackwardStep | ||
| 387 | : public fusion::JointUnaryVisitorBase< | ||
| 388 | JointTorqueRegressorBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
| 389 | { | ||
| 390 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 391 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 392 | typedef | ||
| 393 | typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType BodyRegressorType; | ||
| 394 | |||
| 395 | typedef boost::fusion::vector<const Model &, Data &, const JointIndex &> ArgsType; | ||
| 396 | |||
| 397 | template<typename JointModel> | ||
| 398 | 552 | static void algo( | |
| 399 | const JointModelBase<JointModel> & jmodel, | ||
| 400 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 401 | const Model & model, | ||
| 402 | Data & data, | ||
| 403 | const JointIndex & col_idx) | ||
| 404 | { | ||
| 405 | typedef typename Model::JointIndex JointIndex; | ||
| 406 | |||
| 407 | 552 | const JointIndex i = jmodel.id(); | |
| 408 | 552 | const JointIndex parent = model.parents[i]; | |
| 409 | |||
| 410 |
1/2✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
|
552 | data.jointTorqueRegressor.block( |
| 411 |
3/8✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 276 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
552 | jmodel.idx_v(), 10 * (Eigen::DenseIndex(col_idx) - 1), jmodel.nv(), 10) = |
| 412 |
1/2✓ Branch 3 taken 222 times.
✗ Branch 4 not taken.
|
552 | jdata.S().transpose() * data.bodyRegressor; |
| 413 | |||
| 414 |
2/2✓ Branch 0 taken 222 times.
✓ Branch 1 taken 54 times.
|
552 | if (parent > 0) |
| 415 | 444 | forceSet::se3Action(data.liMi[i], data.bodyRegressor, data.bodyRegressor); | |
| 416 | } | ||
| 417 | }; | ||
| 418 | |||
| 419 | template< | ||
| 420 | typename Scalar, | ||
| 421 | int Options, | ||
| 422 | template<typename, int> class JointCollectionTpl, | ||
| 423 | typename ConfigVectorType, | ||
| 424 | typename TangentVectorType1, | ||
| 425 | typename TangentVectorType2> | ||
| 426 | inline typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & | ||
| 427 | 2 | computeJointTorqueRegressor( | |
| 428 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 429 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 430 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 431 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 432 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
| 433 | { | ||
| 434 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
| 435 |
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(q.size(), model.nq); |
| 436 |
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(v.size(), model.nv); |
| 437 |
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(a.size(), model.nv); |
| 438 | |||
| 439 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | data.v[0].setZero(); |
| 440 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | data.a_gf[0] = -model.gravity; |
| 441 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | data.jointTorqueRegressor.setZero(); |
| 442 | |||
| 443 | typedef JointTorqueRegressorForwardStep< | ||
| 444 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, TangentVectorType2> | ||
| 445 | Pass1; | ||
| 446 |
1/2✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), a.derived()); |
| 447 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 448 | { | ||
| 449 |
2/4✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
|
54 | Pass1::run(model.joints[i], data.joints[i], arg1); |
| 450 | } | ||
| 451 | |||
| 452 | typedef JointTorqueRegressorBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
| 453 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
| 454 | { | ||
| 455 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | jointBodyRegressor(model, data, i); |
| 456 | |||
| 457 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | typename Pass2::ArgsType arg2(model, data, i); |
| 458 |
2/2✓ Branch 0 taken 276 times.
✓ Branch 1 taken 54 times.
|
330 | for (JointIndex j = i; j > 0; j = model.parents[j]) |
| 459 | { | ||
| 460 |
2/4✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 276 times.
✗ Branch 7 not taken.
|
276 | Pass2::run(model.joints[j], data.joints[j], arg2); |
| 461 | } | ||
| 462 | } | ||
| 463 | |||
| 464 | 2 | return data.jointTorqueRegressor; | |
| 465 | } | ||
| 466 | |||
| 467 | template< | ||
| 468 | typename Scalar, | ||
| 469 | int Options, | ||
| 470 | template<typename, int> class JointCollectionTpl, | ||
| 471 | typename ConfigVectorType, | ||
| 472 | typename TangentVectorType> | ||
| 473 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs & | ||
| 474 | 1 | computeKineticEnergyRegressor( | |
| 475 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 476 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 477 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 478 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
| 479 | { | ||
| 480 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
| 481 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
| 482 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
| 483 | |||
| 484 | 1 | forwardKinematics(model, data, q.derived(), v.derived()); | |
| 485 | |||
| 486 | 1 | data.kineticEnergyRegressor.setZero(); | |
| 487 | // iterate over each joint and compute the kinetic energy regressor | ||
| 488 |
2/4✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
28 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
| 489 | { | ||
| 490 | // linear and angular velocities | ||
| 491 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const auto linear_vel = data.v[joint_id].linear(); |
| 492 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const auto angular_vel = data.v[joint_id].angular(); |
| 493 | |||
| 494 |
3/12✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
27 | const Scalar v_x = linear_vel[0], v_y = linear_vel[1], v_z = linear_vel[2], |
| 495 |
3/12✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
27 | w_x = angular_vel[0], w_y = angular_vel[1], w_z = angular_vel[2]; |
| 496 | |||
| 497 | auto joint_regressor = | ||
| 498 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | data.kineticEnergyRegressor.template segment<10>(10 * Eigen::DenseIndex(joint_id - 1)); |
| 499 | |||
| 500 |
2/10✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ 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.
|
27 | joint_regressor[0] = 0.5 * linear_vel.dot(linear_vel); |
| 501 |
1/12✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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 16 not taken.
✗ Branch 17 not taken.
|
27 | joint_regressor[1] = -w_y * v_z + w_z * v_y; |
| 502 |
1/10✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
|
27 | joint_regressor[2] = w_x * v_z - w_z * v_x; |
| 503 |
1/12✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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 16 not taken.
✗ Branch 17 not taken.
|
27 | joint_regressor[3] = -w_x * v_y + w_y * v_x; |
| 504 |
1/10✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
|
27 | joint_regressor[4] = 0.5 * w_x * w_x; |
| 505 |
1/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
27 | joint_regressor[5] = w_x * w_y; |
| 506 |
1/10✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
|
27 | joint_regressor[6] = 0.5 * w_y * w_y; |
| 507 |
1/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
27 | joint_regressor[7] = w_x * w_z; |
| 508 |
1/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
27 | joint_regressor[8] = w_y * w_z; |
| 509 |
1/10✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
|
27 | joint_regressor[9] = 0.5 * w_z * w_z; |
| 510 | } | ||
| 511 | |||
| 512 | 1 | return data.kineticEnergyRegressor; | |
| 513 | } | ||
| 514 | |||
| 515 | template< | ||
| 516 | typename Scalar, | ||
| 517 | int Options, | ||
| 518 | template<typename, int> class JointCollectionTpl, | ||
| 519 | typename ConfigVectorType> | ||
| 520 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs & | ||
| 521 | 1 | computePotentialEnergyRegressor( | |
| 522 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 523 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 524 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 525 | { | ||
| 526 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
| 527 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
| 528 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 529 | |||
| 530 | 1 | forwardKinematics(model, data, q.derived()); | |
| 531 | |||
| 532 | 1 | data.potentialEnergyRegressor.setZero(); | |
| 533 | |||
| 534 | // iterate over each joint and compute the kinetic energy regressor | ||
| 535 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
| 536 | { | ||
| 537 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const auto & t = data.oMi[joint_id].translation(); |
| 538 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const auto & R = data.oMi[joint_id].rotation(); |
| 539 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | const auto g = -model.gravity.linear(); |
| 540 | |||
| 541 | auto joint_regressor = | ||
| 542 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | data.potentialEnergyRegressor.template segment<10>(10 * Eigen::DenseIndex(joint_id - 1)); |
| 543 | |||
| 544 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
|
27 | const typename Data::Vector3 gravity_local = R.transpose() * g; |
| 545 |
2/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
27 | joint_regressor[0] = g.dot(t); |
| 546 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | joint_regressor.template segment<3>(1) = gravity_local; |
| 547 | } | ||
| 548 | |||
| 549 | 1 | return data.potentialEnergyRegressor; | |
| 550 | } | ||
| 551 | } // namespace pinocchio | ||
| 552 | |||
| 553 | #endif // ifndef __pinocchio_algorithm_regressor_hxx__ | ||
| 554 |