| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/rnea-second-order-derivatives.hxx |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 216 | 216 | 100.0% |
| Branches: | 382 | 1092 | 35.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017-2020 CNRS INRIA | ||
| 3 | |||
| 4 | #ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ | ||
| 5 | #define __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ | ||
| 6 | |||
| 7 | #include "pinocchio/algorithm/check.hpp" | ||
| 8 | #include "pinocchio/multibody/visitor.hpp" | ||
| 9 | |||
| 10 | namespace pinocchio | ||
| 11 | { | ||
| 12 | |||
| 13 | template< | ||
| 14 | typename Scalar, | ||
| 15 | int Options, | ||
| 16 | template<typename, int> class JointCollectionTpl, | ||
| 17 | typename ConfigVectorType, | ||
| 18 | typename TangentVectorType1, | ||
| 19 | typename TangentVectorType2> | ||
| 20 | struct ComputeRNEASecondOrderDerivativesForwardStep | ||
| 21 | : public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesForwardStep< | ||
| 22 | Scalar, | ||
| 23 | Options, | ||
| 24 | JointCollectionTpl, | ||
| 25 | ConfigVectorType, | ||
| 26 | TangentVectorType1, | ||
| 27 | TangentVectorType2>> | ||
| 28 | { | ||
| 29 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 30 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 31 | |||
| 32 | typedef boost::fusion::vector< | ||
| 33 | const Model &, | ||
| 34 | Data &, | ||
| 35 | const ConfigVectorType &, | ||
| 36 | const TangentVectorType1 &, | ||
| 37 | const TangentVectorType2 &> | ||
| 38 | ArgsType; | ||
| 39 | |||
| 40 | template<typename JointModel> | ||
| 41 | 216 | static void algo( | |
| 42 | const JointModelBase<JointModel> & jmodel, | ||
| 43 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 44 | const Model & model, | ||
| 45 | Data & data, | ||
| 46 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 47 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 48 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
| 49 | { | ||
| 50 | typedef typename Model::JointIndex JointIndex; | ||
| 51 | typedef typename Data::Motion Motion; | ||
| 52 | typedef typename Data::Inertia Inertia; | ||
| 53 | |||
| 54 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex i = jmodel.id(); |
| 55 | 216 | const JointIndex parent = model.parents[i]; | |
| 56 | 216 | Motion & ov = data.ov[i]; | |
| 57 | 216 | Motion & oa = data.oa[i]; | |
| 58 | 216 | Motion & vJ = data.v[i]; | |
| 59 | |||
| 60 |
1/2✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
| 61 | |||
| 62 |
6/10✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 104 times.
✗ Branch 13 not taken.
|
216 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 63 |
2/2✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
|
216 | if (parent > 0) |
| 64 | { | ||
| 65 |
2/4✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
|
208 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
| 66 |
1/2✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
|
208 | ov = data.ov[parent]; |
| 67 |
1/2✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
|
208 | oa = data.oa[parent]; |
| 68 | } | ||
| 69 | else | ||
| 70 | { | ||
| 71 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
8 | data.oMi[i] = data.liMi[i]; |
| 72 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | ov.setZero(); |
| 73 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | oa = -model.gravity; |
| 74 | } | ||
| 75 | |||
| 76 | typedef | ||
| 77 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 78 | ColsBlock; | ||
| 79 | ColsBlock J_cols = | ||
| 80 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | jmodel.jointCols(data.J); // data.J has all the phi (in world frame) stacked in columns |
| 81 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock psid_cols = jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame |
| 82 | ColsBlock psidd_cols = | ||
| 83 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame |
| 84 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); // This here is phi_dot in world frame |
| 85 | |||
| 86 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | J_cols.noalias() = |
| 87 |
2/4✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
|
216 | data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint |
| 88 |
3/6✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
|
216 | vJ = data.oMi[i].act(jdata.v()); |
| 89 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | motionSet::motionAction(ov, J_cols, psid_cols); // This ov here is v(p(i)), psi_dot calcs |
| 90 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | motionSet::motionAction(oa, J_cols, psidd_cols); // This oa here is a(p(i)) , psi_dotdot calcs |
| 91 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | motionSet::motionAction<ADDTO>( |
| 92 | ov, psid_cols, | ||
| 93 | psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs | ||
| 94 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ov += vJ; |
| 95 |
9/18✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 108 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 108 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 108 times.
✗ Branch 27 not taken.
|
216 | oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c()); |
| 96 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) |
| 97 | // + vJ Composite rigid body inertia | ||
| 98 | 216 | Inertia & oY = data.oYcrb[i]; | |
| 99 | |||
| 100 |
2/4✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
|
216 | oY = data.oMi[i].act(model.inertias[i]); |
| 101 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
|
216 | data.oh[i] = oY * ov; |
| 102 | |||
| 103 |
4/8✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
|
216 | data.of[i] = oY * oa + oY.vxiv(ov); // f_i in world frame |
| 104 | |||
| 105 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | data.doYcrb[i] = oY.variation(ov); |
| 106 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
216 | addForceCrossMatrix(data.oh[i], data.doYcrb[i]); // BC{i} |
| 107 | } | ||
| 108 | template<typename ForceDerived, typename M6> | ||
| 109 | static void | ||
| 110 | 216 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
| 111 | { | ||
| 112 | 216 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
| 113 |
3/6✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
|
216 | addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
| 114 |
3/6✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
|
216 | addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
| 115 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
216 | addSkew( |
| 116 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
| 117 | 216 | } | |
| 118 | }; | ||
| 119 | |||
| 120 | template< | ||
| 121 | typename Scalar, | ||
| 122 | int Options, | ||
| 123 | template<typename, int> class JointCollectionTpl, | ||
| 124 | typename Tensor1, | ||
| 125 | typename Tensor2, | ||
| 126 | typename Tensor3, | ||
| 127 | typename Tensor4> | ||
| 128 | struct ComputeRNEASecondOrderDerivativesBackwardStep | ||
| 129 | : public fusion::JointUnaryVisitorBase<ComputeRNEASecondOrderDerivativesBackwardStep< | ||
| 130 | Scalar, | ||
| 131 | Options, | ||
| 132 | JointCollectionTpl, | ||
| 133 | Tensor1, | ||
| 134 | Tensor2, | ||
| 135 | Tensor3, | ||
| 136 | Tensor4>> | ||
| 137 | { | ||
| 138 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 139 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 140 | |||
| 141 | typedef boost::fusion::vector< | ||
| 142 | const Model &, | ||
| 143 | Data &, | ||
| 144 | const Tensor1 &, | ||
| 145 | const Tensor2 &, | ||
| 146 | const Tensor3 &, | ||
| 147 | const Tensor4 &> | ||
| 148 | ArgsType; | ||
| 149 | |||
| 150 | template<typename JointModel> | ||
| 151 | 216 | static void algo( | |
| 152 | const JointModelBase<JointModel> & jmodel, | ||
| 153 | const Model & model, | ||
| 154 | Data & data, | ||
| 155 | const Tensor1 & d2tau_dqdq, | ||
| 156 | const Tensor2 & d2tau_dvdv, | ||
| 157 | const Tensor3 & dtau_dqdv, | ||
| 158 | const Tensor3 & dtau_dadq) | ||
| 159 | { | ||
| 160 | typedef typename Data::Motion Motion; | ||
| 161 | typedef typename Data::Force Force; | ||
| 162 | typedef typename Data::Inertia Inertia; | ||
| 163 | typedef typename Model::JointIndex JointIndex; | ||
| 164 | typedef typename Motion::ActionMatrixType ActionMatrixType; | ||
| 165 | typedef typename Data::Matrix6 Matrix6; | ||
| 166 | typedef typename Data::Vector6r Vector6r; | ||
| 167 | typedef typename Data::Vector6c Vector6c; | ||
| 168 | |||
| 169 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex i = jmodel.id(); |
| 170 | 216 | const JointIndex parent = model.parents[i]; | |
| 171 | |||
| 172 | 216 | const Inertia & oYcrb = data.oYcrb[i]; // IC{i} | |
| 173 | 216 | const Matrix6 & oBcrb = data.doYcrb[i]; // BC{i} | |
| 174 | |||
| 175 | 216 | Tensor1 & d2tau_dqdq_ = const_cast<Tensor1 &>(d2tau_dqdq); | |
| 176 | 216 | Tensor2 & d2tau_dvdv_ = const_cast<Tensor2 &>(d2tau_dvdv); | |
| 177 | 216 | Tensor3 & dtau_dqdv_ = const_cast<Tensor3 &>(dtau_dqdv); | |
| 178 | 216 | Tensor4 & dtau_dadq_ = const_cast<Tensor4 &>(dtau_dadq); | |
| 179 | |||
| 180 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6r u1; |
| 181 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6r u2; |
| 182 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u3; |
| 183 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u4; |
| 184 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u5; |
| 185 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u6; |
| 186 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u7; |
| 187 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u8; |
| 188 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u9; |
| 189 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u10; |
| 190 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6r u11; |
| 191 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6r u12; |
| 192 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Vector6c u13; |
| 193 | |||
| 194 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Matrix6 Bicphii; |
| 195 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Matrix6 oBicpsidot; |
| 196 | |||
| 197 | Scalar p1, p2, p3, p4, p5, p6; | ||
| 198 | |||
| 199 |
8/16✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 108 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 108 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 108 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 108 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 108 times.
✗ Branch 23 not taken.
|
216 | Matrix6 r0, r1, r2, r3, r4, r5, r6, r7; |
| 200 | |||
| 201 |
2/2✓ Branch 1 taken 128 times.
✓ Branch 2 taken 108 times.
|
472 | for (int p = 0; p < model.nvs[i]; p++) |
| 202 | { | ||
| 203 | 256 | const Eigen::DenseIndex ip = model.idx_vs[i] + p; | |
| 204 | |||
| 205 |
2/4✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
|
256 | const MotionRef<typename Data::Matrix6x::ColXpr> S_i = data.J.col(ip); // S{i}(:,p) |
| 206 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | const ActionMatrixType S_iA = S_i.toActionMatrix(); //(S{i}(:,p) )x matrix |
| 207 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = |
| 208 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | data.psid.col(ip); // psi_dot for p DOF |
| 209 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = |
| 210 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | data.psidd.col(ip); // psi_ddot for p DOF |
| 211 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = |
| 212 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | data.dJ.col(ip); // phi_dot for p DOF |
| 213 | |||
| 214 |
2/4✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
|
256 | r1 = Bicphii = oYcrb.variation(S_i); // S{i}(p)x*IC{i} - IC{i} S{i}(p)x |
| 215 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | oBicpsidot = oYcrb.variation(psid_dm); // new Bicpsidot in world frame |
| 216 | |||
| 217 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | Force f_tmp = oYcrb * S_i; // IC{i}S{i}(:,p) |
| 218 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | ForceCrossMatrix(f_tmp, r0); // cmf_bar(IC{i}S{i}(:,p)) |
| 219 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | Bicphii += r0; |
| 220 | |||
| 221 |
2/4✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
|
256 | f_tmp = oYcrb * psid_dm; // IC{i}S{i}(:,p) |
| 222 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | addForceCrossMatrix(f_tmp, oBicpsidot); // cmf_bar(IC{i}S{i}(:,p)) |
| 223 | |||
| 224 |
4/8✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
|
256 | r2.noalias() = 2 * r0 - Bicphii; |
| 225 | |||
| 226 |
6/12✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
|
256 | r3.noalias() = oBicpsidot - S_iA.transpose() * oBcrb |
| 227 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | - oBcrb * S_iA; // Bicpsidot + S{i}(p)x*BC{i}- BC {i}S{i}(p)x |
| 228 | |||
| 229 | // r4 | ||
| 230 |
6/12✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 128 times.
✗ Branch 17 not taken.
|
256 | f_tmp.toVector().noalias() = oBcrb.transpose() * S_i.toVector(); |
| 231 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | ForceCrossMatrix(f_tmp, r4); // cmf_bar(BC{i}.'S{i}(:,p)) |
| 232 | // r5 | ||
| 233 |
5/10✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
|
256 | f_tmp.toVector().noalias() = oBcrb * psid_dm.toVector(); |
| 234 |
2/4✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
|
256 | f_tmp += S_i.cross(data.of[i]); |
| 235 |
3/6✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
|
256 | motionSet::inertiaAction<ADDTO>(oYcrb, psidd_dm.toVector(), f_tmp.toVector()); |
| 236 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | ForceCrossMatrix( |
| 237 | f_tmp, | ||
| 238 | r5); // cmf_bar(BC{i}psi_dot{i}(:,p)+IC{i}psi_ddot{i}(:,p)+S{i}(:,p)x*f{i}) | ||
| 239 | |||
| 240 | // S{i}(:,p)x* IC{i} + r0 | ||
| 241 |
3/6✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
|
256 | r6 = r0 + oYcrb.vxi(S_i); |
| 242 | |||
| 243 | // r7 | ||
| 244 |
5/10✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
|
256 | f_tmp.toVector().noalias() = oBcrb * S_i.toVector(); |
| 245 |
3/6✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
|
256 | f_tmp += oYcrb * (psid_dm + phid_dm); |
| 246 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
256 | ForceCrossMatrix(f_tmp, r7); // cmf_bar(BC{i}S{i}(:,p) + |
| 247 | // IC{i}(psi_dot{i}(:,p)+phi_dot{i}(:,p))) | ||
| 248 | |||
| 249 | 256 | JointIndex j = i; | |
| 250 | |||
| 251 |
2/2✓ Branch 0 taken 572 times.
✓ Branch 1 taken 128 times.
|
1400 | while (j > 0) |
| 252 | { | ||
| 253 | |||
| 254 |
2/2✓ Branch 1 taken 1212 times.
✓ Branch 2 taken 572 times.
|
3568 | for (int q = 0; q < model.nvs[j]; q++) |
| 255 | { | ||
| 256 | 2424 | const Eigen::DenseIndex jq = model.idx_vs[j] + q; | |
| 257 | |||
| 258 |
2/4✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
|
2424 | const MotionRef<typename Data::Matrix6x::ColXpr> S_j = data.J.col(jq); |
| 259 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = |
| 260 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | data.psid.col(jq); // psi_dot{j}(:,q) |
| 261 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = |
| 262 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | data.psidd.col(jq); // psi_ddot{j}(:,q) |
| 263 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = |
| 264 |
1/2✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
|
2424 | data.dJ.col(jq); // phi_dot{j}(:,q) |
| 265 | |||
| 266 |
5/10✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
|
2424 | u1.noalias() = S_j.toVector().transpose() * r3; |
| 267 |
5/10✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
|
2424 | u2.noalias() = S_j.toVector().transpose() * r1; |
| 268 |
10/20✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1212 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1212 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1212 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1212 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1212 times.
✗ Branch 29 not taken.
|
2424 | u3.noalias() = r3 * psid_dm.toVector() + r1 * psidd_dm.toVector() + r5 * S_j.toVector(); |
| 269 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u4.noalias() = r6 * S_j.toVector(); |
| 270 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u5.noalias() = r2 * psid_dm.toVector(); |
| 271 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u6.noalias() = Bicphii * psid_dm.toVector(); |
| 272 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u6.noalias() += r7 * S_j.toVector(); |
| 273 |
9/18✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1212 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1212 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1212 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1212 times.
✗ Branch 26 not taken.
|
2424 | u7.noalias() = r3 * S_j.toVector() + r1 * (psid_dm.toVector() + phid_dm.toVector()); |
| 274 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u8.noalias() = r4 * S_j.toVector(); |
| 275 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u9.noalias() = r0 * S_j.toVector(); |
| 276 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u10.noalias() = Bicphii * S_j.toVector(); |
| 277 |
5/10✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
|
2424 | u11.noalias() = S_j.toVector().transpose() * Bicphii; |
| 278 |
5/10✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1212 times.
✗ Branch 14 not taken.
|
2424 | u12.noalias() = psid_dm.toVector().transpose() * Bicphii; |
| 279 |
4/8✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
|
2424 | u13.noalias() = r1 * S_j.toVector(); |
| 280 | |||
| 281 | 2424 | JointIndex k = j; | |
| 282 | |||
| 283 |
2/2✓ Branch 0 taken 2604 times.
✓ Branch 1 taken 1212 times.
|
7632 | while (k > 0) |
| 284 | { | ||
| 285 | |||
| 286 |
2/2✓ Branch 1 taken 8664 times.
✓ Branch 2 taken 2604 times.
|
22536 | for (int r = 0; r < model.nvs[k]; r++) |
| 287 | { | ||
| 288 | 17328 | const Eigen::DenseIndex kr = model.idx_vs[k] + r; | |
| 289 | |||
| 290 |
2/4✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8664 times.
✗ Branch 5 not taken.
|
17328 | const MotionRef<typename Data::Matrix6x::ColXpr> S_k(data.J.col(kr)); |
| 291 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | const MotionRef<typename Data::Matrix6x::ColXpr> psid_dm = |
| 292 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | data.psid.col(kr); // psi_dot{k}(:,r) |
| 293 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | const MotionRef<typename Data::Matrix6x::ColXpr> psidd_dm = |
| 294 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | data.psidd.col(kr); // psi_ddot{k}(:,r) |
| 295 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | const MotionRef<typename Data::Matrix6x::ColXpr> phid_dm = |
| 296 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | data.dJ.col(kr); // phi_dot{k}(:,r) |
| 297 | |||
| 298 |
3/6✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8664 times.
✗ Branch 8 not taken.
|
17328 | p1 = u11 * psid_dm.toVector(); |
| 299 |
2/4✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8664 times.
✗ Branch 5 not taken.
|
17328 | p2 = u9.dot(psidd_dm.toVector()); |
| 300 |
6/12✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8664 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8664 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8664 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8664 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8664 times.
✗ Branch 17 not taken.
|
17328 | p2 += (-u12 + u8.transpose()) * psid_dm.toVector(); |
| 301 | |||
| 302 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | d2tau_dqdq_(ip, jq, kr) = p2; |
| 303 |
1/2✓ Branch 1 taken 8664 times.
✗ Branch 2 not taken.
|
17328 | dtau_dqdv_(ip, kr, jq) = -p1; |
| 304 | |||
| 305 |
2/2✓ Branch 0 taken 6732 times.
✓ Branch 1 taken 1932 times.
|
17328 | if (j != i) |
| 306 | { | ||
| 307 |
4/8✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6732 times.
✗ Branch 11 not taken.
|
13464 | p3 = -u11 * S_k.toVector(); |
| 308 |
2/4✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
|
13464 | p4 = S_k.toVector().dot(u13); |
| 309 |
4/8✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6732 times.
✗ Branch 11 not taken.
|
13464 | d2tau_dqdq_(jq, kr, ip) = u1 * psid_dm.toVector(); |
| 310 |
4/8✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6732 times.
✗ Branch 11 not taken.
|
13464 | d2tau_dqdq_(jq, kr, ip) += u2 * psidd_dm.toVector(); |
| 311 |
2/4✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
|
13464 | d2tau_dqdq_(jq, ip, kr) = d2tau_dqdq_(jq, kr, ip); |
| 312 |
1/2✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
|
13464 | dtau_dqdv_(jq, kr, ip) = p1; |
| 313 |
4/8✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6732 times.
✗ Branch 11 not taken.
|
13464 | dtau_dqdv_(jq, ip, kr) = u1 * S_k.toVector(); |
| 314 |
6/12✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6732 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6732 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6732 times.
✗ Branch 17 not taken.
|
13464 | dtau_dqdv_(jq, ip, kr) += u2 * (psid_dm.toVector() + phid_dm.toVector()); |
| 315 |
1/2✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
|
13464 | d2tau_dvdv_(jq, kr, ip) = -p3; |
| 316 |
1/2✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
|
13464 | d2tau_dvdv_(jq, ip, kr) = -p3; |
| 317 |
1/2✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
|
13464 | dtau_dadq_(kr, jq, ip) = p4; |
| 318 |
1/2✓ Branch 1 taken 6732 times.
✗ Branch 2 not taken.
|
13464 | dtau_dadq_(jq, kr, ip) = p4; |
| 319 | } | ||
| 320 | |||
| 321 |
2/2✓ Branch 0 taken 3612 times.
✓ Branch 1 taken 5052 times.
|
17328 | if (k != j) |
| 322 | { | ||
| 323 |
4/8✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3612 times.
✗ Branch 11 not taken.
|
7224 | p3 = -u11 * S_k.toVector(); |
| 324 |
2/4✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
|
7224 | p5 = S_k.toVector().dot(u9); |
| 325 |
1/2✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
|
7224 | d2tau_dqdq_(ip, kr, jq) = p2; |
| 326 |
3/6✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
|
7224 | d2tau_dqdq_(kr, ip, jq) = S_k.toVector().dot(u3); |
| 327 |
1/2✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
|
7224 | d2tau_dvdv_(ip, jq, kr) = p3; |
| 328 |
1/2✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
|
7224 | d2tau_dvdv_(ip, kr, jq) = p3; |
| 329 |
4/8✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3612 times.
✗ Branch 11 not taken.
|
7224 | dtau_dqdv_(ip, jq, kr) = S_k.toVector().dot(u5 + u8); |
| 330 |
5/10✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3612 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3612 times.
✗ Branch 14 not taken.
|
7224 | dtau_dqdv_(ip, jq, kr) += u9.dot(psid_dm.toVector() + phid_dm.toVector()); |
| 331 |
3/6✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
|
7224 | dtau_dqdv_(kr, jq, ip) = S_k.toVector().dot(u6); |
| 332 |
1/2✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
|
7224 | dtau_dadq_(kr, ip, jq) = p5; |
| 333 |
1/2✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
|
7224 | dtau_dadq_(ip, kr, jq) = p5; |
| 334 |
2/2✓ Branch 0 taken 2648 times.
✓ Branch 1 taken 964 times.
|
7224 | if (j != i) |
| 335 | { | ||
| 336 |
2/4✓ Branch 1 taken 2648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2648 times.
✗ Branch 5 not taken.
|
5296 | p6 = S_k.toVector().dot(u10); |
| 337 |
2/4✓ Branch 1 taken 2648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2648 times.
✗ Branch 5 not taken.
|
5296 | d2tau_dqdq_(kr, jq, ip) = d2tau_dqdq_(kr, ip, jq); |
| 338 |
1/2✓ Branch 1 taken 2648 times.
✗ Branch 2 not taken.
|
5296 | d2tau_dvdv_(kr, ip, jq) = p6; |
| 339 |
1/2✓ Branch 1 taken 2648 times.
✗ Branch 2 not taken.
|
5296 | d2tau_dvdv_(kr, jq, ip) = p6; |
| 340 |
3/6✓ Branch 1 taken 2648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2648 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2648 times.
✗ Branch 8 not taken.
|
5296 | dtau_dqdv_(kr, ip, jq) = S_k.toVector().dot(u7); |
| 341 | } | ||
| 342 | else | ||
| 343 | { | ||
| 344 |
3/6✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
|
1928 | d2tau_dvdv_(kr, jq, ip) = S_k.toVector().dot(u4); |
| 345 | } | ||
| 346 | } | ||
| 347 | else | ||
| 348 | { | ||
| 349 |
5/10✓ Branch 1 taken 5052 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5052 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5052 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5052 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5052 times.
✗ Branch 14 not taken.
|
10104 | d2tau_dvdv_(ip, jq, kr) = -u2 * S_k.toVector(); |
| 350 | } | ||
| 351 | } | ||
| 352 | |||
| 353 | 5208 | k = model.parents[k]; | |
| 354 | } | ||
| 355 | } | ||
| 356 | 1144 | j = model.parents[j]; | |
| 357 | } | ||
| 358 | } | ||
| 359 | |||
| 360 |
2/2✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
|
216 | if (parent > 0) |
| 361 | { | ||
| 362 |
1/2✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
|
208 | data.oYcrb[parent] += data.oYcrb[i]; |
| 363 |
1/2✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
|
208 | data.doYcrb[parent] += data.doYcrb[i]; |
| 364 |
1/2✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
|
208 | data.of[parent] += data.of[i]; |
| 365 | } | ||
| 366 | } | ||
| 367 | |||
| 368 | // Function for cmf_bar operator | ||
| 369 | template<typename ForceDerived, typename M6> | ||
| 370 | static void | ||
| 371 | 512 | ForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
| 372 | { | ||
| 373 | 512 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
| 374 |
1/2✓ Branch 2 taken 512 times.
✗ Branch 3 not taken.
|
512 | mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::LINEAR).setZero(); |
| 375 |
1/2✓ Branch 1 taken 512 times.
✗ Branch 2 not taken.
|
512 | mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR) = |
| 376 |
5/10✓ Branch 2 taken 512 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 512 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 512 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 512 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 512 times.
✗ Branch 15 not taken.
|
512 | mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR) = skew(-f.linear()); |
| 377 |
4/8✓ Branch 2 taken 512 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 512 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 512 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 512 times.
✗ Branch 12 not taken.
|
512 | mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR) = skew(-f.angular()); |
| 378 | 512 | } | |
| 379 | template<typename ForceDerived, typename M6> | ||
| 380 | |||
| 381 | static void | ||
| 382 | 128 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
| 383 | { | ||
| 384 | 128 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
| 385 |
3/6✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
|
128 | addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
| 386 |
3/6✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
|
128 | addSkew(-f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
| 387 |
2/4✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
|
128 | addSkew( |
| 388 |
1/2✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
|
128 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
| 389 | 128 | } | |
| 390 | }; | ||
| 391 | |||
| 392 | template< | ||
| 393 | typename Scalar, | ||
| 394 | int Options, | ||
| 395 | template<typename, int> class JointCollectionTpl, | ||
| 396 | typename ConfigVectorType, | ||
| 397 | typename TangentVectorType1, | ||
| 398 | typename TangentVectorType2, | ||
| 399 | typename Tensor1, | ||
| 400 | typename Tensor2, | ||
| 401 | typename Tensor3, | ||
| 402 | typename Tensor4> | ||
| 403 | 8 | inline void ComputeRNEASecondOrderDerivatives( | |
| 404 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 405 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 406 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 407 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 408 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
| 409 | const Tensor1 & d2tau_dqdq, | ||
| 410 | const Tensor2 & d2tau_dvdv, | ||
| 411 | const Tensor3 & dtau_dqdv, | ||
| 412 | const Tensor4 & dtau_dadq) | ||
| 413 | { | ||
| 414 | // Extra safety here | ||
| 415 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 416 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
| 417 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 418 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
| 419 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 420 | a.size(), model.nv, "The joint acceleration vector is not of right size"); | ||
| 421 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(0), model.nv); |
| 422 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(1), model.nv); |
| 423 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dqdq.dimension(2), model.nv); |
| 424 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(0), model.nv); |
| 425 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(1), model.nv); |
| 426 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(d2tau_dvdv.dimension(2), model.nv); |
| 427 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(0), model.nv); |
| 428 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(1), model.nv); |
| 429 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dqdv.dimension(2), model.nv); |
| 430 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(0), model.nv); |
| 431 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(1), model.nv); |
| 432 |
1/26✗ Branch 1 not taken.
✓ Branch 2 taken 4 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
8 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(2), model.nv); |
| 433 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | assert(model.check(data) && "data is not consistent with model."); |
| 434 | |||
| 435 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 436 | typedef typename Model::JointIndex JointIndex; | ||
| 437 | |||
| 438 | typedef ComputeRNEASecondOrderDerivativesForwardStep< | ||
| 439 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, TangentVectorType2> | ||
| 440 | Pass1; | ||
| 441 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
224 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 442 | { | ||
| 443 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Pass1::run( |
| 444 | 216 | model.joints[i], data.joints[i], | |
| 445 | 432 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
| 446 | } | ||
| 447 | |||
| 448 | typedef ComputeRNEASecondOrderDerivativesBackwardStep< | ||
| 449 | Scalar, Options, JointCollectionTpl, Tensor1, Tensor2, Tensor3, Tensor4> | ||
| 450 | Pass2; | ||
| 451 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
224 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
| 452 | { | ||
| 453 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | Pass2::run( |
| 454 | 216 | model.joints[i], | |
| 455 | 432 | typename Pass2::ArgsType( | |
| 456 | model, data, const_cast<Tensor1 &>(d2tau_dqdq), const_cast<Tensor2 &>(d2tau_dvdv), | ||
| 457 | const_cast<Tensor3 &>(dtau_dqdv), const_cast<Tensor4 &>(dtau_dadq))); | ||
| 458 | } | ||
| 459 | 8 | } | |
| 460 | |||
| 461 | } // namespace pinocchio | ||
| 462 | |||
| 463 | #endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hxx__ | ||
| 464 |