graph-path-validation.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-manipulation.
// hpp-manipulation 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-manipulation 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-manipulation. If not, see <http://www.gnu.org/licenses/>.


#ifndef HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH
# define HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH

# include <hpp/core/path-validation.hh>
# include <hpp/core/obstacle-user.hh>

# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/graph/fwd.hh>

namespace hpp {
  namespace manipulation {
    using hpp::core::PathValidation;
    using hpp::core::PathValidationPtr_t;
    using hpp::core::Path;
    using hpp::core::PathPtr_t;
    using hpp::core::PathVector;
    using hpp::core::PathVectorPtr_t;


    class HPP_MANIPULATION_DLLAPI GraphPathValidation :
      public PathValidation,
      public core::ObstacleUserInterface
    {
      public:
    virtual bool validate (const PathPtr_t& path, bool reverse,
                   PathPtr_t& validPart,
                   PathValidationReportPtr_t& report);

        void innerValidation (const PathValidationPtr_t& pathValidation)
        {
          pathValidation_ = pathValidation;
        }

        const PathValidationPtr_t& innerValidation ()
        {
          return pathValidation_;
        }

        void constraintGraph (const graph::GraphPtr_t& graph)
        {
          constraintGraph_ = graph;
        }

        graph::GraphPtr_t constraintGraph () const
        {
          return constraintGraph_;
        }

        //         checking.
        //  \param graph A pointer to the constraint graph.
        static GraphPathValidationPtr_t create (
            const PathValidationPtr_t& pathValidation);

        template <typename T>
          static GraphPathValidationPtr_t create (
              const pinocchio::DevicePtr_t& robot, const value_type& stepSize);

        void addObstacle (const hpp::core::CollisionObjectConstPtr_t& object)
        {
          boost::shared_ptr<core::ObstacleUserInterface> oui =
            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
          if (oui) oui->addObstacle (object);
        }

        void removeObstacleFromJoint (const JointPtr_t& joint,
            const pinocchio::CollisionObjectConstPtr_t& obstacle)
        {
          boost::shared_ptr<core::ObstacleUserInterface> oui =
            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
          if (oui) oui->removeObstacleFromJoint (joint, obstacle);
        }

        void filterCollisionPairs (const core::RelativeMotion::matrix_type& relMotion)
        {
          boost::shared_ptr<core::ObstacleUserInterface> oui =
            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
          if (oui) oui->filterCollisionPairs (relMotion);
        }

        void setSecurityMargins(const matrix_t& securityMargins)
        {
          boost::shared_ptr<core::ObstacleUserInterface> oui =
            HPP_DYNAMIC_PTR_CAST(core::ObstacleUserInterface, pathValidation_);
          if (oui) oui->setSecurityMargins(securityMargins);
        }

      protected:
        GraphPathValidation (const PathValidationPtr_t& pathValidation);

      private:
        bool impl_validate (const PathVectorPtr_t& path, bool reverse,
            PathPtr_t& validPart, PathValidationReportPtr_t& report);
        bool impl_validate (const PathPtr_t& path, bool reverse,
            PathPtr_t& validPart, PathValidationReportPtr_t& report);
        PathValidationPtr_t pathValidation_;
        graph::GraphPtr_t constraintGraph_;
    };

    template <typename T>
      GraphPathValidationPtr_t GraphPathValidation::create
      (const pinocchio::DevicePtr_t& robot, const value_type& stepSize)
    {
      return GraphPathValidation::create (T::create (robot, stepSize));
    }
  } // namespace manipulation
} // namespace hpp

#endif // HPP_MANIPULATION_GRAPHPATHVALIDATOR_HH