liegroup-element.hh

Go to the documentation of this file.

// Copyright (c) 2017, CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH
# define HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH

# include <hpp/pinocchio/liegroup-space.hh>
# include <hpp/pinocchio/deprecated.hh>

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 ());
      }

    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>
    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

#endif // HPP_PINOCCHIO_LIEGROUP_ELEMENT_HH