| Directory: | ./ |
|---|---|
| File: | src/generic-transformation/helper.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 207 | 210 | 98.6% |
| Branches: | 263 | 458 | 57.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2016, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | |||
| 5 | // Redistribution and use in source and binary forms, with or without | ||
| 6 | // modification, are permitted provided that the following conditions are | ||
| 7 | // met: | ||
| 8 | // | ||
| 9 | // 1. Redistributions of source code must retain the above copyright | ||
| 10 | // notice, this list of conditions and the following disclaimer. | ||
| 11 | // | ||
| 12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 13 | // notice, this list of conditions and the following disclaimer in the | ||
| 14 | // documentation and/or other materials provided with the distribution. | ||
| 15 | // | ||
| 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 27 | // DAMAGE. | ||
| 28 | |||
| 29 | #ifndef SRC_GENERIC_TRANSFORMATION_HELPER_HH | ||
| 30 | #define SRC_GENERIC_TRANSFORMATION_HELPER_HH | ||
| 31 | |||
| 32 | #include <hpp/constraints/macros.hh> | ||
| 33 | #include <hpp/constraints/matrix-view.hh> | ||
| 34 | #include <hpp/constraints/tools.hh> // for logSO3 | ||
| 35 | #include <hpp/pinocchio/device.hh> | ||
| 36 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 37 | #include <hpp/pinocchio/joint.hh> | ||
| 38 | #include <pinocchio/multibody/model.hpp> | ||
| 39 | |||
| 40 | namespace hpp { | ||
| 41 | namespace constraints { | ||
| 42 | |||
| 43 | namespace { | ||
| 44 | 110 | inline const Transform3s& Id() { | |
| 45 |
5/8✓ Branch 0 taken 4 times.
✓ Branch 1 taken 106 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 3 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
110 | static const Transform3s id(Transform3s::Identity()); |
| 46 | 110 | return id; | |
| 47 | } | ||
| 48 | |||
| 49 | /// ------- Generic Transform Data --------------------------------------- | ||
| 50 | template <bool rel> | ||
| 51 | struct GTOriDataV {}; | ||
| 52 | template <> | ||
| 53 | struct GTOriDataV<true> { | ||
| 54 | Transform3s M; | ||
| 55 | value_type theta; | ||
| 56 | }; | ||
| 57 | template <bool rel> | ||
| 58 | struct GTOriDataJ {}; | ||
| 59 | template <> | ||
| 60 | struct GTOriDataJ<true> { | ||
| 61 | eigen::matrix3_t Jlog_from1; | ||
| 62 | }; | ||
| 63 | /// This class contains the data of the GenericTransformation class. | ||
| 64 | template <bool rel> | ||
| 65 | struct GTDataBase { | ||
| 66 | hpp::pinocchio::DeviceSync device; | ||
| 67 | const GenericTransformationModel<rel>& model; | ||
| 68 | 402060 | hpp::pinocchio::DeviceData& ddata() { return device.d(); } | |
| 69 | |||
| 70 | 64948 | const JointJacobian_t& J2() { return model.joint2->jacobian(ddata()); } | |
| 71 | 131136 | const Transform3s& M2() { | |
| 72 |
2/2✓ Branch 1 taken 131031 times.
✓ Branch 2 taken 108 times.
|
171166 | if (model.joint2) |
| 73 | 170955 | return model.joint2->currentTransformation(ddata()); | |
| 74 | else | ||
| 75 | 217 | return Id(); | |
| 76 | } | ||
| 77 | const vector3_t& t2() { return M2().translation(); } | ||
| 78 | 51358 | const matrix3_t& R2() { return M2().rotation(); } | |
| 79 | |||
| 80 | 35261 | const JointJacobian_t& J1() { | |
| 81 |
2/4✓ Branch 3 taken 35261 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 35262 times.
✗ Branch 7 not taken.
|
35261 | return model.getJoint1()->jacobian(this->ddata()); |
| 82 | } | ||
| 83 | 109643 | const Transform3s& M1() { | |
| 84 |
2/4✓ Branch 3 taken 109640 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 109643 times.
✗ Branch 7 not taken.
|
109643 | return model.getJoint1()->currentTransformation(this->ddata()); |
| 85 | } | ||
| 86 | 35252 | const matrix3_t& R1() { return M1().rotation(); } | |
| 87 | const vector3_t& t1() { return M1().translation(); } | ||
| 88 | |||
| 89 | 95039 | GTDataBase(const GenericTransformationModel<rel>& m, const DevicePtr_t& d) | |
| 90 | 95039 | : device(d), model(m) {} | |
| 91 | }; | ||
| 92 | template <bool rel, bool pos, bool ori, bool ose3> | ||
| 93 | struct GTDataV : GTDataBase<rel>, GTOriDataV<ori> { | ||
| 94 | enum { ValueSize = (pos ? 3 : 0) + (ori ? (ose3 ? 4 : 3) : 0) }; | ||
| 95 | typedef Eigen::Matrix<value_type, ValueSize, 1> ValueType; | ||
| 96 | ValueType value; | ||
| 97 | |||
| 98 | 95034 | GTDataV(const GenericTransformationModel<rel>& m, const DevicePtr_t& d) | |
| 99 |
2/4✓ Branch 2 taken 66851 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65309 times.
✗ Branch 6 not taken.
|
95034 | : GTDataBase<rel>(m, d) {} |
| 100 | }; | ||
| 101 | /// This class contains the data of the GenericTransformation class. | ||
| 102 | template <bool rel, bool pos, bool ori, bool ose3> | ||
| 103 | struct GTDataJ : GTDataV<rel, pos, ori, ose3>, GTOriDataJ<ori> { | ||
| 104 | enum { | ||
| 105 | ValueSize = GTDataV<rel, pos, ori, ose3>::ValueSize, | ||
| 106 | JacobianSize = (pos ? 3 : 0) + (ori ? 3 : 0), | ||
| 107 | RowPos = (pos ? 0 : -1), | ||
| 108 | RowOri = (ori ? (pos ? 3 : 0) : -1) | ||
| 109 | }; | ||
| 110 | typedef Eigen::Matrix<value_type, JacobianSize, Eigen::Dynamic> JacobianType; | ||
| 111 | typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic> matrix3x_t; | ||
| 112 | JacobianType jacobian; | ||
| 113 | matrix3x_t tmpJac; | ||
| 114 | eigen::vector3_t cross1, cross2; | ||
| 115 | |||
| 116 | 24689 | GTDataJ(const GenericTransformationModel<rel>& m, const DevicePtr_t& d) | |
| 117 |
5/10✓ Branch 2 taken 21105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21101 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 21101 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 21105 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20617 times.
✗ Branch 15 not taken.
|
24689 | : GTDataV<rel, pos, ori, ose3>(m, d) |
| 118 | // TODO the two following matrices should be of type Eigen::Map<...> | ||
| 119 | // and they should point to some buffer in m.device | ||
| 120 | // , jacobian (buffer1, NbRows, m.cols) | ||
| 121 | // , tmpJac (buffer2, 3, m.cols) | ||
| 122 | { | ||
| 123 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2031 times.
|
4062 | assert(!ose3 || (!ori || m.fullOri)); |
| 124 |
5/6✓ Branch 0 taken 20565 times.
✓ Branch 1 taken 540 times.
✓ Branch 2 taken 478 times.
✓ Branch 3 taken 20087 times.
✓ Branch 5 taken 1018 times.
✗ Branch 6 not taken.
|
24691 | if (!m.fullPos || !m.fullOri) jacobian.resize((int)JacobianSize, m.cols); |
| 125 |
1/2✓ Branch 1 taken 21104 times.
✗ Branch 2 not taken.
|
24691 | cross1.setZero(); |
| 126 |
3/4✓ Branch 0 taken 3282 times.
✓ Branch 1 taken 17822 times.
✓ Branch 3 taken 3282 times.
✗ Branch 4 not taken.
|
24689 | if (m.t2isZero) cross2.setZero(); |
| 127 | 24689 | } | |
| 128 | }; | ||
| 129 | |||
| 130 | /// ------- Compute log -------------------------------------------------- | ||
| 131 | /** Compute jacobian of function log of rotation matrix in SO(3) | ||
| 132 | |||
| 133 | Let us consider a matrix | ||
| 134 | \f$R=\exp \left[\mathbf{r}\right]_{\times}\in SO(3)\f$. | ||
| 135 | This functions computes the Jacobian of the function from | ||
| 136 | \f$SO(3)\f$ into \f$\mathbf{R}^3\f$ that maps \f$R\f$ to | ||
| 137 | \f$\mathbf{r}\f$. In other words, | ||
| 138 | \f{equation*} | ||
| 139 | \dot{\mathbf{r}} = J_{log}(R)\ \omega\,\,\,\mbox{with}\,\,\, | ||
| 140 | \dot {R} = \left[\omega\right]_{\times} R | ||
| 141 | \f} | ||
| 142 | \warning Two representations of the angular velocity \f$\omega\f$ are | ||
| 143 | possible: | ||
| 144 | \li \f$\dot{R} = \left[\omega\right]_{\times}R\f$ or | ||
| 145 | \li \f$\dot{R} = R\left[\omega\right]_{\times}\f$. | ||
| 146 | |||
| 147 | The expression below assumes the first representation is | ||
| 148 | used. | ||
| 149 | \param theta angle of rotation \f$R\f$, also \f$\|r\|\f$, | ||
| 150 | \param log 3d vector \f$\mathbf{r}\f$, | ||
| 151 | \retval Jlog matrix \f$J_{log} (R)\f$. | ||
| 152 | |||
| 153 | \f{align*} | ||
| 154 | J_{log} (R) &=& | ||
| 155 | \frac{\|\mathbf{r}\|\sin\|\mathbf{r}\|}{2(1-\cos\|\mathbf{r}\|)} I_3 - \frac | ||
| 156 | {1}{2}\left[\mathbf{r}\right]_{\times} + (\frac{1}{\|\mathbf{r}\|^2} - | ||
| 157 | \frac{\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|(1-\cos\|\mathbf{r}\|)}) | ||
| 158 | \mathbf{r}\mathbf{r}^T\\ | ||
| 159 | &=& I_3 -\frac{1}{2}\left[\mathbf{r}\right]_{\times} + | ||
| 160 | \left(\frac{2(1-\cos\|\mathbf{r}\|) - | ||
| 161 | \|\mathbf{r}\|\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|^2(1-\cos\|\mathbf{r}\|)}\right)\left[\mathbf{r}\right]_{\times}^2 | ||
| 162 | \f} */ | ||
| 163 | template <typename Derived> | ||
| 164 | 19650 | void computeJlog(const value_type& theta, const Eigen::MatrixBase<Derived>& log, | |
| 165 | matrix3_t& Jlog) { | ||
| 166 |
2/2✓ Branch 0 taken 754 times.
✓ Branch 1 taken 17830 times.
|
19650 | if (theta < 1e-6) |
| 167 | 754 | Jlog.setIdentity(); | |
| 168 | else { | ||
| 169 | // Jlog = alpha I | ||
| 170 | 18896 | const value_type ct = cos(theta), st = sin(theta); | |
| 171 | 18896 | const value_type st_1mct = st / (1 - ct); | |
| 172 | |||
| 173 |
1/2✓ Branch 1 taken 17830 times.
✗ Branch 2 not taken.
|
18896 | Jlog.setZero(); |
| 174 |
2/4✓ Branch 1 taken 17830 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17829 times.
✗ Branch 5 not taken.
|
18896 | Jlog.diagonal().setConstant(theta * st_1mct); |
| 175 | |||
| 176 | // Jlog += -r_{\times}/2 | ||
| 177 |
2/4✓ Branch 1 taken 17829 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17830 times.
✗ Branch 5 not taken.
|
18893 | Jlog(0, 1) = log(2); |
| 178 |
2/4✓ Branch 1 taken 17831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
|
18896 | Jlog(1, 0) = -log(2); |
| 179 |
2/4✓ Branch 1 taken 17831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
|
18899 | Jlog(0, 2) = -log(1); |
| 180 |
2/4✓ Branch 1 taken 17832 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
|
18899 | Jlog(2, 0) = log(1); |
| 181 |
2/4✓ Branch 1 taken 17832 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
|
18899 | Jlog(1, 2) = log(0); |
| 182 |
2/4✓ Branch 1 taken 17832 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
|
18899 | Jlog(2, 1) = -log(0); |
| 183 |
1/2✓ Branch 1 taken 17832 times.
✗ Branch 2 not taken.
|
18899 | Jlog /= 2; |
| 184 | |||
| 185 | 18899 | const value_type alpha = 1 / (theta * theta) - st_1mct / (2 * theta); | |
| 186 |
5/10✓ Branch 1 taken 17828 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17832 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17832 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17830 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17826 times.
✗ Branch 14 not taken.
|
18899 | Jlog.noalias() += alpha * log * log.transpose(); |
| 187 | } | ||
| 188 | 19643 | } | |
| 189 | |||
| 190 | typedef JointJacobian_t::ConstNRowsBlockXpr<3>::Type HalfJacobian_t; | ||
| 191 | 73638 | inline HalfJacobian_t omega(const JointJacobian_t& j) { | |
| 192 | 73638 | return j.bottomRows<3>(); | |
| 193 | } | ||
| 194 | 38229 | inline HalfJacobian_t trans(const JointJacobian_t& j) { return j.topRows<3>(); } | |
| 195 | |||
| 196 | 94 | static inline size_type size(std::vector<bool> mask) { | |
| 197 | 94 | size_type res = 0; | |
| 198 |
2/2✓ Branch 4 taken 399 times.
✓ Branch 5 taken 94 times.
|
493 | for (std::vector<bool>::iterator it = mask.begin(); it != mask.end(); ++it) { |
| 199 |
2/2✓ Branch 2 taken 394 times.
✓ Branch 3 taken 5 times.
|
399 | if (*it) ++res; |
| 200 | } | ||
| 201 | 94 | return res; | |
| 202 | } | ||
| 203 | |||
| 204 | template <bool flag /* false */> | ||
| 205 | struct unary { | ||
| 206 | template <bool rel, bool pos, bool ose3> | ||
| 207 | 3074 | static inline void log(GTDataV<rel, pos, flag, ose3>&) {} | |
| 208 | template <bool rel, bool pos, bool ose3> | ||
| 209 | 976 | static inline void Jlog(GTDataJ<rel, pos, flag, ose3>&) {} | |
| 210 | }; | ||
| 211 | template <> | ||
| 212 | struct unary<true> { | ||
| 213 | template <bool rel, bool pos, bool ose3> | ||
| 214 | 92058 | static inline void log(GTDataV<rel, pos, true, ose3>& d) { | |
| 215 | if (ose3) { | ||
| 216 |
2/4✓ Branch 2 taken 21572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21572 times.
✗ Branch 6 not taken.
|
43144 | matrixToQuat(d.M.rotation(), d.value.template tail<4>()); |
| 217 | } else { | ||
| 218 |
2/4✓ Branch 2 taken 43740 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 43738 times.
✗ Branch 6 not taken.
|
48914 | logSO3(d.M.rotation(), d.theta, d.value.template tail<3>()); |
| 219 | hppDnum(info, "theta=" << d.theta); | ||
| 220 | } | ||
| 221 | 92063 | } | |
| 222 | template <bool rel, bool pos, bool ose3> | ||
| 223 | 23707 | static inline void Jlog(GTDataJ<rel, pos, true, ose3>& d) { | |
| 224 | if (ose3) { | ||
| 225 |
4/8✓ Branch 3 taken 2031 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2031 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2031 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2031 times.
✗ Branch 13 not taken.
|
4062 | d.Jlog_from1 = d.model.F2inJ2.rotation().transpose() * d.R2().transpose(); |
| 226 |
2/2✓ Branch 3 taken 10 times.
✓ Branch 4 taken 5 times.
|
30 | if (rel && d.model.getJoint1()) d.Jlog_from1 *= d.R1(); |
| 227 | } else { | ||
| 228 |
1/2✓ Branch 2 taken 18580 times.
✗ Branch 3 not taken.
|
19645 | computeJlog(d.theta, d.value.template tail<3>(), d.Jlog_from1); |
| 229 | hppDnum(info, "Jlog_: " << d.Jlog_from1); | ||
| 230 |
2/2✓ Branch 0 taken 18268 times.
✓ Branch 1 taken 312 times.
|
19643 | if (!d.model.R1isID) |
| 231 |
1/2✓ Branch 3 taken 18270 times.
✗ Branch 4 not taken.
|
19019 | d.Jlog_from1 *= d.model.F1inJ1.rotation().transpose(); |
| 232 | } | ||
| 233 | 23708 | } | |
| 234 | }; | ||
| 235 | |||
| 236 | template <bool ori, typename Data, typename Derived> | ||
| 237 | 82374 | void assign_if(bool cond, Data& d, matrixOut_t J, | |
| 238 | const Eigen::MatrixBase<Derived>& rhs, | ||
| 239 | const size_type& startRow) { | ||
| 240 | 82374 | const int& rowCache = (ori ? Data::RowOri : Data::RowPos); | |
| 241 |
2/2✓ Branch 0 taken 25 times.
✓ Branch 1 taken 41162 times.
|
82374 | if (cond) |
| 242 |
3/6✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
|
50 | d.jacobian.template middleRows<3>(rowCache).noalias() = rhs; |
| 243 | else | ||
| 244 |
4/8✓ Branch 1 taken 41165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41167 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 41163 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 41159 times.
✗ Branch 11 not taken.
|
82324 | J.template middleRows<3>(startRow).leftCols(d.model.cols).noalias() = rhs; |
| 245 | } | ||
| 246 | |||
| 247 | /// ------- Compute jacobian --------------------------------------------- | ||
| 248 | template <bool lflag /*rel*/, bool rflag /*false*/> | ||
| 249 | struct binary { | ||
| 250 | // the first template allow us to consider relative transformation as | ||
| 251 | // absolute when joint1 is NULL, at run time | ||
| 252 | template <bool rel, bool pos, bool ose3> | ||
| 253 | 976 | static inline void Jorientation(GTDataJ<rel, pos, rflag, ose3>&, | |
| 254 | 976 | matrixOut_t) {} | |
| 255 | template <bool rel, bool ori, bool ose3> | ||
| 256 | 1060 | static inline void Jtranslation(GTDataJ<rel, rflag, ori, ose3>&, | |
| 257 | 1060 | matrixOut_t) {} | |
| 258 | }; | ||
| 259 | template <> | ||
| 260 | struct binary<false, true> // Absolute | ||
| 261 | { | ||
| 262 | template <bool rel, bool pos, bool ose3> | ||
| 263 | 5747 | static inline void Jorientation(GTDataJ<rel, pos, true, ose3>& d, | |
| 264 | matrixOut_t J) { | ||
| 265 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2021 times.
|
4042 | assert(!ose3 || d.model.fullOri); |
| 266 |
2/4✓ Branch 1 taken 3031 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3028 times.
✗ Branch 5 not taken.
|
5747 | assign_if<true>(!(ose3 || d.model.fullOri), d, J, |
| 267 |
3/6✓ Branch 3 taken 3031 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3029 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3031 times.
✗ Branch 10 not taken.
|
5747 | (d.Jlog_from1 * d.R2()) * omega(d.J2()), d.model.rowOri); |
| 268 | 5743 | } | |
| 269 | template <bool rel, bool ori, bool ose3> | ||
| 270 | 5483 | static inline void Jtranslation(GTDataJ<rel, true, ori, ose3>& d, | |
| 271 | matrixOut_t J) { | ||
| 272 | 5483 | const JointJacobian_t& J2(d.J2()); | |
| 273 | 5481 | const matrix3_t& R2(d.R2()); | |
| 274 | 5483 | const matrix3_t& R1inJ1(d.model.F1inJ1.rotation()); | |
| 275 | |||
| 276 | // hpp-model: J = 1RT* ( 0Jt2 - [ 0R2 2t* ]x 0Jw2 ) | ||
| 277 | // pinocchio: J = 1RT* ( 0R2 2Jt2 - [ 0R2 2t* ]x 0R2 2Jw2 ) | ||
| 278 |
2/2✓ Branch 0 taken 331 times.
✓ Branch 1 taken 2566 times.
|
5480 | if (!d.model.t2isZero) { |
| 279 |
5/10✓ Branch 2 taken 332 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 331 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 331 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 330 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 331 times.
✗ Branch 15 not taken.
|
452 | d.tmpJac.noalias() = (R2.colwise().cross(d.cross2)) * omega(J2); |
| 280 |
3/6✓ Branch 2 taken 332 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 332 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 331 times.
✗ Branch 9 not taken.
|
453 | d.tmpJac.noalias() += R2 * trans(J2); |
| 281 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 331 times.
|
453 | if (d.model.R1isID) { |
| 282 | ✗ | assign_if<false>(!d.model.fullPos, d, J, d.tmpJac, 0); | |
| 283 | } else { // Generic case | ||
| 284 |
3/6✓ Branch 1 taken 332 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 332 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 332 times.
✗ Branch 8 not taken.
|
454 | assign_if<false>(!d.model.fullPos, d, J, R1inJ1.transpose() * d.tmpJac, |
| 285 |
1/2✓ Branch 1 taken 332 times.
✗ Branch 2 not taken.
|
907 | 0); |
| 286 | } | ||
| 287 | } else { | ||
| 288 |
2/2✓ Branch 0 taken 2311 times.
✓ Branch 1 taken 255 times.
|
5028 | if (d.model.R1isID) |
| 289 |
4/8✓ Branch 1 taken 2311 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2311 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2311 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2311 times.
✗ Branch 11 not taken.
|
4622 | assign_if<false>(!d.model.fullPos, d, J, R2 * trans(J2), 0); |
| 290 | else | ||
| 291 |
2/4✓ Branch 1 taken 254 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 255 times.
✗ Branch 5 not taken.
|
403 | assign_if<false>(!d.model.fullPos, d, J, |
| 292 |
4/8✓ Branch 1 taken 254 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 255 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 256 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 254 times.
✗ Branch 11 not taken.
|
811 | (R1inJ1.transpose() * R2) * trans(J2), 0); |
| 293 | } | ||
| 294 | 5481 | } | |
| 295 | }; | ||
| 296 | template <> | ||
| 297 | struct binary<true, true> // Relative | ||
| 298 | { | ||
| 299 | template <bool pos, bool ose3> | ||
| 300 | 17966 | static inline void Jorientation(GTDataJ<true, pos, true, ose3>& d, | |
| 301 | matrixOut_t J) { | ||
| 302 |
3/6✓ Branch 3 taken 17584 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17584 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17582 times.
✗ Branch 10 not taken.
|
17966 | d.tmpJac.noalias() = -omega(d.J1()); |
| 303 |
2/2✓ Branch 1 taken 17562 times.
✓ Branch 2 taken 20 times.
|
17960 | if (d.model.joint2) |
| 304 |
7/14✓ Branch 3 taken 17566 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17566 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17564 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 17566 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 17566 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 17564 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 17563 times.
✗ Branch 22 not taken.
|
17920 | d.tmpJac.noalias() += (d.R1().transpose() * d.R2()) * omega(d.J2()); |
| 305 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
20 | assert(!ose3 || d.model.fullOri); |
| 306 |
2/4✓ Branch 1 taken 17585 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17582 times.
✗ Branch 5 not taken.
|
17964 | assign_if<true>(!(ose3 || d.model.fullOri), d, J, d.Jlog_from1 * d.tmpJac, |
| 307 | 17962 | d.model.rowOri); | |
| 308 | 17960 | } | |
| 309 | template <bool ori, bool ose3> | ||
| 310 | 18148 | static inline void Jtranslation(GTDataJ<true, true, ori, ose3>& d, | |
| 311 | matrixOut_t J) { | ||
| 312 | 18148 | const JointJacobian_t& J1(d.J1()); | |
| 313 | 18148 | const matrix3_t& R1(d.R1()); | |
| 314 | 18148 | const matrix3_t& R2(d.R2()); | |
| 315 | 18148 | const matrix3_t& R1inJ1(d.model.F1inJ1.rotation()); | |
| 316 | |||
| 317 | // J = 1RT* 0RT1 ( A + B ) | ||
| 318 | // hpp-model: | ||
| 319 | // A = [ 0t2 - 0t1 0R2 2t* ]x 0Jw1 | ||
| 320 | // B = ( 0Jt2 - 0Jt1 - [ 0R2 2t* ]x 0Jw2 ) | ||
| 321 | // pinocchio: | ||
| 322 | // A = [ 0t2 - 0t1 0R2 2t* ]x 0R1 1Jw1 | ||
| 323 | // B = ( 0R2 2Jt2 - 0R1 1Jt1 - [ 0R2 2t* ]x 0R2 2Jw2 ) | ||
| 324 |
5/10✓ Branch 1 taken 17675 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17675 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17676 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17675 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17676 times.
✗ Branch 14 not taken.
|
18146 | d.tmpJac.noalias() = |
| 325 |
3/6✓ Branch 2 taken 17676 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17676 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 17675 times.
✗ Branch 9 not taken.
|
18148 | (-R1.transpose() * R1.colwise().cross(d.cross1)) * omega(J1); // A |
| 326 |
2/2✓ Branch 1 taken 17656 times.
✓ Branch 2 taken 20 times.
|
18148 | if (d.model.joint2) |
| 327 |
5/10✓ Branch 3 taken 17656 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17656 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17656 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 17656 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 17656 times.
✗ Branch 16 not taken.
|
18108 | d.tmpJac.noalias() += (R1.transpose() * R2) * trans(d.J2()); // B1 |
| 328 |
2/4✓ Branch 2 taken 17676 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17676 times.
✗ Branch 6 not taken.
|
18148 | d.tmpJac.noalias() -= trans(J1); // B2 |
| 329 |
6/6✓ Branch 0 taken 17468 times.
✓ Branch 1 taken 208 times.
✓ Branch 3 taken 17448 times.
✓ Branch 4 taken 20 times.
✓ Branch 5 taken 17448 times.
✓ Branch 6 taken 228 times.
|
18148 | if (!d.model.t2isZero && d.model.joint2) |
| 330 |
2/4✓ Branch 1 taken 17448 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17448 times.
✗ Branch 5 not taken.
|
17692 | d.tmpJac.noalias() += |
| 331 |
5/10✓ Branch 3 taken 17448 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17448 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 17448 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 17448 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 17448 times.
✗ Branch 16 not taken.
|
35384 | R1.transpose() * R2.colwise().cross(d.cross2) * omega(d.J2()); // B3 |
| 332 |
2/2✓ Branch 0 taken 208 times.
✓ Branch 1 taken 17468 times.
|
18148 | if (d.model.R1isID) |
| 333 |
2/4✓ Branch 1 taken 208 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 208 times.
✗ Branch 5 not taken.
|
416 | assign_if<false>(!d.model.fullPos, d, J, d.tmpJac, 0); |
| 334 | else | ||
| 335 |
3/6✓ Branch 1 taken 17468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17468 times.
✗ Branch 8 not taken.
|
17732 | assign_if<false>(!d.model.fullPos, d, J, R1inJ1.transpose() * d.tmpJac, |
| 336 |
1/2✓ Branch 1 taken 17468 times.
✗ Branch 2 not taken.
|
35464 | 0); |
| 337 | 18148 | } | |
| 338 | }; | ||
| 339 | |||
| 340 | /// ------- Compute relative transform ----------------------------------- | ||
| 341 | template <bool compileTimeRel /* false */, bool ori /* false */> | ||
| 342 | struct relativeTransform { | ||
| 343 | template <bool runtimeRel, bool pos, bool ose3> | ||
| 344 | 1768 | static inline void run(GTDataV<runtimeRel, pos, false, ose3>& d) { | |
| 345 | using hpp::pinocchio::LiegroupElement; | ||
| 346 | using hpp::pinocchio::LiegroupSpace; | ||
| 347 | // There is no joint1 | ||
| 348 | 1768 | const Transform3s& M2 = d.M2(); | |
| 349 |
2/4✓ Branch 3 taken 882 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 882 times.
✗ Branch 7 not taken.
|
1768 | d.value.noalias() = M2.act(d.model.F2inJ2.translation()); |
| 350 | #ifndef NDEBUG | ||
| 351 | if (ose3) { | ||
| 352 | hpp::pinocchio::vector4_t quat; | ||
| 353 | size_type n = d.value.size(); | ||
| 354 | assert(n >= 4); | ||
| 355 | quat << d.value[n - 4], d.value[n - 3], d.value[n - 2], d.value[n - 1]; | ||
| 356 | assert(hpp::pinocchio::checkNormalized( | ||
| 357 | LiegroupElement(quat, LiegroupSpace::SO3()))); | ||
| 358 | } | ||
| 359 | #endif | ||
| 360 |
3/4✓ Branch 0 taken 517 times.
✓ Branch 1 taken 365 times.
✓ Branch 5 taken 517 times.
✗ Branch 6 not taken.
|
1764 | if (!d.model.t1isZero) d.value.noalias() -= d.model.F1inJ1.translation(); |
| 361 | #ifndef NDEBUG | ||
| 362 | if (ose3) { | ||
| 363 | hpp::pinocchio::vector4_t quat; | ||
| 364 | size_type n = d.value.size(); | ||
| 365 | assert(n >= 4); | ||
| 366 | quat << d.value[n - 4], d.value[n - 3], d.value[n - 2], d.value[n - 1]; | ||
| 367 | assert(hpp::pinocchio::checkNormalized( | ||
| 368 | LiegroupElement(quat, LiegroupSpace::SO3()))); | ||
| 369 | } | ||
| 370 | #endif | ||
| 371 |
2/2✓ Branch 0 taken 517 times.
✓ Branch 1 taken 365 times.
|
1764 | if (!d.model.R1isID) |
| 372 |
1/2✓ Branch 3 taken 518 times.
✗ Branch 4 not taken.
|
1034 | d.value.applyOnTheLeft(d.model.F1inJ1.rotation().transpose()); |
| 373 | #ifndef NDEBUG | ||
| 374 | if (ose3) { | ||
| 375 | hpp::pinocchio::vector4_t quat; | ||
| 376 | size_type n = d.value.size(); | ||
| 377 | assert(n >= 4); | ||
| 378 | quat << d.value[n - 4], d.value[n - 3], d.value[n - 2], d.value[n - 1]; | ||
| 379 | assert(hpp::pinocchio::checkNormalized( | ||
| 380 | LiegroupElement(quat, LiegroupSpace::SO3()))); | ||
| 381 | } | ||
| 382 | #endif | ||
| 383 | } | ||
| 384 | }; | ||
| 385 | template <> | ||
| 386 | struct relativeTransform<false, true> { | ||
| 387 | template <bool runtimeRel, bool pos, bool ose3> | ||
| 388 | 17827 | static inline void run(GTDataV<runtimeRel, pos, true, ose3>& d) { | |
| 389 | 17827 | const Transform3s& M2 = d.M2(); | |
| 390 |
2/4✓ Branch 2 taken 9384 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9390 times.
✗ Branch 6 not taken.
|
17835 | d.M = d.model.F1inJ1.actInv(M2 * d.model.F2inJ2); |
| 391 |
2/4✓ Branch 3 taken 8010 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8014 times.
✗ Branch 7 not taken.
|
15087 | if (pos) d.value.template head<3>().noalias() = d.M.translation(); |
| 392 | 17831 | } | |
| 393 | }; | ||
| 394 | template <> | ||
| 395 | struct relativeTransform<true, true> { | ||
| 396 | template <bool pos, bool ose3> | ||
| 397 | 79647 | static inline void run(GTDataV<true, pos, true, ose3>& d) { | |
| 398 |
2/2✓ Branch 1 taken 3174 times.
✓ Branch 2 taken 55923 times.
|
79647 | if (d.model.joint1 == NULL) { |
| 399 | // runtime absolute reference. | ||
| 400 | 5409 | relativeTransform<false, true>::run(d); | |
| 401 | 5412 | return; | |
| 402 | } | ||
| 403 | 74234 | const Transform3s& M1 = d.M1(); | |
| 404 | 74232 | const Transform3s& M2 = d.M2(); | |
| 405 |
3/6✓ Branch 2 taken 55922 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 55920 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 55924 times.
✗ Branch 9 not taken.
|
74236 | d.M = d.model.F1inJ1.actInv(M1.actInv(M2 * d.model.F2inJ2)); |
| 406 |
2/4✓ Branch 3 taken 55199 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 55200 times.
✗ Branch 7 not taken.
|
72788 | if (pos) d.value.template head<3>().noalias() = d.M.translation(); |
| 407 | } | ||
| 408 | }; | ||
| 409 | template <> | ||
| 410 | struct relativeTransform<true, false> { | ||
| 411 | template <bool pos, bool ose3> | ||
| 412 | 654 | static inline void run(GTDataV<true, pos, false, ose3>& d) { | |
| 413 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 652 times.
|
654 | if (d.model.joint1 == NULL) { |
| 414 | // runtime absolute reference. | ||
| 415 | ✗ | relativeTransform<false, false>::run(d); | |
| 416 | ✗ | return; | |
| 417 | } | ||
| 418 | 652 | const Transform3s& M2 = d.M2(); | |
| 419 | 653 | const Transform3s& M1 = d.M1(); | |
| 420 |
3/6✓ Branch 4 taken 654 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 654 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 652 times.
✗ Branch 11 not taken.
|
654 | d.value.noalias() = M2.act(d.model.F2inJ2.translation()) - M1.translation(); |
| 421 |
1/2✓ Branch 3 taken 653 times.
✗ Branch 4 not taken.
|
652 | d.value.applyOnTheLeft(M1.rotation().transpose()); |
| 422 | |||
| 423 |
3/4✓ Branch 0 taken 453 times.
✓ Branch 1 taken 200 times.
✓ Branch 5 taken 453 times.
✗ Branch 6 not taken.
|
653 | if (!d.model.t1isZero) d.value.noalias() -= d.model.F1inJ1.translation(); |
| 424 |
2/2✓ Branch 0 taken 453 times.
✓ Branch 1 taken 200 times.
|
653 | if (!d.model.R1isID) |
| 425 |
1/2✓ Branch 3 taken 454 times.
✗ Branch 4 not taken.
|
453 | d.value.applyOnTheLeft(d.model.F1inJ1.rotation().transpose()); |
| 426 | } | ||
| 427 | }; | ||
| 428 | |||
| 429 | template <bool rel, bool pos, bool ori, bool ose3> | ||
| 430 | struct compute { | ||
| 431 | 95141 | static inline void error(GTDataV<rel, pos, ori, ose3>& d) { | |
| 432 | using hpp::pinocchio::LiegroupElement; | ||
| 433 | using hpp::pinocchio::LiegroupSpace; | ||
| 434 | 95141 | relativeTransform<rel, ori>::run(d); | |
| 435 | 95133 | unary<ori>::log(d); | |
| 436 | #ifndef NDEBUG | ||
| 437 | if (ose3) { | ||
| 438 |
1/2✓ Branch 1 taken 21572 times.
✗ Branch 2 not taken.
|
43144 | hpp::pinocchio::vector4_t quat; |
| 439 | 43144 | size_type n = d.value.size(); | |
| 440 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 21572 times.
|
43144 | assert(n >= 4); |
| 441 |
8/16✓ Branch 1 taken 21572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21572 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21572 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21572 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 21572 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 21572 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 21572 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 21572 times.
✗ Branch 23 not taken.
|
43144 | quat << d.value[n - 4], d.value[n - 3], d.value[n - 2], d.value[n - 1]; |
| 442 |
5/10✓ Branch 1 taken 21572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21572 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21572 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21572 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 21572 times.
|
43144 | assert(hpp::pinocchio::checkNormalized( |
| 443 | LiegroupElement(quat, LiegroupSpace::SO3()))); | ||
| 444 | } | ||
| 445 | #endif | ||
| 446 | 95135 | } | |
| 447 | |||
| 448 | 24687 | static inline void jacobian(GTDataJ<rel, pos, ori, ose3>& d, | |
| 449 | matrixOut_t jacobian, | ||
| 450 | const std::vector<bool>& mask) { | ||
| 451 | 24687 | const Transform3s& M2 = d.M2(); | |
| 452 | 24689 | const vector3_t& t2inJ2(d.model.F2inJ2.translation()); | |
| 453 | 24689 | const vector3_t& t2(M2.translation()); | |
| 454 | 24691 | const matrix3_t& R2(M2.rotation()); | |
| 455 | |||
| 456 |
4/6✓ Branch 0 taken 17825 times.
✓ Branch 1 taken 3279 times.
✓ Branch 4 taken 17825 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17824 times.
✗ Branch 8 not taken.
|
24689 | if (!d.model.t2isZero) d.cross2.noalias() = R2 * t2inJ2; |
| 457 | |||
| 458 | 24688 | unary<ori>::Jlog(d); | |
| 459 | |||
| 460 | // rel: relative known at compile time | ||
| 461 | // d.getJoint1(): relative known at run time | ||
| 462 |
2/2✓ Branch 3 taken 17817 times.
✓ Branch 4 taken 325 times.
|
18764 | if (rel && d.model.getJoint1()) { |
| 463 | 18430 | const Transform3s& M1 = d.M1(); | |
| 464 | 18432 | const vector3_t& t1(M1.translation()); | |
| 465 |
3/6✓ Branch 2 taken 17818 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17817 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 17816 times.
✗ Branch 9 not taken.
|
18430 | d.cross1.noalias() = d.cross2 + t2 - t1; |
| 466 |
1/2✓ Branch 2 taken 17676 times.
✗ Branch 3 not taken.
|
18428 | binary<rel, pos>::Jtranslation(d, jacobian); |
| 467 |
1/2✓ Branch 2 taken 17582 times.
✗ Branch 3 not taken.
|
18430 | binary<rel, ori>::Jorientation(d, jacobian); |
| 468 | } else { | ||
| 469 |
2/4✓ Branch 2 taken 3286 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3286 times.
✗ Branch 6 not taken.
|
6259 | d.cross1.noalias() = d.cross2 + t2; |
| 470 |
1/2✓ Branch 2 taken 2897 times.
✗ Branch 3 not taken.
|
6257 | binary<false, pos>::Jtranslation(d, jacobian); |
| 471 |
1/2✓ Branch 2 taken 3028 times.
✗ Branch 3 not taken.
|
6256 | binary<false, ori>::Jorientation(d, jacobian); |
| 472 | } | ||
| 473 | |||
| 474 | // Copy necessary rows. | ||
| 475 | 24679 | size_type index = 0; | |
| 476 | 24679 | const size_type lPos = (pos ? 3 : 0), lOri = (ori ? 3 : 0); | |
| 477 |
2/2✓ Branch 0 taken 535 times.
✓ Branch 1 taken 20563 times.
|
24679 | if (!d.model.fullPos) { |
| 478 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 535 times.
|
1130 | for (size_type i = 0; i < lPos; ++i) { |
| 479 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 10 times.
|
60 | if (mask[i]) { |
| 480 |
4/8✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
|
40 | jacobian.row(index).leftCols(d.model.cols).noalias() = |
| 481 | 40 | d.jacobian.row(i); | |
| 482 | 40 | ++index; | |
| 483 | } | ||
| 484 | } | ||
| 485 | } else | ||
| 486 | 23609 | index = lPos; | |
| 487 |
2/2✓ Branch 0 taken 503 times.
✓ Branch 1 taken 20595 times.
|
24679 | if (!d.model.fullOri) { |
| 488 |
2/2✓ Branch 0 taken 45 times.
✓ Branch 1 taken 503 times.
|
1096 | for (size_type i = lPos; i < lPos + lOri; ++i) { |
| 489 |
2/2✓ Branch 1 taken 30 times.
✓ Branch 2 taken 15 times.
|
90 | if (mask[i]) { |
| 490 |
4/8✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
|
60 | jacobian.row(index).leftCols(d.model.cols).noalias() = |
| 491 | 60 | d.jacobian.row(i); | |
| 492 | 60 | ++index; | |
| 493 | } | ||
| 494 | } | ||
| 495 | } | ||
| 496 |
1/2✓ Branch 3 taken 21101 times.
✗ Branch 4 not taken.
|
24679 | jacobian.rightCols(jacobian.cols() - d.model.cols).setZero(); |
| 497 | 24683 | } | |
| 498 | }; | ||
| 499 | |||
| 500 | 246 | void setActiveParameters(const DevicePtr_t& robot, const JointConstPtr_t& j1, | |
| 501 | const JointConstPtr_t& j2, ArrayXb& activeParameters, | ||
| 502 | ArrayXb& activeDerivativeParameters) { | ||
| 503 | typedef ::pinocchio::JointIndex JointIndex; | ||
| 504 | |||
| 505 | 246 | const pinocchio::Model& model = robot->model(); | |
| 506 |
4/4✓ Branch 1 taken 121 times.
✓ Branch 2 taken 125 times.
✓ Branch 6 taken 140 times.
✓ Branch 7 taken 106 times.
|
246 | const JointIndex id1 = (j1 ? j1->index() : 0), id2 = (j2 ? j2->index() : 0); |
| 507 | 246 | JointIndex i1 = id1, i2 = id2; | |
| 508 | |||
| 509 | 246 | std::vector<JointIndex> from1, from2; | |
| 510 |
2/2✓ Branch 0 taken 1245 times.
✓ Branch 1 taken 246 times.
|
1491 | while (i1 != i2) { |
| 511 | JointIndex i; | ||
| 512 |
2/2✓ Branch 0 taken 511 times.
✓ Branch 1 taken 734 times.
|
1245 | if (i1 > i2) { |
| 513 | 511 | i = i1; | |
| 514 | 511 | i1 = model.parents[i1]; | |
| 515 | } else /* if (i1 < i2) */ { | ||
| 516 | 734 | i = i2; | |
| 517 | 734 | i2 = model.parents[i2]; | |
| 518 | } | ||
| 519 |
1/2✓ Branch 0 taken 1245 times.
✗ Branch 1 not taken.
|
1245 | if (i > 0) { |
| 520 |
3/6✓ Branch 2 taken 1245 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1245 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1245 times.
✗ Branch 10 not taken.
|
1245 | activeParameters.segment(model.joints[i].idx_q(), model.joints[i].nq()) |
| 521 |
1/2✓ Branch 1 taken 1245 times.
✗ Branch 2 not taken.
|
1245 | .setConstant(true); |
| 522 | activeDerivativeParameters | ||
| 523 |
3/6✓ Branch 2 taken 1245 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1245 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1245 times.
✗ Branch 10 not taken.
|
1245 | .segment(model.joints[i].idx_v(), model.joints[i].nv()) |
| 524 |
1/2✓ Branch 1 taken 1245 times.
✗ Branch 2 not taken.
|
1245 | .setConstant(true); |
| 525 | } | ||
| 526 | } | ||
| 527 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 246 times.
|
246 | assert(i1 == i2); |
| 528 | 246 | } | |
| 529 | } // namespace | ||
| 530 | } // namespace constraints | ||
| 531 | } // namespace hpp | ||
| 532 | |||
| 533 | #endif // SRC_GENERIC_TRANSFORMATION_HELPER_HH | ||
| 534 |