generic-transformation.hh

Go to the documentation of this file.

//
// Copyright (c) 2016 CNRS
// Authors: Joseph Mirabel
//
//
// 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_GENERIC_TRANSFORMATION_HH
# define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH

# include <pinocchio/spatial/se3.hpp>

# include <hpp/pinocchio/joint.hh>

# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/matrix-view.hh>

namespace hpp {
  namespace constraints {
    template <bool rel> struct GenericTransformationModel
    {
      JointConstPtr_t joint2;
      bool R1isID, R2isID, t1isZero, t2isZero;
      Transform3f F1inJ1, F2inJ2;
      bool fullPos, fullOri;
      size_type rowOri;
      const size_type cols;

      inline JointConstPtr_t getJoint1() const { return JointConstPtr_t(); }
      inline void setJoint1(const JointConstPtr_t&) {}
      GenericTransformationModel (const size_type nCols) :
        joint2(), R1isID(true), R2isID(true), t1isZero(true), t2isZero(true),
        fullPos(false), fullOri(false), cols (nCols)
      { F1inJ1.setIdentity(); F2inJ2.setIdentity(); }
      void checkIsIdentity1() {
        R1isID = F1inJ1.rotation().isIdentity();
        t1isZero = F1inJ1.translation().isZero();
      }
      void checkIsIdentity2() {
        R2isID = F2inJ2.rotation().isIdentity();
        t2isZero = F2inJ2.translation().isZero();
      }
    };
    template <> struct GenericTransformationModel<true> :
      GenericTransformationModel<false>
    {
      JointConstPtr_t joint1;
      inline JointConstPtr_t getJoint1() const { return joint1; }
      void setJoint1(const JointConstPtr_t& j);
      GenericTransformationModel (const size_type nCols) :
        GenericTransformationModel<false>(nCols), joint1() {}
    };


    template <int _Options>
    class HPP_CONSTRAINTS_DLLAPI GenericTransformation :
      public DifferentiableFunction
    {
    public:
      typedef boost::shared_ptr <GenericTransformation> Ptr_t;
      typedef boost::weak_ptr <GenericTransformation> WkPtr_t;
#if __cplusplus >= 201103L
      static constexpr bool
        IsRelative         = _Options & RelativeBit,
        ComputeOrientation = _Options & OrientationBit,
        ComputePosition    = _Options & PositionBit,
        OutputSE3          = _Options & OutputSE3Bit,
        IsPosition         = ComputePosition  && !ComputeOrientation,
        IsOrientation      = !ComputePosition && ComputeOrientation,
        IsTransform        = ComputePosition  && ComputeOrientation;
      static constexpr int
        ValueSize          = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
        DerSize            = (ComputePosition?3:0) + (ComputeOrientation ?3:0);
#else
      enum {
        IsRelative         = _Options & RelativeBit,
        ComputeOrientation = _Options & OrientationBit,
        ComputePosition    = _Options & PositionBit,
        OutputSE3          = _Options & OutputSE3Bit,
        IsPosition         = ComputePosition  && !ComputeOrientation,
        IsOrientation      = !ComputePosition && ComputeOrientation,
        IsTransform        = ComputePosition  && ComputeOrientation,
        ValueSize          = (ComputePosition?3:0) + (ComputeOrientation?(OutputSE3?4:3):0),
        DerSize            = (ComputePosition?3:0) + (ComputeOrientation ?3:0)
        };
#endif

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
          const JointConstPtr_t& joint2, const Transform3f& reference,
          std::vector <bool> mask = std::vector<bool>(DerSize,true));

      static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
          /* World frame          */ const JointConstPtr_t& joint2,
          const Transform3f& frame2, const Transform3f& frame1,
         std::vector <bool> mask = std::vector<bool>(DerSize,true));

      static Ptr_t create (const std::string& name, const DevicePtr_t& robot,
          const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
          const Transform3f& reference,
          std::vector <bool> mask = std::vector<bool>(DerSize,true));

      static Ptr_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>(DerSize,true));

      virtual ~GenericTransformation () {}

      inline void reference (const Transform3f& reference)
      {
    m_.F1inJ1 = reference;
        m_.checkIsIdentity1();
    m_.F2inJ2.setIdentity ();
        m_.checkIsIdentity2();
      }

      inline Transform3f reference () const
      {
    return m_.F1inJ1.actInv(m_.F2inJ2);
      }

      inline void joint1 (const JointConstPtr_t& joint) {
        // static_assert(IsRelative);
    m_.setJoint1(joint);
        computeActiveParams();
    assert (!joint || joint->robot () == robot_);
      }

      inline JointConstPtr_t joint1 () const {
    return m_.getJoint1();
      }

      inline void joint2 (const JointConstPtr_t& joint) {
    m_.joint2 = joint;
        computeActiveParams();
    assert (!joint || (joint->index() > 0 && joint->robot () == robot_));
      }

      inline JointConstPtr_t joint2 () const {
    return m_.joint2;
      }

      inline void frame1InJoint1 (const Transform3f& M) {
    m_.F1inJ1 = M;
        m_.checkIsIdentity1();
      }
      inline const Transform3f& frame1InJoint1 () const {
    return m_.F1inJ1;
      }
      inline void frame2InJoint2 (const Transform3f& M) {
    m_.F2inJ2 = M;
        m_.checkIsIdentity2();
      }
      inline const Transform3f& frame2InJoint2 () const {
    return m_.F2inJ2;
      }

      virtual std::ostream& print (std::ostream& o) const;

      GenericTransformation (const std::string& name,
                              const DevicePtr_t& robot,
                              std::vector <bool> mask);

    protected:
      void init (const WkPtr_t& self)
      {
        self_ = self;
        computeActiveParams();
      }

      virtual void impl_compute (LiegroupElementRef result,
                 ConfigurationIn_t argument) const;
      virtual void impl_jacobian (matrixOut_t jacobian,
                  ConfigurationIn_t arg) const;
    private:
      void computeActiveParams ();
      DevicePtr_t robot_;
      GenericTransformationModel<IsRelative> m_;
      Eigen::RowBlockIndices Vindices_;
      const std::vector <bool> mask_;
      WkPtr_t self_;
    }; // class GenericTransformation
  } // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH