Loading...
Searching...
No Matches
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
40void ompl::base::Constraint::function(const State *state, Eigen::Ref<Eigen::VectorXd> out) const
41{
42 function(*state->as<ConstrainedStateSpace::StateType>(), out);
43}
44
45void ompl::base::Constraint::jacobian(const State *state, Eigen::Ref<Eigen::MatrixXd> out) const
46{
47 jacobian(*state->as<ConstrainedStateSpace::StateType>(), out);
48}
49
50void 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
88bool ompl::base::Constraint::project(State *state) const
89{
90 return project(*state->as<ConstrainedStateSpace::StateType>());
91}
92
93bool 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
114double ompl::base::Constraint::distance(const State *state) const
115{
116 return distance(*state->as<ConstrainedStateSpace::StateType>());
117}
118
119double 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
126bool ompl::base::Constraint::isSatisfied(const State *state) const
127{
128 return isSatisfied(*state->as<ConstrainedStateSpace::StateType>());
129}
130
131bool 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}