| Directory: | ./ |
|---|---|
| File: | include/hpp/constraints/tools.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 106 | 107 | 99.1% |
| Branches: | 116 | 208 | 55.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2014, LAAS-CNRS | ||
| 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 HPP_CONSTRAINTS_TOOL_HH | ||
| 30 | #define HPP_CONSTRAINTS_TOOL_HH | ||
| 31 | |||
| 32 | #include <boost/math/constants/constants.hpp> | ||
| 33 | #include <hpp/constraints/fwd.hh> | ||
| 34 | #include <pinocchio/spatial/se3.hpp> | ||
| 35 | |||
| 36 | namespace hpp { | ||
| 37 | namespace constraints { | ||
| 38 | |||
| 39 | template <typename VectorType, typename MatrixType> | ||
| 40 | 8450 | static void computeCrossMatrix(const VectorType& v, MatrixType& m) { | |
| 41 |
1/2✓ Branch 2 taken 4675 times.
✗ Branch 3 not taken.
|
8450 | m.diagonal().setZero(); |
| 42 | 8450 | m(0, 1) = -v[2]; | |
| 43 | 8450 | m(1, 0) = v[2]; | |
| 44 | 8450 | m(0, 2) = v[1]; | |
| 45 | 8450 | m(2, 0) = -v[1]; | |
| 46 | 8450 | m(1, 2) = -v[0]; | |
| 47 | 8450 | m(2, 1) = v[0]; | |
| 48 | 8450 | } | |
| 49 | |||
| 50 | /// \addtogroup hpp_constraints_tools | ||
| 51 | /// \{ | ||
| 52 | |||
| 53 | /// Compute log of rotation matrix as a 3d vector | ||
| 54 | /// | ||
| 55 | /// \param R rotation matrix in SO(3), | ||
| 56 | /// \retval theta angle of rotation, | ||
| 57 | /// \retval result 3d vector \f$\mathbf{r}\f$ such that | ||
| 58 | /// \f$R=\exp [\mathbf{r}]_{\times}\f$ | ||
| 59 | template <typename Derived> | ||
| 60 | 88030 | inline void logSO3(const matrix3_t& R, value_type& theta, | |
| 61 | Eigen::MatrixBase<Derived> const& result) { | ||
| 62 | 88030 | Eigen::MatrixBase<Derived>& value = | |
| 63 | const_cast<Eigen::MatrixBase<Derived>&>(result); | ||
| 64 | 88030 | const value_type PI = ::boost::math::constants::pi<value_type>(); | |
| 65 | 88030 | const value_type tr = R.trace(); | |
| 66 |
2/2✓ Branch 0 taken 28 times.
✓ Branch 1 taken 45031 times.
|
88032 | if (tr > 3) |
| 67 | 55 | theta = 0; // acos((3-1)/2) | |
| 68 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 45031 times.
|
87977 | else if (tr < -1) |
| 69 | ✗ | theta = PI; // acos((-1-1)/2) | |
| 70 | else | ||
| 71 | 87977 | theta = acos((tr - 1) / 2); | |
| 72 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 45059 times.
|
88032 | assert(theta == theta); |
| 73 | // From runs of tests/logarithm.cc: 1e-6 is too small. | ||
| 74 |
2/2✓ Branch 0 taken 43916 times.
✓ Branch 1 taken 1143 times.
|
88032 | if (theta < PI - 1e-2) { |
| 75 |
2/2✓ Branch 0 taken 39499 times.
✓ Branch 1 taken 4417 times.
|
85757 | const value_type t = ((theta > 1e-6) ? theta / sin(theta) : 1) / 2; |
| 76 | 85757 | value(0) = t * (R(2, 1) - R(1, 2)); | |
| 77 | 85767 | value(1) = t * (R(0, 2) - R(2, 0)); | |
| 78 | 85761 | value(2) = t * (R(1, 0) - R(0, 1)); | |
| 79 | } else { | ||
| 80 | // 1e-2: A low value is not required since the computation is | ||
| 81 | // using explicit formula. However, the precision of this method | ||
| 82 | // is the square root of the precision with the antisymmetric | ||
| 83 | // method (Nominal case). | ||
| 84 | 2275 | const value_type cphi = cos(theta - PI); | |
| 85 | 2275 | const value_type beta = theta * theta / (1 + cphi); | |
| 86 | 2275 | const value_type tmp0 = (R(0, 0) + cphi) * beta; | |
| 87 | 2265 | const value_type tmp1 = (R(1, 1) + cphi) * beta; | |
| 88 | 2265 | const value_type tmp2 = (R(2, 2) + cphi) * beta; | |
| 89 |
4/4✓ Branch 2 taken 568 times.
✓ Branch 3 taken 570 times.
✓ Branch 4 taken 1135 times.
✓ Branch 5 taken 3 times.
|
2265 | value(0) = (R(2, 1) > R(1, 2) ? 1 : -1) * (tmp0 > 0 ? sqrt(tmp0) : 0); |
| 90 |
4/4✓ Branch 2 taken 516 times.
✓ Branch 3 taken 622 times.
✓ Branch 4 taken 1135 times.
✓ Branch 5 taken 3 times.
|
2265 | value(1) = (R(0, 2) > R(2, 0) ? 1 : -1) * (tmp1 > 0 ? sqrt(tmp1) : 0); |
| 91 |
4/4✓ Branch 2 taken 608 times.
✓ Branch 3 taken 530 times.
✓ Branch 4 taken 1135 times.
✓ Branch 5 taken 3 times.
|
2265 | value(2) = (R(1, 0) > R(0, 1) ? 1 : -1) * (tmp2 > 0 ? sqrt(tmp2) : 0); |
| 92 | } | ||
| 93 | 88032 | } | |
| 94 | |||
| 95 | /** Compute jacobian of function log of rotation matrix in SO(3) | ||
| 96 | |||
| 97 | Let us consider a matrix | ||
| 98 | \f$R=\exp \left[\mathbf{r}\right]_{\times}\in SO(3)\f$. | ||
| 99 | This functions computes the Jacobian of the function from | ||
| 100 | \f$SO(3)\f$ into \f$\mathbf{R}^3\f$ that maps \f$R\f$ to | ||
| 101 | \f$\mathbf{r}\f$. In other words, | ||
| 102 | \f{equation*} | ||
| 103 | \dot{\mathbf{r}} = J_{log}(R)\ \omega\,\,\,\mbox{with}\,\,\, | ||
| 104 | \dot {R} = \left[\omega\right]_{\times} R | ||
| 105 | \f} | ||
| 106 | \warning Two representations of the angular velocity \f$\omega\f$ are | ||
| 107 | possible: | ||
| 108 | \li \f$\dot{R} = \left[\omega\right]_{\times}R\f$ or | ||
| 109 | \li \f$\dot{R} = R\left[\omega\right]_{\times}\f$. | ||
| 110 | |||
| 111 | The expression below assumes the second representation is used. | ||
| 112 | \param theta angle of rotation \f$R\f$, also \f$\|r\|\f$, | ||
| 113 | \param log 3d vector \f$\mathbf{r}\f$, | ||
| 114 | \retval Jlog matrix \f$J_{log} (R)\f$. | ||
| 115 | |||
| 116 | \f{align*} | ||
| 117 | J_{log} (R) &=& | ||
| 118 | \frac{\|\mathbf{r}\|\sin\|\mathbf{r}\|}{2(1-\cos\|\mathbf{r}\|)} I_3 - \frac | ||
| 119 | {1}{2}\left[\mathbf{r}\right]_{\times} + (\frac{1}{\|\mathbf{r}\|^2} - | ||
| 120 | \frac{\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|(1-\cos\|\mathbf{r}\|)}) | ||
| 121 | \mathbf{r}\mathbf{r}^T\\ | ||
| 122 | &=& I_3 +\frac{1}{2}\left[\mathbf{r}\right]_{\times} + | ||
| 123 | \left(\frac{2(1-\cos\|\mathbf{r}\|) - | ||
| 124 | \|\mathbf{r}\|\sin\|\mathbf{r}\|}{2\|\mathbf{r}\|^2(1-\cos\|\mathbf{r}\|)}\right)\left[\mathbf{r}\right]_{\times}^2 | ||
| 125 | \f} | ||
| 126 | \todo remove this and use pinocchio::Jlog3 | ||
| 127 | */ | ||
| 128 | template <typename Derived> | ||
| 129 | 500 | void JlogSO3(const value_type& theta, const Eigen::MatrixBase<Derived>& log, | |
| 130 | matrix3_t& Jlog) { | ||
| 131 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 499 times.
|
500 | if (theta < 1e-6) |
| 132 | 1 | Jlog.setIdentity(); | |
| 133 | else { | ||
| 134 | // Jlog = alpha I | ||
| 135 | 499 | const value_type ct = cos(theta), st = sin(theta); | |
| 136 | 499 | const value_type st_1mct = st / (1 - ct); | |
| 137 | |||
| 138 |
1/2✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
|
499 | Jlog.setZero(); |
| 139 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog.diagonal().setConstant(theta * st_1mct); |
| 140 | |||
| 141 | // Jlog += r_{\times}/2 | ||
| 142 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(0, 1) = -log(2); |
| 143 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(1, 0) = log(2); |
| 144 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(0, 2) = log(1); |
| 145 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(2, 0) = -log(1); |
| 146 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(1, 2) = -log(0); |
| 147 |
2/4✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
|
499 | Jlog(2, 1) = log(0); |
| 148 |
1/2✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
|
499 | Jlog /= 2; |
| 149 | |||
| 150 | 499 | const value_type alpha = 1 / (theta * theta) - st_1mct / (2 * theta); | |
| 151 |
5/10✓ Branch 1 taken 499 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 499 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 499 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 499 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 499 times.
✗ Branch 14 not taken.
|
499 | Jlog.noalias() += alpha * log * log.transpose(); |
| 152 | } | ||
| 153 | 500 | } | |
| 154 | |||
| 155 | /// Compute log of rigid-body transform | ||
| 156 | /// | ||
| 157 | /// \param M rigid body transform, | ||
| 158 | /// \retval theta angle of rotation, | ||
| 159 | /// \retval result 6d vector \f$(\mathbf{v},\mathbf{r})\f$ such that | ||
| 160 | /// the screw motion of linear velocity (of the origin) \f$\mathbf{v}\f$ | ||
| 161 | /// expressed in the moving frame and of angular velocity \f$\mathbf{r}\f$ | ||
| 162 | /// expressed in the moving reaches $M$ in unit time. | ||
| 163 | template <typename Derived> | ||
| 164 | 700 | inline void logSE3(const Transform3s& M, Eigen::MatrixBase<Derived>& result) { | |
| 165 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 700 times.
|
700 | assert(result.size() == 6); |
| 166 | 700 | Eigen::MatrixBase<Derived>& value = | |
| 167 | const_cast<Eigen::MatrixBase<Derived>&>(result); | ||
| 168 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | const matrix3_t& R = M.rotation(); |
| 169 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | const vector3_t& p = M.translation(); |
| 170 | value_type theta; | ||
| 171 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | vector3_t r; |
| 172 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | logSO3(R, theta, r); |
| 173 |
2/4✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 700 times.
✗ Branch 5 not taken.
|
700 | value.segment(3, 3) = r; |
| 174 | value_type alpha, beta; | ||
| 175 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 693 times.
|
700 | if (fabs(theta) < 1e-2) { |
| 176 | 7 | alpha = 1 - theta * theta / 12 - theta * theta * theta * theta / 720; | |
| 177 | 7 | beta = 1. / 12 + theta * theta / 720; | |
| 178 | } else { | ||
| 179 | 693 | alpha = theta * sin(theta) / (2 * (1 - cos(theta))); | |
| 180 | 693 | beta = 1 / (theta * theta) - sin(theta) / (2 * theta * (1 - cos(theta))); | |
| 181 | } | ||
| 182 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | matrix3_t rcross; |
| 183 |
1/2✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
|
700 | computeCrossMatrix(r, rcross); |
| 184 |
9/18✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 700 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 700 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 700 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 700 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 700 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 700 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 700 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 700 times.
✗ Branch 26 not taken.
|
700 | value.segment(0, 3) = alpha * p - .5 * rcross * p + beta * r.dot(p) * r; |
| 185 | 700 | } | |
| 186 | |||
| 187 | template <typename Derived> | ||
| 188 | 100 | void JlogSE3(const Transform3s& M, Eigen::MatrixBase<Derived> const& Jlog) { | |
| 189 | 100 | Eigen::MatrixBase<Derived>& value = | |
| 190 | const_cast<Eigen::MatrixBase<Derived>&>(Jlog); | ||
| 191 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | const matrix3_t& R = M.rotation(); |
| 192 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | const vector3_t& p = M.translation(); |
| 193 | value_type theta; | ||
| 194 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | vector3_t r; |
| 195 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | logSO3(R, theta, r); |
| 196 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | matrix3_t Jlog3; |
| 197 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | JlogSO3(theta, r, Jlog3); |
| 198 | value_type alpha, beta, beta_dot_over_theta; | ||
| 199 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
|
100 | if (fabs(theta) < 1e-2) { |
| 200 | 1 | alpha = 1 - theta * theta / 12 - theta * theta * theta * theta / 720; | |
| 201 | 1 | beta = 1. / 12 + theta * theta / 720; | |
| 202 | 1 | beta_dot_over_theta = 1. / 360.; | |
| 203 | } else { | ||
| 204 | 99 | alpha = theta * sin(theta) / (2 * (1 - cos(theta))); | |
| 205 | 99 | beta = 1 / (theta * theta) - sin(theta) / (2 * theta * (1 - cos(theta))); | |
| 206 | 99 | beta_dot_over_theta = | |
| 207 | 99 | -2 / (theta * theta * theta * theta) + | |
| 208 | 99 | (theta + sin(theta)) / (2 * theta * theta * theta * (1 - cos(theta))); | |
| 209 | } | ||
| 210 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | matrix3_t rcross; |
| 211 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | computeCrossMatrix(r, rcross); |
| 212 |
8/16✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
|
100 | matrix3_t V(alpha * matrix3_t::Identity() - .5 * rcross + |
| 213 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | beta * r * r.transpose()); |
| 214 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | value_type rTp(r.dot(p)); |
| 215 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | matrix3_t pcross; |
| 216 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | computeCrossMatrix(p, pcross); |
| 217 |
11/22✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
|
400 | matrix3_t J( |
| 218 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
200 | (.5 * pcross + (beta_dot_over_theta * rTp) * r * r.transpose() - |
| 219 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | (theta * theta * beta_dot_over_theta + 2 * beta) * p * r.transpose() + |
| 220 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
200 | rTp * beta * matrix3_t::Identity() + beta * r * p.transpose()) * |
| 221 | Jlog3); | ||
| 222 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | value.block(0, 0, 3, 3) = V * R; |
| 223 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | value.block(0, 3, 3, 3) = J; |
| 224 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | Eigen::Block<Derived> b(value.block(3, 0, 3, 3)); |
| 225 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | b.setZero(); |
| 226 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | value.block(3, 3, 3, 3) = Jlog3; |
| 227 | 100 | } | |
| 228 | |||
| 229 | template <typename Derived1, typename Derived2> | ||
| 230 | 43144 | void matrixToQuat(const Eigen::MatrixBase<Derived1>& M, | |
| 231 | Eigen::MatrixBase<Derived2> const& q) { | ||
| 232 | 43144 | Derived2& _q = const_cast<Derived2&>(q.derived()); | |
| 233 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 21572 times.
|
43144 | assert(q.size() == 4); |
| 234 |
1/2✓ Branch 2 taken 21572 times.
✗ Branch 3 not taken.
|
43144 | Eigen::Map<Transform3s::Quaternion> quat(_q.data()); |
| 235 |
1/2✓ Branch 1 taken 21572 times.
✗ Branch 2 not taken.
|
43144 | quat = M; |
| 236 | 43144 | } | |
| 237 | |||
| 238 | template <typename Derived> | ||
| 239 | void se3ToConfig(const Transform3s& M, Eigen::MatrixBase<Derived> const& q) { | ||
| 240 | Derived& _q = const_cast<Derived&>(q.derived()); | ||
| 241 | assert(q.size() == 7); | ||
| 242 | _q.template head<3>() = M.translation(); | ||
| 243 | matrixToQuat(M.rotation(), _q.template tail<4>()); | ||
| 244 | } | ||
| 245 | |||
| 246 | /// \} | ||
| 247 | } // namespace constraints | ||
| 248 | } // namespace hpp | ||
| 249 | |||
| 250 | #endif // HPP_CONSTRAINTS_TOOL_HH | ||
| 251 |