PlanarManipulatorStateValidityChecker.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, 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 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: Ryan Luna */
36 
37 #ifndef PLANAR_MANIPULATOR_STATE_VALIDITY_CHECKER_H_
38 #define PLANAR_MANIPULATOR_STATE_VALIDITY_CHECKER_H_
39 
40 #include <ompl/base/StateValidityChecker.h>
41 #include <eigen3/Eigen/Dense>
42 #include "PlanarManipulator.h"
43 #include "PlanarManipulatorStateSpace.h"
44 #include "PolyWorld.h"
45 
46 typedef std::pair<Point, Point> Segment;
47 
48 // Returns true if the x and y coordinates of p2 lie between the
49 // x and y coordinates of p0 and p1.
50 bool inBetween(Point p0, Point p1, Point p2)
51 {
52  // p2.first should be between p0.first and p1.first
53  bool goodx;
54  // Check if the x coords are equal first
55  if (cmpDouble(p1.first, p0.first))
56  goodx = cmpDouble(p2.first, p0.first);
57  else
58  {
59  if (p0.first > p1.first)
60  goodx = (p2.first <= p0.first && p2.first >= p1.first);
61  else
62  goodx = (p2.first >= p0.first && p2.first <= p1.first);
63  }
64 
65  bool goody;
66  // Check if the y coords are equal first
67  if (cmpDouble(p1.second, p0.second))
68  goody = cmpDouble(p2.second, p0.second);
69  else
70  {
71  if (p0.second > p1.second)
72  goody = (p2.second <= p0.second && p2.second >= p1.second);
73  else
74  goody = (p2.second >= p0.second && p2.second <= p1.second);
75  }
76 
77  return goodx && goody && !equalPoints(p0, p2) && !equalPoints(p1, p2);
78 }
79 
80 // Return true if the three coordinates are collinear.
81 bool collinearPts(Point p0, Point p1, Point p2)
82 {
83  // Check degenerate infinite slope case
84  if (fabs(p1.first - p0.first) < 1e-6)
85  {
86  bool samex = fabs(p2.first - p1.first) < 1e-6;
87  bool goody;
88  if (p0.second > p1.second)
89  goody = (p2.second <= p0.second && p2.second >= p1.second);
90  else
91  goody = (p2.second >= p0.second && p2.second <= p1.second);
92 
93  return samex && goody;
94  }
95 
96  const double m = (p1.second - p0.second) / (p1.first - p0.first);
97  const double b = p0.second - p0.first * m;
98 
99  // y = mx+b
100  const double y = m * p2.first + b;
101  const bool collinear = fabs(y - p2.second) < 1e-6;
102  return collinear;
103 }
104 
105 // Returns true if the line segments defined by p1-p2 and p3-p4 intersect.
106 bool lineLineIntersection(Point p1, Point p2, Point p3, Point p4)
107 {
108  // There is a degenerate case where one of the endpoints lies on the line
109  // defined by the other two points check this case (all combinations)
110  if (collinearPts(p3, p4, p1) && inBetween(p3, p4, p1))
111  {
112  // p1 lies on the line between p3 and p4
113  return true;
114  }
115 
116  if (collinearPts(p3, p4, p2) && inBetween(p3, p4, p2))
117  {
118  // p2 lies on the line between p3 and p4
119  return true;
120  }
121 
122  if (collinearPts(p1, p2, p3) && inBetween(p1, p2, p3))
123  {
124  // p3 lies on the line between p1 and p2
125  return true;
126  }
127 
128  if (collinearPts(p1, p2, p4) && inBetween(p1, p2, p4))
129  {
130  // p4 lies on the line between p1 and p2
131  return true;
132  }
133 
134  double x1 = p1.first;
135  double x2 = p2.first;
136  double x3 = p3.first;
137  double x4 = p4.first;
138 
139  double y1 = p1.second;
140  double y2 = p2.second;
141  double y3 = p3.second;
142  double y4 = p4.second;
143 
144  // Check if the lines are parallel
145  double check = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
146  if (fabs(check) < 1e-6) // lines are parallel if check == 0
147  return false;
148 
149  // Computing intersection point using determinant method
150  double x = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) /
151  ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4));
152  double y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) /
153  ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4));
154 
155  Point candidate(x, y);
156  // If candidate is between p1-p2 and p3-p4, then we win
157  if (inBetween(p1, p2, candidate) && inBetween(p3, p4, candidate))
158  {
159  // out = candidate;
160  return true;
161  }
162  return false;
163 }
164 
166 {
167 public:
169  const PolyWorld &world)
170  : ompl::base::StateValidityChecker(si), manip_(manip), world_(world)
171  {
172  }
173 
175 
176  virtual bool isValid(const ompl::base::State *state) const
177  {
178  const double *angles = state->as<PlanarManipulatorStateSpace::StateType>()->values;
179  std::vector<Eigen::Affine2d> frames;
180  manip_.FK(angles, frames);
181 
182  // Get the coordinates of the endpoint of each segment and the base
183  // of the manipulator.
184  std::vector<Point> coordinates;
185  const Eigen::Affine2d &baseFrame = manip_.getBaseFrame();
186  coordinates.push_back({baseFrame.translation()[0], baseFrame.translation()[1]});
187  for (size_t i = 0; i < frames.size(); ++i)
188  coordinates.push_back({frames[i].translation()(0), frames[i].translation()(1)});
189 
190  // Check each coordinate to make sure they are in bounds.
191  for (size_t i = 1; i < coordinates.size(); ++i)
192  if (world_.outOfBounds(coordinates[i]))
193  return false;
194 
195  // Check each coordinate for obstacle intersection.
196  for (size_t i = 1; i < coordinates.size(); ++i)
197  for (size_t j = 0; j < world_.numObstacles(); ++j)
198  if (world_.obstacle(j).inside(coordinates[i]))
199  return false;
200 
201  // Self-collision with the manipulator.
202  if (inSelfCollision(coordinates))
203  return false;
204 
205  // Above, we only checked the endpoints of the links of the manipulator.
206  // We can still cut a corner of an obstacle. Check for this.
207  for (size_t i = 0; i < coordinates.size() - 1; ++i)
208  {
209  Point p1 = coordinates[i];
210  Point p2 = coordinates[i + 1];
211 
212  for (size_t j = 0; j < world_.numObstacles(); ++j)
213  {
214  const ConvexPolygon &obstacle = world_.obstacle(j);
215  Point prev = obstacle[obstacle.numPoints() - 1];
216  for (size_t k = 0; k < obstacle.numPoints(); ++k)
217  {
218  if (lineLineIntersection(p1, p2, prev, obstacle[k]))
219  return false;
220  prev = obstacle[k];
221  }
222  }
223  }
224 
225  return true;
226  }
227 
228 private:
229  bool inSelfCollision(const std::vector<Point> &points) const
230  {
231  // a single line cannot intersect with itself
232  if (points.size() < 3)
233  return false;
234 
235  // intersect all pairs of lines
236  for (size_t i = 0; i < points.size() - 1; ++i)
237  for (size_t j = i + 1; j < points.size() - 1; ++j)
238  if (lineLineIntersection(points[i], points[i + 1], points[j], points[j + 1]))
239  {
240  return true;
241  }
242  return false;
243  }
244 
245  const PlanarManipulator &manip_;
246  const PolyWorld &world_;
247 };
248 
249 #endif
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:114
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
virtual bool isValid(const ompl::base::State *state) const
Return true if the state state is valid. Usually, this means at least collision checking....
Abstract definition for a class checking the validity of states. The implementation of this class mus...