convex-shape-contact.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// 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_CONVEX_SHAPE_CONTACT_HH
# define HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH

# include <vector>

# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/deprecated.hh>
# include <hpp/constraints/generic-transformation.hh>
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/convex-shape.hh>

namespace hpp {
  namespace constraints {


    class HPP_CONSTRAINTS_DLLAPI ConvexShapeContact :
      public DifferentiableFunction {
      public:
        friend class ConvexShapeContactComplement;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        enum ContactType {
          POINT_ON_PLANE,
          LINE_ON_PLANE,
          PLANE_ON_PLANE
        };

        struct ForceData {
          JointPtr_t joint;
          JointPtr_t supportJoint;
          std::vector<vector3_t> points;
          vector3_t normal;
        };

        ConvexShapeContact (const std::string& name,
                const DevicePtr_t& robot);

        static ConvexShapeContactPtr_t create (
            const std::string& name,
            const DevicePtr_t& robot);

        static ConvexShapeContactPtr_t create (
            const DevicePtr_t& robot);

        void addObjectTriangle (const fcl::TriangleP& t,
                const JointPtr_t& joint)
          HPP_CONSTRAINTS_DEPRECATED;

        void addFloorTriangle (const fcl::TriangleP& t,
                   const JointPtr_t& joint)
          HPP_CONSTRAINTS_DEPRECATED;

        void addObject (const ConvexShape& t);

        void addFloor (const ConvexShape& t);

        void setNormalMargin (const value_type& margin);

        std::vector <ForceData> computeContactPoints (ConfigurationIn_t q,
            const value_type& normalMargin) const;

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

      private:
        void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument)
          const;
        void computeInternalValue (const ConfigurationIn_t& argument,
            bool& isInside, ContactType& type, vector6_t& value) const;

        void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t argument) const;
        void computeInternalJacobian (const ConfigurationIn_t& argument,
            bool& isInside, ContactType& type, matrix_t& jacobian) const;

        typedef std::vector <ConvexShape> ConvexShapes_t;
        bool selectConvexShapes (const pinocchio::DeviceData& data,
            ConvexShapes_t::const_iterator& object,
            ConvexShapes_t::const_iterator& floor) const;
        ContactType contactType (const ConvexShape& object,
            const ConvexShape& floor) const;

        DevicePtr_t robot_;
        mutable GenericTransformationModel<true> relativeTransformationModel_;

        ConvexShapes_t objectConvexShapes_;
        ConvexShapes_t floorConvexShapes_;

        value_type normalMargin_;
    };

    class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactComplement :
      public DifferentiableFunction
    {
    public:
      static std::pair <ConvexShapeContactPtr_t,
            ConvexShapeContactComplementPtr_t >
    createPair (const std::string& name, const std::string& complementName,
            const DevicePtr_t& robot);

    protected:
      ConvexShapeContactComplement (const std::string& name,
                    const std::string& complementName,
                    const DevicePtr_t& robot);


    private:
      void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument) const;

      void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t argument)
    const;

      ConvexShapeContactPtr_t sibling_;
    }; // class ConvexShapeContactComplement
  } // namespace constraints
} // namespace hpp

#endif //  HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH