PlanarManipulatorPolyWorld.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_POLY_WORLD_H_
38 #define PLANAR_MANIPULATOR_POLY_WORLD_H_
39 
40 #include "PolyWorld.h"
41 #include <eigen3/Eigen/Dense>
42 #include <cmath>
43 #include <vector>
44 
45 // Create the 'corridor' environment for a unit-length planar manipulator with
46 // n links. The chain must navigate a narrow gap.
47 PolyWorld createCorridorProblem(int n, Eigen::Affine2d &basePose, Eigen::Affine2d &goalPose)
48 {
49  // The dimensions of the world.
50  const double minX = 0;
51  const double minY = 0;
52  const double maxX = 1.25;
53  const double maxY = 1.25;
54  PolyWorld world("corridor", {minX, maxX}, {minY, maxY});
55 
56  const double len = 1.0 / n;
57  const double gap = len * (1.1 * M_PI + log(n) / (double)n);
58 
59  std::vector<Point> pts(4);
60 
61  // two obstacles with a gap in the middle
62  const double w = ((maxX - minX) * 0.50) - gap;
63  const double h1 = minY + gap;
64  const double h2 = maxY - gap;
65 
66  pts[0] = std::make_pair(minX, h1);
67  pts[1] = std::make_pair(minX + w, h1);
68  pts[2] = std::make_pair(minX + w, h2);
69  pts[3] = std::make_pair(minX, h2);
70  const ConvexPolygon left_obj(pts);
71 
72  pts[0] = std::make_pair(minX + w + gap, h1);
73  pts[1] = std::make_pair(maxX, h1);
74  pts[2] = std::make_pair(maxX, h2);
75  pts[3] = std::make_pair(minX + w + gap, h2);
76  const ConvexPolygon right_obj(pts);
77 
78  world.addObstacle(left_obj);
79  world.addObstacle(right_obj);
80 
81  // Set the base frame for the manipulator.
82  basePose = Eigen::Affine2d::Identity();
83  basePose.translation()(1) = gap / 2.0; // set the y coordinate of the base.
84 
85  // Set the goal frame for the end effector.
86  goalPose = Eigen::Affine2d::Identity();
87  goalPose.translation()(0) = (w + gap) * 0.95;
88  goalPose.translation()(1) = std::min(4.0 * gap, 0.50);
89  goalPose.rotate(0.0);
90 
91  return world;
92 }
93 
94 // Create the 'constricted' environment for a planar manipulator with n links
95 // and unit-length.
96 // The first half of the chain lies in a narrow gap between two obstacles,
97 // whereas the second half of the chain is in a fairly open environment.
98 // Returns the position of the base of the chain.
99 PolyWorld createConstrictedProblem(int n, Eigen::Affine2d &basePose, Eigen::Affine2d &goalPose)
100 {
101  const double minX = 0;
102  const double minY = 0;
103  const double maxX = 1.25;
104  const double maxY = 1.25;
105  PolyWorld world("constricted", {minX, maxX}, {minY, maxY});
106 
107  const double gap = (log10(n) / (double)n) * 2.0;
108 
109  // Make the interior gap small enough so that it is difficult for the
110  // chain to double back on itself on the first half.
111  const double inner_gap = 1.0 / n;
112 
113  // robot base at (0, 0.5), chain has unit length.
114  const double robot_y = 0.5;
115  const double ymax = 1.25;
116 
117  std::vector<Point> pts(4);
118 
119  // lower corridor obstacle
120  const double lower_interior_y = robot_y - inner_gap;
121  const double upper_interior_y = robot_y + inner_gap;
122 
123  {
124  pts[0] = std::make_pair(0, 0);
125  pts[1] = std::make_pair(0.5, 0);
126  pts[2] = std::make_pair(0.5, lower_interior_y);
127  pts[3] = std::make_pair(0, lower_interior_y);
128  const ConvexPolygon lower(pts);
129  world.addObstacle(lower);
130  }
131 
132  // upper corridor obstacle
133  {
134  pts[0] = std::make_pair(0, upper_interior_y);
135  pts[1] = std::make_pair(0.5, upper_interior_y);
136  pts[2] = std::make_pair(0.5, ymax);
137  pts[3] = std::make_pair(0, ymax);
138  const ConvexPolygon upper(pts);
139  world.addObstacle(upper);
140  }
141 
142  // The obstacle separating the start and goal configurations.
143  {
144  pts[0] = std::make_pair(0.5 + gap, 0.5 + (gap / 2.0));
145  pts[1] = std::make_pair(1.0, 0.5 + (gap / 2.0));
146  pts[2] = std::make_pair(1.0, 0.5 + gap);
147  pts[3] = std::make_pair(0.5 + gap, 0.5 + gap);
148  const ConvexPolygon right(pts);
149  world.addObstacle(right);
150  }
151 
152  basePose = Eigen::Affine2d::Identity();
153  basePose.translation()(1) = robot_y;
154 
155  goalPose = Eigen::Affine2d::Identity();
156  goalPose.translation()(0) = 0.75;
157  goalPose.translation()(1) = 0.5 + 1.5 * gap;
158  goalPose.rotate(0.0);
159 
160  return world;
161 }
162 
163 #endif
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120