relative-pose.hh

Go to the documentation of this file.

// Copyright (c) 2018, LAAS-CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-constraints.
// hpp-constraints 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-constraints 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-constraints. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH
# define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH

# include <hpp/constraints/explicit.hh>
# include <hpp/constraints/implicit/relative-pose.hh>
# include <pinocchio/spatial/se3.hpp>

namespace hpp {
  namespace constraints {
    namespace explicit_ {
      class HPP_CONSTRAINTS_DLLAPI RelativePose :
        public Explicit, public implicit::RelativePose
      {
      public:
        virtual ImplicitPtr_t copy () const;
        static RelativePosePtr_t create
          (const std::string& name, const DevicePtr_t& robot,
           const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
           const Transform3f& frame1, const Transform3f& frame2,
           std::vector <bool> mask = std::vector<bool>(6,true),
           ComparisonTypes_t comp = std::vector <ComparisonType> (),
           vectorIn_t rhs = vector_t ());

        static RelativePosePtr_t createCopy (const RelativePosePtr_t& other);

        virtual void implicitToExplicitRhs (vectorIn_t implicitRhs,
                                            vectorOut_t explicitRhs) const;
        virtual void explicitToImplicitRhs (vectorIn_t explicitRhs,
                                            vectorOut_t implicitRhs) const;
      protected:
        RelativePose (const std::string& name, const DevicePtr_t& robot,
                      const JointConstPtr_t& joint1,
                      const JointConstPtr_t& joint2,
                      const Transform3f& frame1, const Transform3f& frame2,
                      std::vector <bool> mask = std::vector<bool>(6,true),
                      ComparisonTypes_t comp = std::vector <ComparisonType> (),
                      vectorIn_t rhs = vector_t ());

        RelativePose (const RelativePose& other);

        void init (RelativePoseWkPtr_t weak);
      private:
        // Create LiegroupSpace instances to avoid useless allocation.
        static LiegroupSpacePtr_t SE3;
        static LiegroupSpacePtr_t R3xSO3;
        Transform3f frame1_;
        Transform3f frame2_;
        RelativePoseWkPtr_t weak_;
      }; // class RelativePose
    } // namespace explicit_
  } // namespace constraints
} // namespace hpp
#endif //HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH