liegroup-space.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_SPACE_HH
# define HPP_PINOCCHIO_LIEGROUP_SPACE_HH

# include <vector>
# include <string>
# include <pinocchio/fwd.hpp>
# include <boost/variant.hpp>
# include <pinocchio/multibody/liegroup/special-euclidean.hpp>
# include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
# include <pinocchio/multibody/liegroup/vector-space.hpp>
# include <hpp/pinocchio/liegroup.hh>
# include <hpp/pinocchio/fwd.hh>

namespace hpp {
  namespace pinocchio {

#ifdef HPP_PINOCCHIO_PARSED_BY_DOXYGEN
    typedef ABoostVariant LiegroupType;
#else
    typedef boost::variant <liegroup::VectorSpaceOperation
                            <Eigen::Dynamic, false>,
                            liegroup::VectorSpaceOperation <1, true>,
                            liegroup::VectorSpaceOperation <1, false>,
                            liegroup::VectorSpaceOperation <2, false>,
                            liegroup::VectorSpaceOperation <3, false>,
                            liegroup::VectorSpaceOperation <3, true>,
                            liegroup::CartesianProductOperation<
                              liegroup::VectorSpaceOperation<3, false>,
                              liegroup::SpecialOrthogonalOperation<3> >,
                            liegroup::CartesianProductOperation<
                              liegroup::VectorSpaceOperation<2, false>,
                              liegroup::SpecialOrthogonalOperation<2> >,
                            liegroup::SpecialOrthogonalOperation <2>,
                            liegroup::SpecialOrthogonalOperation <3>,
                            liegroup::SpecialEuclideanOperation <2>,
                            liegroup::SpecialEuclideanOperation <3> >
    LiegroupType;
#endif

    enum DerivativeProduct {
      DerivativeTimesInput,
      InputTimesDerivative
    };

    class LiegroupSpace
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      static LiegroupSpacePtr_t Rn (const size_type& n);
      static LiegroupSpacePtr_t R1 ();
      static LiegroupSpacePtr_t R2 ();
      static LiegroupSpacePtr_t R3 ();
      static LiegroupSpacePtr_t SE2 ();
      static LiegroupSpacePtr_t SE3 ();
      static LiegroupSpacePtr_t SO2 ();
      static LiegroupSpacePtr_t SO3 ();
      static LiegroupSpacePtr_t R2xSO2 ();
      static LiegroupSpacePtr_t R3xSO3 ();
      static LiegroupSpacePtr_t empty ();

      static LiegroupSpacePtr_t create (const size_type& size)
      {
        LiegroupSpace* ptr (new LiegroupSpace (size));
        LiegroupSpacePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      static LiegroupSpacePtr_t createCopy
      (const LiegroupSpaceConstPtr_t& other)
      {
        LiegroupSpace* ptr (new LiegroupSpace (*other));
        LiegroupSpacePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      static LiegroupSpacePtr_t create (const LiegroupType& type)
      {
        LiegroupSpace* ptr (new LiegroupSpace (type));
        LiegroupSpacePtr_t shPtr (ptr);
        ptr->init (shPtr);
        return shPtr;
      }

      size_type nq () const
      {
        return nq_;
      }
      size_type nv () const
      {
        return nv_;
      }
      size_type nq (const std::size_t& rank) const;
      size_type nv (const std::size_t& rank) const;

      const std::vector <LiegroupType>& liegroupTypes () const
      {
        return liegroupTypes_;
      }

      LiegroupElement neutral () const;

      LiegroupElement element (vectorIn_t q) const;

      LiegroupElementRef elementRef (vectorOut_t q) const;

      LiegroupElementConstRef elementConstRef (vectorIn_t q) const;

      LiegroupElement exp (vectorIn_t v) const;

      template <DerivativeProduct side>
      void dIntegrate_dq (LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jq) const;

      template <DerivativeProduct side>
      void dIntegrate_dv (LiegroupElementConstRef q, vectorIn_t v, matrixOut_t Jv) const;

      template <bool ApplyOnTheLeft>
      void Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;

      template <DerivativeProduct side>
      void dDifference_dq0 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0) const;

      template <DerivativeProduct side>
      void dDifference_dq1 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const;

      void interpolate (vectorIn_t q0, vectorIn_t q1, value_type u,
          vectorOut_t result) const;

      std::string name () const;

      void mergeVectorSpaces ();

      LiegroupSpacePtr_t vectorSpacesMerged () const;

      bool isVectorSpace () const;

      bool operator== (const LiegroupSpace& other) const;
      bool operator!= (const LiegroupSpace& other) const;

      LiegroupSpacePtr_t operator*= (const LiegroupSpaceConstPtr_t& other);

    protected:

      LiegroupSpace (const size_type& size);
      LiegroupSpace (const LiegroupSpace& other);
      LiegroupSpace (const LiegroupType& type);

    private:
      LiegroupSpace ();
      void init (const LiegroupSpaceWkPtr_t weak);
      void computeSize ();
      void computeNeutral ();
      typedef std::vector <LiegroupType> LiegroupTypes;
      LiegroupTypes liegroupTypes_;
      size_type nq_, nv_;
      vector_t neutral_;
      LiegroupSpaceWkPtr_t weak_;
    }; // class LiegroupSpace
    inline std::ostream& operator<< (std::ostream& os,
                                     const LiegroupSpace& space)
    {
      os << space.name (); return os;
    }

  } // namespace pinocchio
} // namespace hpp

namespace boost {

  hpp::pinocchio::LiegroupSpacePtr_t operator* (
      const hpp::pinocchio::LiegroupSpaceConstPtr_t& sp1,
      const hpp::pinocchio::LiegroupSpaceConstPtr_t& sp2);
  hpp::pinocchio::LiegroupSpacePtr_t operator^
  (const hpp::pinocchio::LiegroupSpaceConstPtr_t& sp,
   hpp::pinocchio::size_type n);
} // namespace boost

#endif // HPP_PINOCCHIO_LIEGROUP_SPACE_HH