liegroup-element.hh

Go to the documentation of this file.

// Copyright (c) 2017, CNRS
// Authors: Florent Lamiraux
//
 
// 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_PINOCCHIO_LIEGROUP_ELEMENT_HH
#define HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
 
#include <hpp/pinocchio/deprecated.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <pinocchio/math/quaternion.hpp>
 
namespace hpp {
namespace pinocchio {
 
template <typename vector_type>
class LiegroupElementConstBase {
 public:
  template <typename Derived>
  LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value,
                           const LiegroupSpacePtr_t& liegroupSpace)
      : value_(value.derived()), space_(liegroupSpace) {
    check();
  }
 
  template <typename Derived>
  explicit LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value)
      : value_(value.derived()),
        space_(LiegroupSpace::create(value.derived().size())) {}
 
  template <typename vector_type2>
  LiegroupElementConstBase(const LiegroupElementConstBase<vector_type2>& other)
      : value_(other.vector()), space_(other.space()) {}
 
  const LiegroupSpacePtr_t& space() const { return space_; }
 
  const vector_type& vector() const { return value_; }
 
  size_type size() const { return value_.size(); }
 
  void check() const { assert(value_.size() == space_->nq()); }
 
  template <typename vector_type2>
  bool operator==(const LiegroupElementConstBase<vector_type2>& other) {
    return ((*space_ == *(other.space_)) && (value_ == other.value_));
  }
 
  template <typename vector_type2>
  bool operator!=(const LiegroupElementConstBase<vector_type2>& other) {
    return ((*space_ != *(other.space_)) || (value_ != other.value_));
  }
 
 protected:
  template <typename Derived>
  LiegroupElementConstBase(const Eigen::EigenBase<Derived>& value,
                           const LiegroupSpacePtr_t& space,
                           void* /*to enable this constructor*/)
      : value_(const_cast<Derived&>(value.derived())), space_(space) {}
 
  vector_type value_;
  LiegroupSpacePtr_t space_;
 
  template <typename vector_type2>
  friend class LiegroupElementConstBase;
  template <typename vector_type2>
  friend class LiegroupElementBase;
};
 
template <typename vector_type>
class LiegroupElementBase : public LiegroupElementConstBase<vector_type> {
 public:
  typedef LiegroupElementConstBase<vector_type> Base;
 
  LiegroupElementBase(const vector_type& value, const LiegroupSpacePtr_t& space)
      : Base(value, space, NULL) {}
 
  LiegroupElementBase(const LiegroupSpacePtr_t& space)
      : Base(vector_t(space->nq()), space) {}
 
  explicit LiegroupElementBase(const vector_type& value)
      : Base(value, LiegroupSpace::create(value.size()), NULL) {}
 
  template <typename vector_type2>
  LiegroupElementBase(const LiegroupElementConstBase<vector_type2>& other)
      : Base(other.value_, other.space()) {}
 
  template <typename vector_type2>
  LiegroupElementBase(const LiegroupElementBase<vector_type2>& other)
      : Base(other.value_, other.space()) {}
 
  template <typename vector_type2>
  LiegroupElementBase(LiegroupElementBase<vector_type2>& other)
      : Base(other.value_, other.space(), NULL) {}
 
  LiegroupElementBase() : Base(vector_t(), LiegroupSpace::empty()) {}
 
  const vector_type& vector() const { return Base::vector(); }
 
  vector_type& vector() { return this->value_; }
 
  void setNeutral() { this->value_ = this->space_->neutral().vector(); }
 
  LiegroupElementBase& operator+=(vectorIn_t v);
 
  template <typename vector_type2>
  LiegroupElementBase& operator=(
      const LiegroupElementConstBase<vector_type2>& other) {
    this->space_ = other.space();
    this->value_ = other.vector();
    return *this;
  }
 
  template <typename Vector>
  LiegroupElementBase& operator=(const Eigen::MatrixBase<Vector>& v) {
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector);
    assert(this->space_->nq() == v.derived().size());
    this->value_.noalias() = v.derived();
    return *this;
  }
};
 
template <typename vector_type>
LiegroupElement operator+(const LiegroupElementConstBase<vector_type>& e,
                          vectorIn_t v);
 
template <typename vector_type1, typename vector_type2>
vector_t operator-(const LiegroupElementConstBase<vector_type1>& e1,
                   const LiegroupElementConstBase<vector_type2>& e2);
 
template <typename vector_type>
bool checkNormalized(
    const LiegroupElementConstBase<vector_type>& e1,
    const value_type& eps = PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE);
 
template <typename vector_type>
vector_t log(const LiegroupElementConstBase<vector_type>& lge);
 
template <typename vector_type>
inline std::ostream& operator<<(
    std::ostream& os, const LiegroupElementConstBase<vector_type>& e) {
  os << "Lie group element in " << *(e.space()) << " represented by vector ("
     << e.vector().transpose() << ")";
  return os;
}
}  // namespace pinocchio
}  // namespace hpp
 
HPP_SERIALIZABLE_SPLIT_FREE(hpp::pinocchio::LiegroupElement)
 
#endif  // HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH