Loading...
Searching...
No Matches
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
46typedef 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.
50bool 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.
81bool 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.
106bool 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{
167public:
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
228private:
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
virtual bool isValid(const ompl::base::State *state) const
Return true if the state state is valid. Usually, this means at least collision checking....
A shared pointer wrapper for ompl::base::SpaceInformation.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66