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.
47PolyWorld 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.
99PolyWorld 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