.. _program_listing_file_src_ompl_base_Constraint.h:

Program Listing for File Constraint.h
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_base_Constraint.h>` (``src/ompl/base/Constraint.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*********************************************************************
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2014, Rice University
    *  All rights reserved.
    *
    *  Redistribution and use in source and binary forms, with or without
    *  modification, are permitted provided that the following conditions
    *  are met:
    *
    *   * Redistributions of source code must retain the above copyright
    *     notice, this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above
    *     copyright notice, this list of conditions and the following
    *     disclaimer in the documentation and/or other materials provided
    *     with the distribution.
    *   * Neither the name of the Rice University nor the names of its
    *     contributors may be used to endorse or promote products derived
    *     from this software without specific prior written permission.
    *
    *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    *  POSSIBILITY OF SUCH DAMAGE.
    *********************************************************************/
   
   /* Author: Zachary Kingston, Ryan Luna */
   
   #ifndef OMPL_BASE_CONSTRAINTS_CONSTRAINT_
   #define OMPL_BASE_CONSTRAINTS_CONSTRAINT_
   
   #include "ompl/base/StateSpace.h"
   #include "ompl/base/OptimizationObjective.h"
   #include "ompl/util/ClassForward.h"
   #include "ompl/util/Exception.h"
   
   #include <Eigen/Core>
   #include <Eigen/Dense>
   #include <utility>
   
   namespace ompl
   {
       namespace magic
       {
           static const double CONSTRAINT_PROJECTION_TOLERANCE = 1e-4;
   
           static const unsigned int CONSTRAINT_PROJECTION_MAX_ITERATIONS = 50;
       }  // namespace magic
   
       namespace base
       {
   
           OMPL_CLASS_FORWARD(Constraint);
   
           class Constraint
           {
           public:
               Constraint(const unsigned int ambientDim, const unsigned int coDim,
                          double tolerance = magic::CONSTRAINT_PROJECTION_TOLERANCE)
                 : n_(ambientDim)
                 , k_(ambientDim - coDim)
                 , tolerance_(tolerance)
                 , maxIterations_(magic::CONSTRAINT_PROJECTION_MAX_ITERATIONS)
               {
                   if (n_ <= 0 || k_ <= 0)
                       throw ompl::Exception("ompl::base::Constraint(): "
                                             "Ambient and manifold dimensions must be positive.");
               }
   
               virtual ~Constraint() = default;
   
               virtual void function(const State *state, Eigen::Ref<Eigen::VectorXd> out) const;
   
               virtual void function(const Eigen::Ref<const Eigen::VectorXd> &x,
                                     Eigen::Ref<Eigen::VectorXd> out) const = 0;
   
               virtual void jacobian(const State *state, Eigen::Ref<Eigen::MatrixXd> out) const;
   
               virtual void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const;
   
               virtual bool project(State *state) const;
   
               virtual bool project(Eigen::Ref<Eigen::VectorXd> x) const;
   
               virtual double distance(const State *state) const;
   
               virtual double distance(const Eigen::Ref<const Eigen::VectorXd> &x) const;
   
               virtual bool isSatisfied(const State *state) const;
   
               virtual bool isSatisfied(const Eigen::Ref<const Eigen::VectorXd> &x) const;
   
               unsigned int getAmbientDimension() const
               {
                   return n_;
               }
   
               unsigned int getManifoldDimension() const
               {
                   return k_;
               }
   
               unsigned int getCoDimension() const
               {
                   return n_ - k_;
               }
   
               void setManifoldDimension(unsigned int k)
               {
                   if (k <= 0)
                       throw ompl::Exception("ompl::base::Constraint(): "
                                             "Space is over constrained!");
                   k_ = k;
               }
   
               double getTolerance() const
               {
                   return tolerance_;
               }
   
               unsigned int getMaxIterations() const
               {
                   return maxIterations_;
               }
   
               void setTolerance(const double tolerance)
               {
                   if (tolerance <= 0)
                       throw ompl::Exception("ompl::base::Constraint::setProjectionTolerance(): "
                                             "tolerance must be positive.");
                   tolerance_ = tolerance;
               }
   
               void setMaxIterations(const unsigned int iterations)
               {
                   if (iterations == 0)
                       throw ompl::Exception("ompl::base::Constraint::setProjectionMaxIterations(): "
                                             "iterations must be positive.");
                   maxIterations_ = iterations;
               }
   
           protected:
               const unsigned int n_;
   
               unsigned int k_;
   
               double tolerance_;
   
               unsigned int maxIterations_;
           };
   
           OMPL_CLASS_FORWARD(ConstraintIntersection);
   
           class ConstraintIntersection : public Constraint
           {
           public:
               ConstraintIntersection(const unsigned int ambientDim, std::vector<ConstraintPtr> constraints)
                 : Constraint(ambientDim, 0)
               {
                   for (const auto &constraint : constraints)
                       addConstraint(constraint);
               }
   
               ConstraintIntersection(const unsigned int ambientDim, std::initializer_list<ConstraintPtr> constraints)
                 : Constraint(ambientDim, 0)
               {
                   for (const auto &constraint : constraints)
                       addConstraint(constraint);
               }
   
               void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
               {
                   unsigned int i = 0;
                   for (const auto &constraint : constraints_)
                   {
                       constraint->function(x, out.segment(i, constraint->getCoDimension()));
                       i += constraint->getCoDimension();
                   }
               }
   
               void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
               {
                   unsigned int i = 0;
                   for (const auto &constraint : constraints_)
                   {
                       constraint->jacobian(x, out.block(i, 0, constraint->getCoDimension(), n_));
                       i += constraint->getCoDimension();
                   }
               }
   
           protected:
               void addConstraint(const ConstraintPtr &constraint)
               {
                   setManifoldDimension(k_ - constraint->getCoDimension());
                   constraints_.push_back(constraint);
               }
   
               std::vector<ConstraintPtr> constraints_;
           };
   
           OMPL_CLASS_FORWARD(ConstraintObjective);
   
           class ConstraintObjective : public OptimizationObjective
           {
           public:
               ConstraintObjective(ConstraintPtr constraint, SpaceInformationPtr si)
                 : OptimizationObjective(std::move(si)), constraint_(std::move(constraint))
               {
               }
   
               Cost stateCost(const State *s) const override
               {
                   return Cost(constraint_->distance(s));
               }
   
           protected:
               ConstraintPtr constraint_;
           };
       }  // namespace base
   }  // namespace ompl
   
   #endif