GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/spatial/explog.hpp Lines: 191 201 95.0 % Date: 2024-04-26 13:14:21 Branches: 313 624 50.2 %

 Line Branch Exec Source 1 // 2 // Copyright (c) 2015-2023 CNRS INRIA 3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 // 5 6 #ifndef __pinocchio_spatial_explog_hpp__ 7 #define __pinocchio_spatial_explog_hpp__ 8 9 #include "pinocchio/fwd.hpp" 10 #include "pinocchio/utils/static-if.hpp" 11 #include "pinocchio/math/fwd.hpp" 12 #include "pinocchio/math/sincos.hpp" 13 #include "pinocchio/math/taylor-expansion.hpp" 14 #include "pinocchio/spatial/motion.hpp" 15 #include "pinocchio/spatial/skew.hpp" 16 #include "pinocchio/spatial/se3.hpp" 17 18 #include  19 20 #include "pinocchio/spatial/log.hpp" 21 22 namespace pinocchio 23 { 24  /// \brief Exp: so3 -> SO3. 25  /// 26  /// Return the integral of the input angular velocity during time 1. 27  /// 28  /// \param[in] v The angular velocity vector. 29  /// 30  /// \return The rotational matrix associated to the integration of the angular velocity during time 1. 31  /// 32  template 33  typename Eigen::Matrix 34 2135  exp3(const Eigen::MatrixBase & v) 35  { 36 ✓✗✓✗✓✗✓✗ 2135  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, v, 3, 1); 37 38  typedef typename Vector3Like::Scalar Scalar; 39  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like) Vector3LikePlain; 40  typedef Eigen::Matrix Matrix3; 41 42 ✓✗ 2135  const Scalar t2 = v.squaredNorm(); 43 44 2135  const Scalar t = math::sqrt(t2); 45 2135  Scalar ct,st; SINCOS(t,&st,&ct); 46 47 ✓✗ 2135  const Scalar alpha_vxvx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion::template precision<3>(), 48  (1 - ct)/t2, 49 ✓✗ 2135  Scalar(1)/Scalar(2) - t2/24); 50 ✓✗ 2135  const Scalar alpha_vx = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion::template precision<3>(), 51  (st)/t, 52 ✓✗ 2135  Scalar(1) - t2/6); 53 ✓✗✓✗✓✗✓✗ 2135  Matrix3 res(alpha_vxvx * v * v.transpose()); 54 ✓✗✓✗✓✗✓✗ 2135  res.coeffRef(0,1) -= alpha_vx * v[2]; res.coeffRef(1,0) += alpha_vx * v[2]; 55 ✓✗✓✗✓✗✓✗ 2135  res.coeffRef(0,2) += alpha_vx * v[1]; res.coeffRef(2,0) -= alpha_vx * v[1]; 56 ✓✗✓✗✓✗✓✗ 2135  res.coeffRef(1,2) -= alpha_vx * v[0]; res.coeffRef(2,1) += alpha_vx * v[0]; 57 58 ✓✗ 2135  ct = internal::if_then_else(internal::GT, t, TaylorSeriesExpansion::template precision<3>(), 59  ct, 60 ✓✗ 2135  Scalar(1) - t2/2); 61 ✓✗✓✗✓✗ 2135  res.diagonal().array() += ct; 62 63 4270  return res; 64  } 65 66  /// \brief Same as \ref log3 67  /// 68  /// \param[in] R the rotation matrix. 69  /// \param[out] theta the angle value. 70  /// 71  /// \return The angular velocity vector associated to the rotation matrix. 72  /// 73  template 74  Eigen::Matrix 75 21810  log3(const Eigen::MatrixBase & R, 76  typename Matrix3Like::Scalar & theta) 77  { 78  typedef typename Matrix3Like::Scalar Scalar; 79  typedef Eigen::Matrix Vector3; 81 21810  Vector3 res; 82 21810  log3_impl::run(R, theta, res); 83 21810  return res; 84  } 85 86  /// \brief Log: SO(3)-> so(3). 87  /// 88  /// Pseudo-inverse of log from \f$SO3 -> { v \in so3, ||v|| \le pi } \f$. 89  /// 90  /// \param[in] R The rotation matrix. 91  /// 92  /// \return The angular velocity vector associated to the rotation matrix. 93  /// 94  template 95  Eigen::Matrix 96 36  log3(const Eigen::MatrixBase & R) 97  { 98  typename Matrix3Like::Scalar theta; 99 ✓✗ 72  return log3(R.derived(),theta); 100  } 101 102  /// 103  /// \brief Derivative of \f$\exp{r} \f$ 104  /// \f[ 105  /// \frac{\sin{||r||}}{||r||} I_3 106  /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x 107  /// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T 108  /// \f] 109  /// 110  template 111 434  void Jexp3(const Eigen::MatrixBase & r, 112  const Eigen::MatrixBase & Jexp) 113  { 114 ✓✗✓✓✗✓✗✓✗ 434  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector3Like, r , 3, 1); 115 ✓✗✓✓✗✓✗✓✗ 434  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix3Like, Jexp, 3, 3); 116 117 434  Matrix3Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jexp); 118  typedef typename Matrix3Like::Scalar Scalar; 119 120 ✓✗ 434  const Scalar n2 = r.squaredNorm(); 121 434  const Scalar n = math::sqrt(n2); 122 434  const Scalar n_inv = Scalar(1)/n; 123 434  const Scalar n2_inv = n_inv * n_inv; 124 434  Scalar cn,sn; SINCOS(n,&sn,&cn); 125 126 ✓✗ 434  const Scalar a = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion::template precision<3>(), 127  Scalar(1) - n2/Scalar(6), 128 ✓✗ 434  sn*n_inv); 129 ✓✗ 434  const Scalar b = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion::template precision<3>(), 130  - Scalar(1)/Scalar(2) - n2/Scalar(24), 131 ✓✗ 434  - (1-cn)*n2_inv); 132 ✓✗ 434  const Scalar c = internal::if_then_else(internal::LT, n, TaylorSeriesExpansion::template precision<3>(), 133  Scalar(1)/Scalar(6) - n2/Scalar(120), 134 ✓✗ 434  n2_inv * (1 - a)); 135 136  switch(op) 137  { 138  case SETTO: 139 ✓✗✓✗ 430  Jout.diagonal().setConstant(a); 140 ✓✗✓✗✓✗✓✗ 430  Jout(0,1) = -b*r[2]; Jout(1,0) = -Jout(0,1); 141 ✓✗✓✗✓✗✓✗ 430  Jout(0,2) = b*r[1]; Jout(2,0) = -Jout(0,2); 142 ✓✗✓✗✓✗✓✗ 430  Jout(1,2) = -b*r[0]; Jout(2,1) = -Jout(1,2); 143 ✓✗✓✗✓✗✓✗✓✗ 430  Jout.noalias() += c * r * r.transpose(); 144 430  break; 145  case ADDTO: 146 ✓✗✓✗✓✗ 2  Jout.diagonal().array() += a; 147 ✓✗✓✗✓✗✓✗ 2  Jout(0,1) += -b*r[2]; Jout(1,0) += b*r[2]; 148 ✓✗✓✗✓✗✓✗ 2  Jout(0,2) += b*r[1]; Jout(2,0) += -b*r[1]; 149 ✓✗✓✗✓✗✓✗ 2  Jout(1,2) += -b*r[0]; Jout(2,1) += b*r[0]; 150 ✓✗✓✗✓✗✓✗✓✗ 2  Jout.noalias() += c * r * r.transpose(); 151 2  break; 152  case RMTO: 153 ✓✗✓✗✓✗ 2  Jout.diagonal().array() -= a; 154 ✓✗✓✗✓✗✓✗ 2  Jout(0,1) -= -b*r[2]; Jout(1,0) -= b*r[2]; 155 ✓✗✓✗✓✗✓✗ 2  Jout(0,2) -= b*r[1]; Jout(2,0) -= -b*r[1]; 156 ✓✗✓✗✓✗✓✗ 2  Jout(1,2) -= -b*r[0]; Jout(2,1) -= b*r[0]; 157 ✓✗✓✗✓✗✓✗✓✗ 2  Jout.noalias() -= c * r * r.transpose(); 158 2  break; 159  default: 160  assert(false && "Wrong Op requesed value"); 161  break; 162  } 163 434  } 164 165  /// 166  /// \brief Derivative of \f$\exp{r} \f$ 167  /// \f[ 168  /// \frac{\sin{||r||}}{||r||} I_3 169  /// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x 170  /// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T 171  /// \f] 172  /// 173  template 174 8  void Jexp3(const Eigen::MatrixBase & r, 175  const Eigen::MatrixBase & Jexp) 176  { 177 8  Jexp3(r, Jexp); 178 8  } 179 180  /** \brief Derivative of log3 181  * 182  * This function is the right derivative of @ref log3, that is, for \f$R \in 183  * SO(3)\f$ and \f$\omega t in \mathfrak{so}(3)\f$, it provides the linear 184  * approximation: 185  * 186  * \f[ 187  * \log_3(R \oplus \omega t) = \log_3(R \exp_3(\omega t)) \approx \log_3(R) + \text{Jlog3}(R) \omega t 188  * \f] 189  * 190  * \param[in] theta the angle value. 191  * \param[in] log the output of log3. 192  * \param[out] Jlog the jacobian 193  * 194  * Equivalently, \f$\text{Jlog3}\f$ is the right Jacobian of \f$\log_3\f$: 195  * 196  * \f[ 197  * \text{Jlog3}(R) = \frac{\partial \log_3(R)}{\partial R} 198  * \f] 199  * 200  * Note that this is the right Jacobian: \f$\text{Jlog3}(R) : T_{R} SO(3) \to T_{\log_6(R)} \mathfrak{so}(3)\f$. 201  * (By convention, calculations in Pinocchio always perform right differentiation, 202  * i.e., Jacobians are in local coordinates (also known as body coordinates), unless otherwise specified.) 203  * 204  * If we denote by \f$\theta = \log_3(R)\f$ and \f$\log = \log_3(R, 205  * \theta)\f$, then \f$\text{Jlog} = \text{Jlog}_3(R)\f$ can be calculated as: 206  * 207  * \f[ 208  * \begin{align*} 209  * \text{Jlog} & = \frac{\theta \sin(\theta)}{2 (1 - \cos(\theta))} I_3 210  * + \frac{1}{2} \widehat{\log} 211  * + \left(\frac{1}{\theta^2} - \frac{\sin(\theta)}{2\theta(1-\cos(\theta))}\right) \log \log^T \\ 212  * & = I_3 213  * + \frac{1}{2} \widehat{\log} 214  * + \left(\frac{1}{\theta^2} - \frac{1 + \cos \theta}{2 \theta \sin \theta}\right) \widehat{\log}^2 215  * \end{align*} 216  * \f] 217  * 218  * where \f$\widehat{v}\f$ denotes the skew-symmetric matrix obtained from the 3D vector \f$v\f$. 219  * 220  * \note The inputs must be such that \f$\theta = \Vert \log \Vert \f$. 221  */ 222  template 223 992  void Jlog3(const Scalar & theta, 224  const Eigen::MatrixBase & log, 225  const Eigen::MatrixBase & Jlog) 226  { 227 992  Jlog3_impl::run(theta, log, 228 992  PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog)); 229 992  } 230 231  /** \brief Derivative of log3 232  * 233  * \param[in] R the rotation matrix. 234  * \param[out] Jlog the jacobian 235  * 236  * Equivalent to 237  * \code 238  * double theta; 239  * Vector3 log = pinocchio::log3 (R, theta); 240  * pinocchio::Jlog3 (theta, log, Jlog); 241  * \endcode 242  */ 243  template 244 11  void Jlog3(const Eigen::MatrixBase & R, 245  const Eigen::MatrixBase & Jlog) 246  { 247  typedef typename Matrix3Like1::Scalar Scalar; 248  typedef Eigen::Matrix Vector3; 249 250  Scalar t; 251 ✓✗ 11  Vector3 w(log3(R,t)); 252 ✓✗ 11  Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,Jlog)); 253 11  } 254 255  template 256 3  void Hlog3(const Scalar & theta, 257  const Eigen::MatrixBase & log, 258  const Eigen::MatrixBase & v, 259  const Eigen::MatrixBase & vt_Hlog) 260  { 261  typedef Eigen::Matrix Vector3; 262 3  Matrix3Like & vt_Hlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,vt_Hlog); 263 264  // theta = (log^T * log)^.5 265  // dt/dl = .5 * 2 * log^T * (log^T * log)^-.5 266  // = log^T / theta 267  // dt_dl = log / theta 268 3  Scalar ctheta,stheta; SINCOS(theta,&stheta,&ctheta); 269 270 3  Scalar denom = .5 / (1-ctheta), 271 3  a = theta * stheta * denom, 272 3  da_dt = (stheta - theta) * denom, // da / dtheta 273 3  b = ( 1 - a ) / (theta*theta), 274  //db_dt = - (2 * (1 - a) / theta + da_dt ) / theta**2; // db / dtheta 275 3  db_dt = - (2 / theta - (theta + stheta) * denom) / (theta*theta); // db / dtheta 276 277  // Compute dl_dv_v = Jlog * v 278  // Jlog = a I3 + .5 [ log ] + b * log * log^T 279  // if v == log, then Jlog * v == v 280 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 3  Vector3 dl_dv_v (a*v + .5*log.cross(v) + b*log*log.transpose()*v); 281 282 ✓✗ 3  Scalar dt_dv_v = log.dot(dl_dv_v) / theta; 283 284  // Derivative of b * log * log^T 285 ✓✗✓✗✓✗✓✗✓✗ 3  vt_Hlog_.noalias() = db_dt * dt_dv_v * log * log.transpose(); 286 ✓✗✓✗✓✗✓✗✓✗ 3  vt_Hlog_.noalias() += b * dl_dv_v * log.transpose(); 287 ✓✗✓✗✓✗✓✗✓✗ 3  vt_Hlog_.noalias() += b * log * dl_dv_v.transpose(); 288  // Derivative of .5 * [ log ] 289 ✓✗✓✗ 3  addSkew(.5 * dl_dv_v, vt_Hlog_); 290  // Derivative of a * I3 291 ✓✗✓✗✓✗ 3  vt_Hlog_.diagonal().array() += da_dt * dt_dv_v; 292 3  } 293 294  /** \brief Second order derivative of log3 295  * 296  * This computes \f$v^T H_{log} \f$. 297  * 298  * \param[in] R the rotation matrix. 299  * \param[in] v the 3D vector. 300  * \param[out] vt_Hlog the product of the Hessian with the input vector 301  */ 302  template 303 3  void Hlog3(const Eigen::MatrixBase & R, 304  const Eigen::MatrixBase & v, 305  const Eigen::MatrixBase & vt_Hlog) 306  { 307  typedef typename Matrix3Like1::Scalar Scalar; 308  typedef Eigen::Matrix Vector3; 309 310  Scalar t; 311 ✓✗ 3  Vector3 w(log3(R,t)); 312 ✓✗ 3  Hlog3(t,w,v,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like2,vt_Hlog)); 313 3  } 314 315  /// 316  /// \brief Exp: se3 -> SE3. 317  /// 318  /// Return the integral of the input twist during time 1. 319  /// 320  /// \param[in] nu The input twist. 321  /// 322  /// \return The rigid transformation associated to the integration of the twist during time 1. 323  /// 324  template 325  SE3Tpl 326 82653  exp6(const MotionDense & nu) 327  { 328  typedef typename MotionDerived::Scalar Scalar; 329  enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options }; 330 331  typedef SE3Tpl SE3; 332 333 ✓✗ 82653  SE3 res; 334 ✓✗ 82653  typename SE3::LinearType & trans = res.translation(); 335 ✓✗ 82653  typename SE3::AngularType & rot = res.rotation(); 336 337 ✓✗ 82653  const typename MotionDerived::ConstAngularType & w = nu.angular(); 338 ✓✗ 82653  const typename MotionDerived::ConstLinearType & v = nu.linear(); 339 340  Scalar alpha_wxv, alpha_v, alpha_w, diagonal_term; 341 ✓✗ 82653  const Scalar t2 = w.squaredNorm(); 342 82653  const Scalar t = math::sqrt(t2); 343 82653  Scalar ct,st; SINCOS(t,&st,&ct); 344 82653  const Scalar inv_t2 = Scalar(1)/t2; 345 346 ✓✗ 82653  alpha_wxv = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 347  Scalar(1)/Scalar(2) - t2/24, 348 ✓✗ 82653  (Scalar(1) - ct)*inv_t2); 349 350 ✓✗ 82653  alpha_v = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 351  Scalar(1) - t2/6, 352 ✓✗ 82653  (st)/t); 353 354 ✓✗ 82653  alpha_w = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 355  (Scalar(1)/Scalar(6) - t2/120), 356 ✓✗ 82653  (Scalar(1) - alpha_v)*inv_t2); 357 358 ✓✗ 82653  diagonal_term = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 359 ✓✗ 82653  Scalar(1) - t2/2, 360  ct); 361 362  // Linear 363 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 82653  trans.noalias() = (alpha_v*v + (alpha_w*w.dot(v))*w + alpha_wxv*w.cross(v)); 364 365  // Rotational 366 ✓✗✓✗✓✗✓✗✓✗ 82653  rot.noalias() = alpha_wxv * w * w.transpose(); 367 ✓✗✓✗✓✗✓✗ 82653  rot.coeffRef(0,1) -= alpha_v * w[2]; rot.coeffRef(1,0) += alpha_v * w[2]; 368 ✓✗✓✗✓✗✓✗ 82653  rot.coeffRef(0,2) += alpha_v * w[1]; rot.coeffRef(2,0) -= alpha_v * w[1]; 369 ✓✗✓✗✓✗✓✗ 82653  rot.coeffRef(1,2) -= alpha_v * w[0]; rot.coeffRef(2,1) += alpha_v * w[0]; 370 ✓✗✓✗✓✗ 82653  rot.diagonal().array() += diagonal_term; 371 372 165306  return res; 373  } 374 375  /// \brief Exp: se3 -> SE3. 376  /// 377  /// Return the integral of the input spatial velocity during time 1. 378  /// 379  /// \param[in] v The twist represented by a vector. 380  /// 381  /// \return The rigid transformation associated to the integration of the twist vector during time 1. 382  /// 383  template 384  SE3Tpl 385 1  exp6(const Eigen::MatrixBase & v) 386  { 387 ✓✗✓✗✓✗✓✗ 1  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Vector6Like, v, 6, 1); 388 389 ✓✗ 1  MotionRef nu(v.derived()); 390 ✓✗ 2  return exp6(nu); 391  } 392 393  /// \brief Log: SE3 -> se3. 394  /// 395  /// Pseudo-inverse of exp from \f$SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } \f$. 396  /// 397  /// \param[in] M The rigid transformation. 398  /// 399  /// \return The twist associated to the rigid transformation during time 1. 400  /// 401  template 402  MotionTpl 403 19396  log6(const SE3Tpl & M) 404  { 405  typedef MotionTpl Motion; 406 19396  Motion mout; 407 19396  log6_impl::run(M, mout); 408 19396  return mout; 409  } 410 411  /// \brief Log: SE3 -> se3. 412  /// 413  /// Pseudo-inverse of exp from \f$SE3 \to { v,\omega \in \mathfrak{se}(3), ||\omega|| < 2\pi } \f$. 414  /// 415  /// \param[in] M The rigid transformation represented as an homogenous matrix. 416  /// 417  /// \return The twist associated to the rigid transformation during time 1. 418  /// 419  template 420  MotionTpl::Options> 421 1  log6(const Eigen::MatrixBase & M) 422  { 423 ✓✗✓✗✓✗✓✗ 1  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like, M, 4, 4); 424 425  typedef typename Matrix4Like::Scalar Scalar; 426  enum {Options = Eigen::internal::traits::Options}; 427  typedef MotionTpl Motion; 428  typedef SE3Tpl SE3; 429 430 ✓✗ 1  SE3 m(M); 431 ✓✗ 1  Motion mout; 432 ✓✗ 1  log6_impl::run(m, mout); 433 2  return mout; 434  } 435 436  /// \brief Derivative of exp6 437  /// Computed as the inverse of Jlog6 438  template 439 232  void Jexp6(const MotionDense & nu, 440  const Eigen::MatrixBase & Jexp) 441  { 442 ✓✗✓✗✓✗✓✗ 232  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE (Matrix6Like, Jexp, 6, 6); 443 444  typedef typename MotionDerived::Scalar Scalar; 445  typedef typename MotionDerived::Vector3 Vector3; 446  typedef Eigen::Matrix Matrix3; 447 232  Matrix6Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jexp); 448 449 ✓✗ 232  const typename MotionDerived::ConstLinearType & v = nu.linear(); 450 ✓✗ 232  const typename MotionDerived::ConstAngularType & w = nu.angular(); 451 ✓✗ 232  const Scalar t2 = w.squaredNorm(); 452 232  const Scalar t = math::sqrt(t2); 453 454 232  const Scalar tinv = Scalar(1)/t, 455 232  t2inv = tinv*tinv; 456 232  Scalar st,ct; SINCOS (t, &st, &ct); 457 232  const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct)); 458 459 460 ✓✗ 232  const Scalar beta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 461  Scalar(1)/Scalar(12) + t2/Scalar(720), 462 ✓✗ 232  t2inv - st*tinv*inv_2_2ct); 463 464 ✓✗ 232  const Scalar beta_dot_over_theta = internal::if_then_else(internal::LT, t, TaylorSeriesExpansion::template precision<3>(), 465  Scalar(1)/Scalar(360), 466 ✓✗ 232  -Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct); 467 468  switch(op) 469  { 470  case SETTO: 471  { 472 ✓✗✓✗ 228  Jexp3(w, Jout.template bottomRightCorner<3,3>()); 473 ✓✗✓✗✓✗ 228  Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>(); 474 ✓✗✓✗✓✗✓✗ 228  const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v; 475 ✓✗ 228  const Scalar wTp (w.dot (p)); 476 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 912  const Matrix3 J (alphaSkew(.5, p) + 477 ✓✗ 228  (beta_dot_over_theta*wTp) *w*w.transpose() 478 ✓✗ 228  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose() 479 ✓✗ 228  + wTp * beta * Matrix3::Identity() 480 ✓✗ 228  + beta *w*p.transpose()); 481 ✓✗✓✗✓✗✓✗✓✗✓✗ 228  Jout.template topRightCorner<3,3>().noalias() = 482  - Jout.template topLeftCorner<3,3>() * J; 483 ✓✗✓✗ 228  Jout.template bottomLeftCorner<3,3>().setZero(); 484 228  break; 485  } 486  case ADDTO: 487  { 488 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH 489 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED 490 ✓✗ 2  Matrix3 Jtmp3; 491 ✓✗ 2  Jexp3(w, Jtmp3); 492 PINOCCHIO_COMPILER_DIAGNOSTIC_POP 493 ✓✗✓✗ 2  Jout.template bottomRightCorner<3,3>() += Jtmp3; 494 ✓✗✓✗ 2  Jout.template topLeftCorner<3,3>() += Jtmp3; 495 ✓✗✓✗✓✗ 2  const Vector3 p = Jtmp3.transpose() * v; 496 ✓✗ 2  const Scalar wTp (w.dot (p)); 497 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 8  const Matrix3 J (alphaSkew(.5, p) + 498 ✓✗ 2  (beta_dot_over_theta*wTp) *w*w.transpose() 499 ✓✗ 2  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose() 500 ✓✗ 2  + wTp * beta * Matrix3::Identity() 501 ✓✗ 2  + beta *w*p.transpose()); 502 ✓✗✓✗✓✗✓✗✓✗ 2  Jout.template topRightCorner<3,3>().noalias() += 503  - Jtmp3 * J; 504 2  break; 505  } 506  case RMTO: 507  { 508 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH 509 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED 510 ✓✗ 2  Matrix3 Jtmp3; 511 ✓✗ 2  Jexp3(w, Jtmp3); 512 PINOCCHIO_COMPILER_DIAGNOSTIC_POP 513 ✓✗✓✗ 2  Jout.template bottomRightCorner<3,3>() -= Jtmp3; 514 ✓✗✓✗ 2  Jout.template topLeftCorner<3,3>() -= Jtmp3; 515 ✓✗✓✗✓✗ 2  const Vector3 p = Jtmp3.transpose() * v; 516 ✓✗ 2  const Scalar wTp (w.dot (p)); 517 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 8  const Matrix3 J (alphaSkew(.5, p) + 518 ✓✗ 2  (beta_dot_over_theta*wTp) *w*w.transpose() 519 ✓✗ 2  - (t2*beta_dot_over_theta+Scalar(2)*beta)*p*w.transpose() 520 ✓✗ 2  + wTp * beta * Matrix3::Identity() 521 ✓✗ 2  + beta *w*p.transpose()); 522 ✓✗✓✗✓✗✓✗✓✗ 2  Jout.template topRightCorner<3,3>().noalias() -= 523  - Jtmp3 * J; 524 2  break; 525  } 526  default: 527  assert(false && "Wrong Op requesed value"); 528  break; 529  } 530 232  } 531 532  /// \brief Derivative of exp6 533  /// Computed as the inverse of Jlog6 534  template 535 60  void Jexp6(const MotionDense & nu, 536  const Eigen::MatrixBase & Jexp) 537  { 538 60  Jexp6(nu, Jexp); 539 60  } 540 541  /** \brief Derivative of log6 542  * 543  * This function is the right derivative of @ref log6, that is, for \f$M \in 544  * SE(3)\f$ and \f$\xi in \mathfrak{se}(3)\f$, it provides the linear 545  * approximation: 546  * 547  * \f[ 548  * \log_6(M \oplus \xi) = \log_6(M \exp_6(\xi)) \approx \log_6(M) + \text{Jlog6}(M) \xi 549  * \f] 550  * 551  * Equivalently, \f$\text{Jlog6}\f$ is the right Jacobian of \f$\log_6\f$: 552  * 553  * \f[ 554  * \text{Jlog6}(M) = \frac{\partial \log_6(M)}{\partial M} 555  * \f] 556  * 557  * Note that this is the right Jacobian: \f$\text{Jlog6}(M) : T_{M} SE(3) \to T_{\log_6(M)} \mathfrak{se}(3)\f$. 558  * (By convention, calculations in Pinocchio always perform right differentiation, 559  * i.e., Jacobians are in local coordinates (also known as body coordinates), unless otherwise specified.) 560  * 561  * Internally, it is calculated using the following formulas: 562  * 563  * \f[ 564  * \text{Jlog6}(M) = 565  * \left(\begin{array}{cc} 566  * \text{Jlog3}(R) & J * \text{Jlog3}(R) \\ 567  * 0 & \text{Jlog3}(R) \\ 568  * \end{array}\right) 569  * \f] 570  * where 571  * \f[ M = 572  * \left(\begin{array}{cc} 573  * \exp(\mathbf{r}) & \mathbf{p} \\ 574  * 0 & 1 \\ 575  * \end{array}\right) 576  * \f] 577  * \f[ 578  * \begin{eqnarray} 579  * J &=& 580  * \left.\frac{1}{2}[\mathbf{p}]_{\times} + \beta'(||r||) \frac{\mathbf{r}^T\mathbf{p}}{||r||}\mathbf{r}\mathbf{r}^T 581  * - (||r||\beta'(||r||) + 2 \beta(||r||)) \mathbf{p}\mathbf{r}^T\right.\\ 582  * &&\left. + \mathbf{r}^T\mathbf{p}\beta(||r||)I_3 + \beta(||r||)\mathbf{r}\mathbf{p}^T\right. 583  * \end{eqnarray} 584  * \f] 585  * and 586  * \f[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \f] 587  * 588  * 589  * \cheatsheet For \f$(A,B) \in SE(3)^2\f$, let \f$M_1(A, B) = A B\f$ and 590  * \f$m_1 = \log_6(M_1) \f$. Then, we have the following partial (right) 591  * Jacobians: \n 592  * - \f$\frac{\partial m_1}{\partial A} = Jlog_6(M_1) Ad_B^{-1} \f$, 593  * - \f$\frac{\partial m_1}{\partial B} = Jlog_6(M_1) \f$. 594  * 595  * \cheatsheet Let \f$A \in SE(3)\f$, \f$M_2(A) = A^{-1}\f$ and \f$m_2 = 596  * \log_6(M_2)\f$. Then, we have the following partial (right) Jacobian: \n 597  * - \f$\frac{\partial m_2}{\partial A} = - Jlog_6(M_2) Ad_A \f$. 598  */ 599  template 600 315  void Jlog6(const SE3Tpl & M, 601  const Eigen::MatrixBase & Jlog) 602  { 603 315  Jlog6_impl::run(M,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog)); 604 315  } 605 606  template 607  template 608 26  SE3Tpl SE3Tpl::Interpolate(const SE3Tpl & A, 609  const SE3Tpl & B, 610  const OtherScalar & alpha) 611  { 612  typedef SE3Tpl ReturnType; 613  typedef MotionTpl Motion; 614 615 ✓✗✓✗ 26  Motion dv = log6(A.actInv(B)); 616 ✓✗✓✗✓✗ 26  ReturnType res = A * exp6(alpha*dv); 617 52  return res; 618  } 619 620 } // namespace pinocchio 621 622 #include "pinocchio/spatial/explog-quaternion.hpp" 623 #include "pinocchio/spatial/log.hxx" 624 625 #endif //#ifndef __pinocchio_spatial_explog_hpp__

 Generated by: GCOVR (Version 4.2)