tools.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CONSTRAINTS_TOOL_HH
#define HPP_CONSTRAINTS_TOOL_HH
 
#include <boost/math/constants/constants.hpp>
#include <hpp/constraints/fwd.hh>
#include <pinocchio/spatial/se3.hpp>
 
namespace hpp {
namespace constraints {
 
template <typename VectorType, typename MatrixType>
static void computeCrossMatrix(const VectorType& v, MatrixType& m) {
  m.diagonal().setZero();
  m(0, 1) = -v[2];
  m(1, 0) = v[2];
  m(0, 2) = v[1];
  m(2, 0) = -v[1];
  m(1, 2) = -v[0];
  m(2, 1) = v[0];
}
 
 
template <typename Derived>
inline void logSO3(const matrix3_t& R, value_type& theta,
                   Eigen::MatrixBase<Derived> const& result) {
  Eigen::MatrixBase<Derived>& value =
      const_cast<Eigen::MatrixBase<Derived>&>(result);
  const value_type PI = ::boost::math::constants::pi<value_type>();
  const value_type tr = R.trace();
  if (tr > 3)
    theta = 0;  // acos((3-1)/2)
  else if (tr < -1)
    theta = PI;  // acos((-1-1)/2)
  else
    theta = acos((tr - 1) / 2);
  assert(theta == theta);
  // From runs of tests/logarithm.cc: 1e-6 is too small.
  if (theta < PI - 1e-2) {
    const value_type t = ((theta > 1e-6) ? theta / sin(theta) : 1) / 2;
    value(0) = t * (R(2, 1) - R(1, 2));
    value(1) = t * (R(0, 2) - R(2, 0));
    value(2) = t * (R(1, 0) - R(0, 1));
  } else {
    // 1e-2: A low value is not required since the computation is
    // using explicit formula. However, the precision of this method
    // is the square root of the precision with the antisymmetric
    // method (Nominal case).
    const value_type cphi = cos(theta - PI);
    const value_type beta = theta * theta / (1 + cphi);
    const value_type tmp0 = (R(0, 0) + cphi) * beta;
    const value_type tmp1 = (R(1, 1) + cphi) * beta;
    const value_type tmp2 = (R(2, 2) + cphi) * beta;
    value(0) = (R(2, 1) > R(1, 2) ? 1 : -1) * (tmp0 > 0 ? sqrt(tmp0) : 0);
    value(1) = (R(0, 2) > R(2, 0) ? 1 : -1) * (tmp1 > 0 ? sqrt(tmp1) : 0);
    value(2) = (R(1, 0) > R(0, 1) ? 1 : -1) * (tmp2 > 0 ? sqrt(tmp2) : 0);
  }
}
 
template <typename Derived>
void JlogSO3(const value_type& theta, const Eigen::MatrixBase<Derived>& log,
             matrix3_t& Jlog) {
  if (theta < 1e-6)
    Jlog.setIdentity();
  else {
    // Jlog = alpha I
    const value_type ct = cos(theta), st = sin(theta);
    const value_type st_1mct = st / (1 - ct);
 
    Jlog.setZero();
    Jlog.diagonal().setConstant(theta * st_1mct);
 
    // Jlog += r_{\times}/2
    Jlog(0, 1) = -log(2);
    Jlog(1, 0) = log(2);
    Jlog(0, 2) = log(1);
    Jlog(2, 0) = -log(1);
    Jlog(1, 2) = -log(0);
    Jlog(2, 1) = log(0);
    Jlog /= 2;
 
    const value_type alpha = 1 / (theta * theta) - st_1mct / (2 * theta);
    Jlog.noalias() += alpha * log * log.transpose();
  }
}
 
template <typename Derived>
inline void logSE3(const Transform3s& M, Eigen::MatrixBase<Derived>& result) {
  assert(result.size() == 6);
  Eigen::MatrixBase<Derived>& value =
      const_cast<Eigen::MatrixBase<Derived>&>(result);
  const matrix3_t& R = M.rotation();
  const vector3_t& p = M.translation();
  value_type theta;
  vector3_t r;
  logSO3(R, theta, r);
  value.segment(3, 3) = r;
  value_type alpha, beta;
  if (fabs(theta) < 1e-2) {
    alpha = 1 - theta * theta / 12 - theta * theta * theta * theta / 720;
    beta = 1. / 12 + theta * theta / 720;
  } else {
    alpha = theta * sin(theta) / (2 * (1 - cos(theta)));
    beta = 1 / (theta * theta) - sin(theta) / (2 * theta * (1 - cos(theta)));
  }
  matrix3_t rcross;
  computeCrossMatrix(r, rcross);
  value.segment(0, 3) = alpha * p - .5 * rcross * p + beta * r.dot(p) * r;
}
 
template <typename Derived>
void JlogSE3(const Transform3s& M, Eigen::MatrixBase<Derived> const& Jlog) {
  Eigen::MatrixBase<Derived>& value =
      const_cast<Eigen::MatrixBase<Derived>&>(Jlog);
  const matrix3_t& R = M.rotation();
  const vector3_t& p = M.translation();
  value_type theta;
  vector3_t r;
  r.setZero();
  logSO3(R, theta, r);
  matrix3_t Jlog3;
  JlogSO3(theta, r, Jlog3);
  value_type alpha, beta, beta_dot_over_theta;
  if (fabs(theta) < 1e-2) {
    alpha = 1 - theta * theta / 12 - theta * theta * theta * theta / 720;
    beta = 1. / 12 + theta * theta / 720;
    beta_dot_over_theta = 1. / 360.;
  } else {
    alpha = theta * sin(theta) / (2 * (1 - cos(theta)));
    beta = 1 / (theta * theta) - sin(theta) / (2 * theta * (1 - cos(theta)));
    beta_dot_over_theta =
        -2 / (theta * theta * theta * theta) +
        (theta + sin(theta)) / (2 * theta * theta * theta * (1 - cos(theta)));
  }
  matrix3_t rcross;
  computeCrossMatrix(r, rcross);
  matrix3_t V(alpha * matrix3_t::Identity() - .5 * rcross +
              beta * r * r.transpose());
  value_type rTp(r.dot(p));
  matrix3_t pcross;
  computeCrossMatrix(p, pcross);
  matrix3_t J(
      (.5 * pcross + (beta_dot_over_theta * rTp) * r * r.transpose() -
       (theta * theta * beta_dot_over_theta + 2 * beta) * p * r.transpose() +
       rTp * beta * matrix3_t::Identity() + beta * r * p.transpose()) *
      Jlog3);
  value.block(0, 0, 3, 3) = V * R;
  value.block(0, 3, 3, 3) = J;
  Eigen::Block<Derived> b(value.block(3, 0, 3, 3));
  b.setZero();
  value.block(3, 3, 3, 3) = Jlog3;
}
 
template <typename Derived1, typename Derived2>
void matrixToQuat(const Eigen::MatrixBase<Derived1>& M,
                  Eigen::MatrixBase<Derived2> const& q) {
  Derived2& _q = const_cast<Derived2&>(q.derived());
  assert(q.size() == 4);
  Eigen::Map<Transform3s::Quaternion> quat(_q.data());
  quat = M;
}
 
template <typename Derived>
void se3ToConfig(const Transform3s& M, Eigen::MatrixBase<Derived> const& q) {
  Derived& _q = const_cast<Derived&>(q.derived());
  assert(q.size() == 7);
  _q.template head<3>() = M.translation();
  matrixToQuat(M.rotation(), _q.template tail<4>());
}
 
}  // namespace constraints
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_TOOL_HH