distance-between-points-in-bodies.hh

Go to the documentation of this file.

//
// Copyright (c) 2015 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_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH
# define HPP_CONSTRAINTS_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH

# include <hpp/pinocchio/liegroup-element.hh>
# include <hpp/constraints/differentiable-function.hh>

namespace hpp {
  namespace constraints {

    class HPP_CONSTRAINTS_DLLAPI DistanceBetweenPointsInBodies :
      public DifferentiableFunction
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      static DistanceBetweenPointsInBodiesPtr_t create
    (const std::string& name, const DevicePtr_t& robot,
     const JointPtr_t& joint1, const JointPtr_t& joint2,
     const vector3_t& point1, const vector3_t& point2);

      static DistanceBetweenPointsInBodiesPtr_t create
    (const std::string& name, const DevicePtr_t& robot,
     const JointPtr_t& joint1, const vector3_t& point1,
     const vector3_t& point2);

      virtual ~DistanceBetweenPointsInBodies () {}

    protected:
      DistanceBetweenPointsInBodies (const std::string& name,
                     const DevicePtr_t& robot,
                     const JointPtr_t& joint1,
                     const JointPtr_t& joint2,
                     const vector3_t& point1,
                     const vector3_t& point2);

      DistanceBetweenPointsInBodies (const std::string& name,
                     const DevicePtr_t& robot,
                     const JointPtr_t& joint1,
                     const vector3_t& point1,
                     const vector3_t& point2);

      virtual void impl_compute (LiegroupElementRef result,
                 ConfigurationIn_t argument) const;
      virtual void impl_jacobian (matrixOut_t jacobian,
                  ConfigurationIn_t arg) const;
    private:
      DevicePtr_t robot_;
      JointPtr_t joint1_;
      JointPtr_t joint2_;
      vector3_t point1_;
      vector3_t point2_;
      mutable vector3_t global1_;
      mutable vector3_t global2_;
      mutable Configuration_t latestArgument_;
      mutable LiegroupElement latestResult_;
    }; // class DistanceBetweenPointsInBodies
  } // namespace constraints
} // namespace hpp

#endif //HPP_CONSTRAINTS_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH