Constraint.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Zachary Kingston */
36 
37 #include "ompl/base/Constraint.h"
38 #include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
39 
40 void ompl::base::Constraint::function(const State *state, Eigen::Ref<Eigen::VectorXd> out) const
41 {
42  function(*state->as<ConstrainedStateSpace::StateType>(), out);
43 }
44 
45 void ompl::base::Constraint::jacobian(const State *state, Eigen::Ref<Eigen::MatrixXd> out) const
46 {
47  jacobian(*state->as<ConstrainedStateSpace::StateType>(), out);
48 }
49 
50 void ompl::base::Constraint::jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const
51 {
52  Eigen::VectorXd y1 = x;
53  Eigen::VectorXd y2 = x;
54  Eigen::VectorXd t1(getCoDimension());
55  Eigen::VectorXd t2(getCoDimension());
56 
57  // Use a 7-point central difference stencil on each column.
58  for (std::size_t j = 0; j < n_; j++)
59  {
60  const double ax = std::fabs(x[j]);
61  // Make step size as small as possible while still giving usable accuracy.
62  const double h = std::sqrt(std::numeric_limits<double>::epsilon()) * (ax >= 1 ? ax : 1);
63 
64  // Can't assume y1[j]-y2[j] == 2*h because of precision errors.
65  y1[j] += h;
66  y2[j] -= h;
67  function(y1, t1);
68  function(y2, t2);
69  const Eigen::VectorXd m1 = (t1 - t2) / (y1[j] - y2[j]);
70  y1[j] += h;
71  y2[j] -= h;
72  function(y1, t1);
73  function(y2, t2);
74  const Eigen::VectorXd m2 = (t1 - t2) / (y1[j] - y2[j]);
75  y1[j] += h;
76  y2[j] -= h;
77  function(y1, t1);
78  function(y2, t2);
79  const Eigen::VectorXd m3 = (t1 - t2) / (y1[j] - y2[j]);
80 
81  out.col(j) = 1.5 * m1 - 0.6 * m2 + 0.1 * m3;
82 
83  // Reset for next iteration.
84  y1[j] = y2[j] = x[j];
85  }
86 }
87 
89 {
90  return project(*state->as<ConstrainedStateSpace::StateType>());
91 }
92 
93 bool ompl::base::Constraint::project(Eigen::Ref<Eigen::VectorXd> x) const
94 {
95  // Newton's method
96  unsigned int iter = 0;
97  double norm = 0;
98  Eigen::VectorXd f(getCoDimension());
99  Eigen::MatrixXd j(getCoDimension(), n_);
100 
101  const double squaredTolerance = tolerance_ * tolerance_;
102 
103  function(x, f);
104  while ((norm = f.squaredNorm()) > squaredTolerance && iter++ < maxIterations_)
105  {
106  jacobian(x, j);
107  x -= j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
108  function(x, f);
109  }
110 
111  return norm < squaredTolerance;
112 }
113 
114 double ompl::base::Constraint::distance(const State *state) const
115 {
116  return distance(*state->as<ConstrainedStateSpace::StateType>());
117 }
118 
119 double ompl::base::Constraint::distance(const Eigen::Ref<const Eigen::VectorXd> &x) const
120 {
121  Eigen::VectorXd f(getCoDimension());
122  function(x, f);
123  return f.norm();
124 }
125 
127 {
128  return isSatisfied(*state->as<ConstrainedStateSpace::StateType>());
129 }
130 
131 bool ompl::base::Constraint::isSatisfied(const Eigen::Ref<const Eigen::VectorXd> &x) const
132 {
133  Eigen::VectorXd f(getCoDimension());
134  function(x, f);
135  return f.allFinite() && f.squaredNorm() <= tolerance_ * tolerance_;
136 }
virtual void function(const State *state, Eigen::Ref< Eigen::VectorXd > out) const
Compute the constraint function at state. Result is returned in out, which should be allocated to siz...
Definition: Constraint.cpp:40
Definition of an abstract state.
Definition: State.h:113
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
virtual bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found,...
Definition: Constraint.cpp:88
virtual void jacobian(const State *state, Eigen::Ref< Eigen::MatrixXd > out) const
Compute the Jacobian of the constraint function at state. Result is returned in out,...
Definition: Constraint.cpp:45
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
virtual bool isSatisfied(const State *state) const
Check whether a state state satisfies the constraints.
Definition: Constraint.cpp:126
virtual double distance(const State *state) const
Returns the distance of state to the constraint manifold.
Definition: Constraint.cpp:114