| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/jacobian.hxx |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 131 | 138 | 94.9% |
| Branches: | 137 | 496 | 27.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_jacobian_hxx__ | ||
| 6 | #define __pinocchio_algorithm_jacobian_hxx__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/visitor.hpp" | ||
| 9 | #include "pinocchio/algorithm/check.hpp" | ||
| 10 | |||
| 11 | /// @cond DEV | ||
| 12 | |||
| 13 | namespace pinocchio | ||
| 14 | { | ||
| 15 | namespace impl | ||
| 16 | { | ||
| 17 | template< | ||
| 18 | typename Scalar, | ||
| 19 | int Options, | ||
| 20 | template<typename, int> | ||
| 21 | class JointCollectionTpl, | ||
| 22 | typename ConfigVectorType, | ||
| 23 | typename Matrix6xLike> | ||
| 24 | struct JointJacobiansForwardStep | ||
| 25 | : public fusion::JointUnaryVisitorBase<JointJacobiansForwardStep< | ||
| 26 | Scalar, | ||
| 27 | Options, | ||
| 28 | JointCollectionTpl, | ||
| 29 | ConfigVectorType, | ||
| 30 | Matrix6xLike>> | ||
| 31 | { | ||
| 32 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 33 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 34 | |||
| 35 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &> | ||
| 36 | ArgsType; | ||
| 37 | |||
| 38 | template<typename JointModel> | ||
| 39 | 14486 | static void algo( | |
| 40 | const JointModelBase<JointModel> & jmodel, | ||
| 41 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 42 | const Model & model, | ||
| 43 | Data & data, | ||
| 44 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 45 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 46 | { | ||
| 47 | typedef typename Model::JointIndex JointIndex; | ||
| 48 | |||
| 49 |
1/2✓ Branch 1 taken 7243 times.
✗ Branch 2 not taken.
|
14486 | const JointIndex & i = jmodel.id(); |
| 50 | 14486 | const JointIndex & parent = model.parents[i]; | |
| 51 | |||
| 52 |
1/2✓ Branch 3 taken 7243 times.
✗ Branch 4 not taken.
|
14486 | jmodel.calc(jdata.derived(), q.derived()); |
| 53 | |||
| 54 |
6/10✓ Branch 1 taken 7243 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6965 times.
✓ Branch 5 taken 278 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6965 times.
✓ Branch 9 taken 278 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6965 times.
✗ Branch 13 not taken.
|
14486 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 55 |
2/2✓ Branch 0 taken 6955 times.
✓ Branch 1 taken 288 times.
|
14486 | if (parent > 0) |
| 56 |
2/4✓ Branch 3 taken 6955 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6955 times.
✗ Branch 8 not taken.
|
13910 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
| 57 | else | ||
| 58 |
1/2✓ Branch 3 taken 288 times.
✗ Branch 4 not taken.
|
576 | data.oMi[i] = data.liMi[i]; |
| 59 | |||
| 60 | 14486 | Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); | |
| 61 |
4/8✓ Branch 2 taken 7243 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7243 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7243 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7243 times.
✗ Branch 12 not taken.
|
14486 | jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); |
| 62 | } | ||
| 63 | }; | ||
| 64 | |||
| 65 | template< | ||
| 66 | typename Scalar, | ||
| 67 | int Options, | ||
| 68 | template<typename, int> | ||
| 69 | class JointCollectionTpl, | ||
| 70 | typename ConfigVectorType> | ||
| 71 | 285 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians( | |
| 72 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 73 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 74 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 75 | { | ||
| 76 |
1/2✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
|
285 | assert(model.check(data) && "data is not consistent with model."); |
| 77 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 285 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.
|
285 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 78 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
| 79 | |||
| 80 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 81 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 82 | typedef typename Model::JointIndex JointIndex; | ||
| 83 | typedef typename Data::Matrix6x Matrix6x; | ||
| 84 | |||
| 85 | typedef JointJacobiansForwardStep< | ||
| 86 | Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6x> | ||
| 87 | Pass; | ||
| 88 | typedef typename Pass::ArgsType ArgsType; | ||
| 89 |
2/2✓ Branch 0 taken 7228 times.
✓ Branch 1 taken 285 times.
|
7513 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 90 | { | ||
| 91 |
1/2✓ Branch 1 taken 7228 times.
✗ Branch 2 not taken.
|
7228 | Pass::run( |
| 92 | 7228 | model.joints[i], data.joints[i], | |
| 93 | 14456 | ArgsType(model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6x, data.J))); | |
| 94 | } | ||
| 95 | |||
| 96 | 285 | return data.J; | |
| 97 | } | ||
| 98 | |||
| 99 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 100 | struct JointJacobiansForwardStep2 | ||
| 101 | : public fusion::JointUnaryVisitorBase< | ||
| 102 | JointJacobiansForwardStep2<Scalar, Options, JointCollectionTpl>> | ||
| 103 | { | ||
| 104 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 105 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 106 | |||
| 107 | typedef boost::fusion::vector<Data &> ArgsType; | ||
| 108 | |||
| 109 | template<typename JointModel> | ||
| 110 | 54 | static void algo( | |
| 111 | const JointModelBase<JointModel> & jmodel, | ||
| 112 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 113 | Data & data) | ||
| 114 | { | ||
| 115 | typedef typename Model::JointIndex JointIndex; | ||
| 116 | |||
| 117 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | const JointIndex & i = jmodel.id(); |
| 118 |
4/8✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
|
54 | jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); |
| 119 | } | ||
| 120 | }; | ||
| 121 | } // namespace impl | ||
| 122 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 123 | 1 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians( | |
| 124 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 125 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
| 126 | { | ||
| 127 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
| 128 | |||
| 129 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 130 | typedef typename Model::JointIndex JointIndex; | ||
| 131 | |||
| 132 | typedef impl::JointJacobiansForwardStep2<Scalar, Options, JointCollectionTpl> Pass; | ||
| 133 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 134 | { | ||
| 135 |
1/2✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(data)); |
| 136 | } | ||
| 137 | |||
| 138 | 1 | return data.J; | |
| 139 | } | ||
| 140 | |||
| 141 | namespace details | ||
| 142 | { | ||
| 143 | template<typename Scalar, int Options, typename Matrix6xLikeIn, typename Matrix6xLikeOut> | ||
| 144 | 302 | void translateJointJacobian( | |
| 145 | const SE3Tpl<Scalar, Options> & placement, | ||
| 146 | const Eigen::MatrixBase<Matrix6xLikeIn> & Jin, | ||
| 147 | const Eigen::MatrixBase<Matrix6xLikeOut> & Jout) | ||
| 148 | { | ||
| 149 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 151 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.
|
302 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6); |
| 150 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 151 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 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.
|
302 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols()); |
| 151 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 151 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.
|
302 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); |
| 152 | |||
| 153 | 302 | Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); | |
| 154 | |||
| 155 | typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; | ||
| 156 | typedef const MotionRef<ConstColXprIn> MotionIn; | ||
| 157 | |||
| 158 | typedef typename Matrix6xLikeOut::ColXpr ColXprOut; | ||
| 159 | typedef MotionRef<ColXprOut> MotionOut; | ||
| 160 | |||
| 161 |
2/2✓ Branch 1 taken 276 times.
✓ Branch 2 taken 151 times.
|
854 | for (Eigen::DenseIndex j = 0; j < Jin.cols(); ++j) |
| 162 | { | ||
| 163 |
2/4✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
|
552 | MotionIn v_in(Jin.col(j)); |
| 164 |
2/4✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
|
552 | MotionOut v_out(Jout_.col(j)); |
| 165 | |||
| 166 |
1/2✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
|
552 | v_out = v_in; |
| 167 |
5/10✓ 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 taken 276 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 276 times.
✗ Branch 14 not taken.
|
552 | v_out.linear() -= placement.translation().cross(v_in.angular()); |
| 168 | } | ||
| 169 | } | ||
| 170 | |||
| 171 | template< | ||
| 172 | typename Scalar, | ||
| 173 | int Options, | ||
| 174 | template<typename, int> | ||
| 175 | class JointCollectionTpl, | ||
| 176 | typename Matrix6xLikeIn, | ||
| 177 | typename Matrix6xLikeOut> | ||
| 178 | 454 | void translateJointJacobian( | |
| 179 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 180 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 181 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex joint_id, | ||
| 182 | const ReferenceFrame rf, | ||
| 183 | const SE3Tpl<Scalar, Options> & placement, | ||
| 184 | const Eigen::MatrixBase<Matrix6xLikeIn> & Jin, | ||
| 185 | const Eigen::MatrixBase<Matrix6xLikeOut> & Jout) | ||
| 186 | { | ||
| 187 |
1/2✓ Branch 1 taken 454 times.
✗ Branch 2 not taken.
|
454 | assert(model.check(data) && "data is not consistent with model."); |
| 188 | |||
| 189 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 454 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.
|
454 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6); |
| 190 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 454 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.
|
454 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv); |
| 191 | |||
| 192 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 454 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.
|
454 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); |
| 193 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 454 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.
|
454 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); |
| 194 | |||
| 195 | 454 | Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); | |
| 196 | |||
| 197 | typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; | ||
| 198 | typedef const MotionRef<ConstColXprIn> MotionIn; | ||
| 199 | |||
| 200 | typedef typename Matrix6xLikeOut::ColXpr ColXprOut; | ||
| 201 | typedef MotionRef<ColXprOut> MotionOut; | ||
| 202 | |||
| 203 | 454 | const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; | |
| 204 |
3/4✓ Branch 0 taken 77 times.
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 317 times.
✗ Branch 3 not taken.
|
454 | switch (rf) |
| 205 | { | ||
| 206 | 77 | case WORLD: { | |
| 207 |
2/2✓ Branch 1 taken 834 times.
✓ Branch 2 taken 77 times.
|
911 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
| 208 | { | ||
| 209 |
2/4✓ Branch 1 taken 834 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 834 times.
✗ Branch 5 not taken.
|
834 | MotionIn v_in(Jin.col(j)); |
| 210 |
2/4✓ Branch 1 taken 834 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 834 times.
✗ Branch 5 not taken.
|
834 | MotionOut v_out(Jout_.col(j)); |
| 211 | |||
| 212 |
1/2✓ Branch 1 taken 834 times.
✗ Branch 2 not taken.
|
834 | v_out = v_in; |
| 213 | } | ||
| 214 | 77 | break; | |
| 215 | } | ||
| 216 | 60 | case LOCAL_WORLD_ALIGNED: { | |
| 217 |
2/2✓ Branch 0 taken 638 times.
✓ Branch 1 taken 60 times.
|
698 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
| 218 | { | ||
| 219 |
2/4✓ Branch 1 taken 638 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 638 times.
✗ Branch 5 not taken.
|
638 | MotionIn v_in(Jin.col(j)); |
| 220 |
2/4✓ Branch 1 taken 638 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 638 times.
✗ Branch 5 not taken.
|
638 | MotionOut v_out(Jout_.col(j)); |
| 221 | |||
| 222 |
1/2✓ Branch 1 taken 638 times.
✗ Branch 2 not taken.
|
638 | v_out = v_in; |
| 223 |
5/10✓ Branch 1 taken 638 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 638 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 638 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 638 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 638 times.
✗ Branch 14 not taken.
|
638 | v_out.linear() -= placement.translation().cross(v_in.angular()); |
| 224 | } | ||
| 225 | 60 | break; | |
| 226 | } | ||
| 227 | 317 | case LOCAL: { | |
| 228 |
2/2✓ Branch 0 taken 3449 times.
✓ Branch 1 taken 317 times.
|
3766 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
| 229 | { | ||
| 230 |
2/4✓ Branch 1 taken 3449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3449 times.
✗ Branch 5 not taken.
|
3449 | MotionIn v_in(Jin.col(j)); |
| 231 |
2/4✓ Branch 1 taken 3449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3449 times.
✗ Branch 5 not taken.
|
3449 | MotionOut v_out(Jout_.col(j)); |
| 232 | |||
| 233 |
2/4✓ Branch 1 taken 3449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3449 times.
✗ Branch 5 not taken.
|
3449 | v_out = placement.actInv(v_in); |
| 234 | } | ||
| 235 | 317 | break; | |
| 236 | } | ||
| 237 | ✗ | default: | |
| 238 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "must never happened"); | |
| 239 | break; | ||
| 240 | } | ||
| 241 | 454 | } | |
| 242 | |||
| 243 | template< | ||
| 244 | typename Scalar, | ||
| 245 | int Options, | ||
| 246 | template<typename, int> | ||
| 247 | class JointCollectionTpl, | ||
| 248 | typename Matrix6xLikeIn, | ||
| 249 | typename Matrix6xLikeOut> | ||
| 250 | 410 | void translateJointJacobian( | |
| 251 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 252 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 253 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex joint_id, | ||
| 254 | const ReferenceFrame rf, | ||
| 255 | const Eigen::MatrixBase<Matrix6xLikeIn> & Jin, | ||
| 256 | const Eigen::MatrixBase<Matrix6xLikeOut> & Jout) | ||
| 257 | { | ||
| 258 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 259 | 410 | const typename Data::SE3 & oMjoint = data.oMi[joint_id]; | |
| 260 | |||
| 261 | 410 | translateJointJacobian(model, data, joint_id, rf, oMjoint, Jin, Jout); | |
| 262 | 410 | } | |
| 263 | } // namespace details | ||
| 264 | namespace impl | ||
| 265 | { | ||
| 266 | /* Return the jacobian of the output frame attached to joint <jointId> in the | ||
| 267 | world frame or in the local frame depending on the template argument. The | ||
| 268 | function computeJacobians should have been called first. */ | ||
| 269 | template< | ||
| 270 | typename Scalar, | ||
| 271 | int Options, | ||
| 272 | template<typename, int> | ||
| 273 | class JointCollectionTpl, | ||
| 274 | typename Matrix6xLike> | ||
| 275 | 404 | void getJointJacobian( | |
| 276 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 277 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 278 | const JointIndex joint_id, | ||
| 279 | const ReferenceFrame reference_frame, | ||
| 280 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 281 | { | ||
| 282 |
1/2✓ Branch 1 taken 404 times.
✗ Branch 2 not taken.
|
404 | assert(model.check(data) && "data is not consistent with model."); |
| 283 | |||
| 284 | 404 | ::pinocchio::details::translateJointJacobian( | |
| 285 | 404 | model, data, joint_id, reference_frame, data.J, | |
| 286 | 404 | PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)); | |
| 287 | 404 | } | |
| 288 | |||
| 289 | template< | ||
| 290 | typename Scalar, | ||
| 291 | int Options, | ||
| 292 | template<typename, int> | ||
| 293 | class JointCollectionTpl, | ||
| 294 | typename ConfigVectorType, | ||
| 295 | typename Matrix6xLike> | ||
| 296 | struct JointJacobianForwardStep | ||
| 297 | : public fusion::JointUnaryVisitorBase<JointJacobianForwardStep< | ||
| 298 | Scalar, | ||
| 299 | Options, | ||
| 300 | JointCollectionTpl, | ||
| 301 | ConfigVectorType, | ||
| 302 | Matrix6xLike>> | ||
| 303 | { | ||
| 304 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 305 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 306 | |||
| 307 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &, Matrix6xLike &> | ||
| 308 | ArgsType; | ||
| 309 | |||
| 310 | template<typename JointModel> | ||
| 311 | 2264 | static void algo( | |
| 312 | const JointModelBase<JointModel> & jmodel, | ||
| 313 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 314 | const Model & model, | ||
| 315 | Data & data, | ||
| 316 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 317 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 318 | { | ||
| 319 | typedef typename Model::JointIndex JointIndex; | ||
| 320 |
1/2✓ Branch 1 taken 1132 times.
✗ Branch 2 not taken.
|
2264 | const JointIndex & i = jmodel.id(); |
| 321 | 2264 | const JointIndex & parent = model.parents[i]; | |
| 322 | |||
| 323 |
1/2✓ Branch 3 taken 1132 times.
✗ Branch 4 not taken.
|
2264 | jmodel.calc(jdata.derived(), q.derived()); |
| 324 | |||
| 325 |
6/10✓ Branch 1 taken 1132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1122 times.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1122 times.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1122 times.
✗ Branch 13 not taken.
|
2264 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 326 |
2/4✓ Branch 3 taken 1132 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1132 times.
✗ Branch 8 not taken.
|
2264 | data.iMf[parent] = data.liMi[i] * data.iMf[i]; |
| 327 | |||
| 328 | 2264 | Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); | |
| 329 |
4/8✓ Branch 2 taken 1132 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1132 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1132 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1132 times.
✗ Branch 12 not taken.
|
2264 | jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); |
| 330 | } | ||
| 331 | }; | ||
| 332 | |||
| 333 | template< | ||
| 334 | typename Scalar, | ||
| 335 | int Options, | ||
| 336 | template<typename, int> | ||
| 337 | class JointCollectionTpl, | ||
| 338 | typename ConfigVectorType, | ||
| 339 | typename Matrix6xLike> | ||
| 340 | 183 | void computeJointJacobian( | |
| 341 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 342 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 343 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 344 | const JointIndex jointId, | ||
| 345 | const Eigen::MatrixBase<Matrix6xLike> & J) | ||
| 346 | { | ||
| 347 |
1/2✓ Branch 1 taken 183 times.
✗ Branch 2 not taken.
|
183 | assert(model.check(data) && "data is not consistent with model."); |
| 348 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 183 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.
|
183 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 349 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
| 350 | |||
| 351 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 352 | typedef typename Model::JointIndex JointIndex; | ||
| 353 | |||
| 354 | 183 | data.iMf[jointId].setIdentity(); | |
| 355 | typedef JointJacobianForwardStep< | ||
| 356 | Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> | ||
| 357 | Pass; | ||
| 358 |
2/2✓ Branch 0 taken 1106 times.
✓ Branch 1 taken 183 times.
|
1289 | for (JointIndex i = jointId; i > 0; i = model.parents[i]) |
| 359 | { | ||
| 360 |
1/2✓ Branch 1 taken 1106 times.
✗ Branch 2 not taken.
|
1106 | Pass::run( |
| 361 | 1106 | model.joints[i], data.joints[i], | |
| 362 | 2212 | typename Pass::ArgsType( | |
| 363 | 1106 | model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J))); | |
| 364 | } | ||
| 365 | 183 | } | |
| 366 | |||
| 367 | template< | ||
| 368 | typename Scalar, | ||
| 369 | int Options, | ||
| 370 | template<typename, int> | ||
| 371 | class JointCollectionTpl, | ||
| 372 | typename ConfigVectorType, | ||
| 373 | typename TangentVectorType> | ||
| 374 | struct JointJacobiansTimeVariationForwardStep | ||
| 375 | : public fusion::JointUnaryVisitorBase<JointJacobiansTimeVariationForwardStep< | ||
| 376 | Scalar, | ||
| 377 | Options, | ||
| 378 | JointCollectionTpl, | ||
| 379 | ConfigVectorType, | ||
| 380 | TangentVectorType>> | ||
| 381 | { | ||
| 382 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 383 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 384 | |||
| 385 | typedef boost::fusion:: | ||
| 386 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
| 387 | ArgsType; | ||
| 388 | |||
| 389 | template<typename JointModel> | ||
| 390 | 702 | static void algo( | |
| 391 | const JointModelBase<JointModel> & jmodel, | ||
| 392 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 393 | const Model & model, | ||
| 394 | Data & data, | ||
| 395 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 396 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
| 397 | { | ||
| 398 | typedef typename Model::JointIndex JointIndex; | ||
| 399 | typedef typename Data::SE3 SE3; | ||
| 400 | typedef typename Data::Motion Motion; | ||
| 401 | |||
| 402 |
1/2✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
|
702 | const JointIndex & i = (JointIndex)jmodel.id(); |
| 403 | 702 | const JointIndex & parent = model.parents[i]; | |
| 404 | |||
| 405 | 702 | SE3 & oMi = data.oMi[i]; | |
| 406 | 702 | Motion & vJ = data.v[i]; | |
| 407 | |||
| 408 |
1/2✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
|
702 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
| 409 | |||
| 410 |
2/4✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
|
702 | vJ = jdata.v(); |
| 411 | |||
| 412 |
6/10✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338 times.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 338 times.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 338 times.
✗ Branch 13 not taken.
|
702 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 413 |
2/2✓ Branch 0 taken 338 times.
✓ Branch 1 taken 13 times.
|
702 | if (parent > 0) |
| 414 | { | ||
| 415 |
2/4✓ Branch 3 taken 338 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 338 times.
✗ Branch 7 not taken.
|
676 | oMi = data.oMi[parent] * data.liMi[i]; |
| 416 |
2/4✓ Branch 3 taken 338 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 338 times.
✗ Branch 7 not taken.
|
676 | vJ += data.liMi[i].actInv(data.v[parent]); |
| 417 | } | ||
| 418 | else | ||
| 419 | { | ||
| 420 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
26 | oMi = data.liMi[i]; |
| 421 | } | ||
| 422 | |||
| 423 |
4/8✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 351 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 351 times.
✗ Branch 11 not taken.
|
702 | jmodel.jointCols(data.J) = oMi.act(jdata.S()); |
| 424 | |||
| 425 | // Spatial velocity of joint i expressed in the global frame o | ||
| 426 |
2/4✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 351 times.
✗ Branch 6 not taken.
|
702 | data.ov[i] = oMi.act(vJ); |
| 427 | |||
| 428 | typedef | ||
| 429 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 430 | ColsBlock; | ||
| 431 |
1/2✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
|
702 | ColsBlock dJcols = jmodel.jointCols(data.dJ); |
| 432 |
1/2✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
|
702 | ColsBlock Jcols = jmodel.jointCols(data.J); |
| 433 | |||
| 434 |
1/2✓ Branch 2 taken 351 times.
✗ Branch 3 not taken.
|
702 | motionSet::motionAction(data.ov[i], Jcols, dJcols); |
| 435 | } | ||
| 436 | }; | ||
| 437 | |||
| 438 | template< | ||
| 439 | typename Scalar, | ||
| 440 | int Options, | ||
| 441 | template<typename, int> | ||
| 442 | class JointCollectionTpl, | ||
| 443 | typename ConfigVectorType, | ||
| 444 | typename TangentVectorType> | ||
| 445 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & | ||
| 446 | 13 | computeJointJacobiansTimeVariation( | |
| 447 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 448 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 449 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 450 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
| 451 | { | ||
| 452 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | assert(model.check(data) && "data is not consistent with model."); |
| 453 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 13 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.
|
13 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 454 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
| 455 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 13 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.
|
13 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
| 456 | |||
| 457 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 458 | typedef typename Model::JointIndex JointIndex; | ||
| 459 | |||
| 460 | typedef JointJacobiansTimeVariationForwardStep< | ||
| 461 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType> | ||
| 462 | Pass; | ||
| 463 |
2/2✓ Branch 0 taken 351 times.
✓ Branch 1 taken 13 times.
|
364 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 464 | { | ||
| 465 |
1/2✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
|
351 | Pass::run( |
| 466 | 351 | model.joints[i], data.joints[i], | |
| 467 | 702 | typename Pass::ArgsType(model, data, q.derived(), v.derived())); | |
| 468 | } | ||
| 469 | |||
| 470 | 13 | return data.dJ; | |
| 471 | } | ||
| 472 | |||
| 473 | template< | ||
| 474 | typename Scalar, | ||
| 475 | int Options, | ||
| 476 | template<typename, int> | ||
| 477 | class JointCollectionTpl, | ||
| 478 | typename Matrix6xLike> | ||
| 479 | 6 | void getJointJacobianTimeVariation( | |
| 480 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 481 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 482 | const JointIndex jointId, | ||
| 483 | const ReferenceFrame rf, | ||
| 484 | const Eigen::MatrixBase<Matrix6xLike> & dJ) | ||
| 485 | { | ||
| 486 | 6 | ::pinocchio::details::translateJointJacobian( | |
| 487 | 6 | model, data, jointId, rf, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ)); | |
| 488 | 6 | } | |
| 489 | } // namespace impl | ||
| 490 | |||
| 491 | template< | ||
| 492 | typename Scalar, | ||
| 493 | int Options, | ||
| 494 | template<typename, int> | ||
| 495 | class JointCollectionTpl, | ||
| 496 | typename ConfigVectorType> | ||
| 497 | 1 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians( | |
| 498 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 499 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 500 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 501 | { | ||
| 502 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return impl::computeJointJacobians(model, data, make_const_ref(q)); |
| 503 | } | ||
| 504 | |||
| 505 | template< | ||
| 506 | typename Scalar, | ||
| 507 | int Options, | ||
| 508 | template<typename, int> | ||
| 509 | class JointCollectionTpl, | ||
| 510 | typename Matrix6Like> | ||
| 511 | 64 | void getJointJacobian( | |
| 512 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 513 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 514 | const JointIndex joint_id, | ||
| 515 | const ReferenceFrame reference_frame, | ||
| 516 | const Eigen::MatrixBase<Matrix6Like> & J) | ||
| 517 | { | ||
| 518 |
1/2✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
|
64 | impl::getJointJacobian(model, data, joint_id, reference_frame, make_ref(J)); |
| 519 | 64 | } | |
| 520 | |||
| 521 | template< | ||
| 522 | typename Scalar, | ||
| 523 | int Options, | ||
| 524 | template<typename, int> | ||
| 525 | class JointCollectionTpl, | ||
| 526 | typename ConfigVectorType, | ||
| 527 | typename Matrix6Like> | ||
| 528 | 89 | void computeJointJacobian( | |
| 529 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 530 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 531 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 532 | const JointIndex joint_id, | ||
| 533 | const Eigen::MatrixBase<Matrix6Like> & J) | ||
| 534 | { | ||
| 535 |
2/4✓ Branch 2 taken 89 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 89 times.
✗ Branch 6 not taken.
|
89 | impl::computeJointJacobian(model, data, make_const_ref(q), joint_id, make_ref(J)); |
| 536 | 89 | } | |
| 537 | |||
| 538 | template< | ||
| 539 | typename Scalar, | ||
| 540 | int Options, | ||
| 541 | template<typename, int> | ||
| 542 | class JointCollectionTpl, | ||
| 543 | typename ConfigVectorType, | ||
| 544 | typename TangentVectorType> | ||
| 545 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & | ||
| 546 | ✗ | computeJointJacobiansTimeVariation( | |
| 547 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 548 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 549 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 550 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
| 551 | |||
| 552 | { | ||
| 553 | ✗ | return impl::computeJointJacobiansTimeVariation( | |
| 554 | ✗ | model, data, make_const_ref(q), make_const_ref(v)); | |
| 555 | } | ||
| 556 | |||
| 557 | template< | ||
| 558 | typename Scalar, | ||
| 559 | int Options, | ||
| 560 | template<typename, int> | ||
| 561 | class JointCollectionTpl, | ||
| 562 | typename Matrix6Like> | ||
| 563 | ✗ | void getJointJacobianTimeVariation( | |
| 564 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 565 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 566 | const JointIndex joint_id, | ||
| 567 | const ReferenceFrame reference_frame, | ||
| 568 | const Eigen::MatrixBase<Matrix6Like> & dJ) | ||
| 569 | { | ||
| 570 | ✗ | impl::getJointJacobianTimeVariation(model, data, joint_id, reference_frame, make_ref(dJ)); | |
| 571 | } | ||
| 572 | |||
| 573 | } // namespace pinocchio | ||
| 574 | |||
| 575 | /// @endcond | ||
| 576 | |||
| 577 | #endif // ifndef __pinocchio_algorithm_jacobian_hxx__ | ||
| 578 |