Loading...
Searching...
No Matches
ConstrainedPlanningImplicitChain.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38from __future__ import print_function
39import argparse
40import math
41from functools import partial
42import numpy as np
43from ConstrainedPlanningCommon import *
44
45
46def normalize(x):
47 norm = np.linalg.norm(x)
48 if norm > 0 and np.isfinite(norm):
49 return x / norm
50 else:
51 return x
52
53
55
56 class Wall(object):
57 # Container class for the "wall" obstacles that are on the surface of the
58 # sphere constraint (when extra = 1).
59
60 def __init__(self, offset, thickness, width, joint_radius, wallType):
61 self.offset = offset
62 self.thickness = thickness + joint_radius
63 self.width = width + joint_radius
64 self.type = wallType
65
66 # Checks if an x coordinate places the sphere within the boundary of
67 # the wall.
68 def within(self, x):
69 if x < (self.offset - self.thickness) or x > (self.offset + self.thickness):
70 return False
71 return True
72
73 def checkJoint(self, v):
74 x, y, z = v
75
76 if not self.within(x):
77 return True
78
79 if z <= self.width:
80 if self.type == 0:
81 if y < 0:
82 return True
83 else:
84 if y > 0:
85 return True
86 return False
87
88 WALL_WIDTH = 0.5
89 JOINT_RADIUS = 0.2
90 LINK_LENGTH = 1.0
91
92 # An implicit kinematic chain, formed out of balls each in R^3, with
93 # distance constraints between successive balls creating spherical joint
94 # kinematics for the system.
95 #
96 # Extra constraints are as follows:
97 # 1 - End-effector is constrained to be on the surface of a sphere of
98 # radius links - 2
99 # 2 - The (links - 5)th and (links - 4)th ball have the same z-value
100 # 3 - The (links - 4)th and (links - 3)th ball have the same x-value
101 # 4 - The (links - 3)th and (links - 2)th ball have the same z-value
102 def __init__(self, links, obstacles=0, extra=1):
103 super(ChainConstraint, self).__init__(3 * links, links + extra)
104 self.links = links
105 self.length = ChainConstraint.LINK_LENGTH
106 self.width = ChainConstraint.WALL_WIDTH
107 self.radius = links - 2
108 self.jointRadius = ChainConstraint.JOINT_RADIUS
109 self.obstacles = obstacles
110 self.extra = extra
111 step = 2. * self.radius / (obstacles + 1.)
112 self.walls = [ChainConstraint.Wall(-self.radius + i * step, self.radius / 8.,
113 self.width, self.jointRadius, i % 2)
114 for i in range(obstacles)]
115
116 def function(self, x, out):
117 joint1 = np.zeros(3)
118 for i in range(self.links):
119 joint2 = x[(3 * i):(3 * i + 3)]
120 out[i] = np.linalg.norm(joint1 - joint2) - self.length
121 joint1 = joint2
122
123 if self.extra >= 1:
124 out[self.links] = np.linalg.norm(x[-3:]) - self.radius
125
126 o = self.links - 5
127
128 if self.extra >= 2:
129 out[self.links + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2]
130 if self.extra >= 3:
131 out[self.links + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0]
132 if self.extra >= 4:
133 out[self.links + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2]
134
135 def jacobian(self, x, out):
136 out[:, :] = np.zeros(
137 (self.getCoDimension(), self.getAmbientDimension()), dtype=np.float64)
138
139 plus = np.zeros(3 * (self.links + 1))
140 plus[:(3 * self.links)] = x[:(3 * self.links)]
141
142 minus = np.zeros(3 * (self.links + 1))
143 minus[-(3 * self.links):] = x[:(3 * self.links)]
144
145 diagonal = plus - minus
146
147 for i in range(self.links):
148 out[i, (3 * i):(3 * i + 3)] = normalize(diagonal[(3 * i):(3 * i + 3)])
149 out[1:self.links, 0:(3 * self.links - 3)
150 ] -= out[1:self.links, 3:(3 * self.links)]
151
152 if self.extra >= 1:
153 out[self.links, -3:] = -normalize(diagonal[-3:])
154
155 o = self.links - 5
156
157 if self.extra >= 2:
158 out[self.links + 1, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
159 if self.extra >= 3:
160 out[self.links + 2, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
161 if self.extra >= 4:
162 out[self.links + 3, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
163
164 # Checks if there are no self-collisions (of the joints themselves) or
165 # collisions with the extra obstacles on the surface of the sphere.
166 def isValid(self, state):
167 x = np.array([state[i] for i in range(self.getAmbientDimension())])
168 for i in range(self.links):
169 joint = x[(3 * i):(3 * i + 3)]
170 if joint[2] < 0:
171 return False
172 if np.linalg.norm(joint) >= self.radius - self.jointRadius:
173 for wall in self.walls:
174 if not wall.checkJoint(joint):
175 return False
176
177 for i in range(self.links):
178 joint1 = x[(3 * i):(3 * i + 3)]
179 if np.max(np.absolute(joint1)) < self.jointRadius:
180 return False
181
182 for j in range(i + 1, self.links):
183 joint2 = x[(3 * j):(3 * j + 3)]
184 if np.max(np.absolute(joint1 - joint2)) < self.jointRadius:
185 return False
186
187 return True
188
189 def createSpace(self):
190 rvss = ob.RealVectorStateSpace(3 * self.links)
191 bounds = ob.RealVectorBounds(3 * self.links)
192
193 for i in range(self.links):
194 bounds.setLow(3 * i + 0, -i - 1)
195 bounds.setHigh(3 * i + 0, i + 1)
196
197 bounds.setLow(3 * i + 1, -i - 1)
198 bounds.setHigh(3 * i + 1, i + 1)
199
200 bounds.setLow(3 * i + 2, -i - 1)
201 bounds.setHigh(3 * i + 2, i + 1)
202
203 rvss.setBounds(bounds)
204 return rvss
205
206 def getStartAndGoalStates(self):
207 start = np.zeros(3 * self.links)
208 goal = np.zeros(3 * self.links)
209
210 for i in range(self.links - 3):
211 start[3 * i] = i + 1
212 start[3 * i + 1] = 0
213 start[3 * i + 2] = 0
214
215 goal[3 * i] = -(i + 1)
216 goal[3 * i + 1] = 0
217 goal[3 * i + 2] = 0
218
219 i = self.links - 3
220
221 start[3 * i] = i
222 start[3 * i + 1] = -1
223 start[3 * i + 2] = 0
224
225 goal[3 * i] = -i
226 goal[3 * i + 1] = 1
227 goal[3 * i + 2] = 0
228
229 i += 1
230
231 start[3 * i] = i
232 start[3 * i + 1] = -1
233 start[3 * i + 2] = 0
234
235 goal[3 * i] = -i
236 goal[3 * i + 1] = 1
237 goal[3 * i + 2] = 0
238
239 i += 1
240
241 start[3 * i] = i - 1
242 start[3 * i + 1] = 0
243 start[3 * i + 2] = 0
244
245 goal[3 * i] = -(i - 1)
246 goal[3 * i + 1] = 0
247 goal[3 * i + 2] = 0
248
249 return start, goal
250
251 # Create a projection evaluator for the chain constraint. Finds the
252 # spherical coordinates of the end-effector on the surface of the sphere of
253 # radius equal to that of the constraint (when extra = 1). */
254 def getProjection(self, space):
255 class ChainProjection(ob.ProjectionEvaluator):
256
257 def __init__(self, space, links, radius):
258 super(ChainProjection, self).__init__(space)
259 self.links = links # Number of chain links.
260 # Radius of sphere end-effector lies on (for extra = 1)
261 self.radius = radius
262 self.defaultCellSizes()
263
264 def getDimension(self):
265 return 2
266
267 def defaultCellSizes(self):
268 self.cellSizes_ = list2vec([.1, .1])
269
270 def project(self, state, projection):
271 s = 3 * (self.links - 1)
272 projection[0] = math.atan2(state[s + 1], state[s])
273 projection[1] = math.acos(state[s + 2] / self.radius)
274
275 return ChainProjection(space, self.links, self.radius)
276
277 def dump(self, outfile):
278 print(self.links, file=outfile)
279 print(self.obstacles, file=outfile)
280 print(self.extra, file=outfile)
281 print(self.jointRadius, file=outfile)
282 print(self.length, file=outfile)
283 print(self.radius, file=outfile)
284 print(self.width, file=outfile)
285
286 def addBenchmarkParameters(self, bench):
287 bench.addExperimentParameter("links", "INTEGER", str(self.links))
288 bench.addExperimentParameter(
289 "obstacles", "INTEGER", str(self.obstacles))
290 bench.addExperimentParameter("extra", "INTEGER", str(self.extra))
291
292
293def chainPlanningOnce(cp, planner, output):
294 cp.setPlanner(planner, "chain")
295
296 # Solve the problem
297 stat = cp.solveOnce(output, "chain")
298
299 if output:
300 ou.OMPL_INFORM("Dumping problem information to `chain_info.txt`.")
301 with open("chain_info.txt", "w") as infofile:
302 print(cp.spaceType, file=infofile)
303 cp.constraint.dump(infofile)
304
305 cp.atlasStats()
306 return stat
307
308
309def chainPlanningBench(cp, planners):
310 cp.setupBenchmark(planners, "chain")
311 cp.constraint.addBenchmarkParameters(cp.bench)
312 cp.runBenchmark()
313
314
315def chainPlanning(options):
316 # Create our constraint.
317 constraint = ChainConstraint(
318 options.links, options.obstacles, options.extra)
319
321 options.space, constraint.createSpace(), constraint, options)
322
323 cp.css.registerProjection("chain", constraint.getProjection(cp.css))
324
325 start, goal = constraint.getStartAndGoalStates()
326 sstart = ob.State(cp.css)
327 sgoal = ob.State(cp.css)
328 for i in range(cp.css.getDimension()):
329 sstart[i] = start[i]
330 sgoal[i] = goal[i]
331 cp.setStartAndGoalStates(sstart, sgoal)
332 cp.ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(
333 ChainConstraint.isValid, constraint)))
334
335 planners = options.planner.split(",")
336 if not options.bench:
337 chainPlanningOnce(cp, planners[0], options.output)
338 else:
339 chainPlanningBench(cp, planners)
340
341
342if __name__ == "__main__":
343 parser = argparse.ArgumentParser()
344 parser.add_argument("-o", "--output", action="store_true",
345 help="Dump found solution path (if one exists) in plain text and planning "
346 "graph in GraphML to `torus_path.txt` and `torus_graph.graphml` "
347 "respectively.")
348 parser.add_argument("--bench", action="store_true",
349 help="Do benchmarking on provided planner list.")
350 parser.add_argument("-l", "--links", type=int, default=5,
351 help="Number of links in the kinematic chain. Minimum is 4.")
352 parser.add_argument("-x", "--obstacles", type=int, default=0, choices=[0, 1, 2],
353 help="Number of `wall' obstacles on the surface of the sphere. Ranges from "
354 "[0, 2]")
355 parser.add_argument("-e", "--extra", type=int, default=1,
356 help="Number of extra constraints to add to the chain. Extra constraints "
357 "are as follows:\n"
358 "1: End-effector is constrained to be on the surface of a sphere of radius "
359 "links - 2\n"
360 "2: (links-5)th and (links-4)th ball have the same z-value\n"
361 "3: (links-4)th and (links-3)th ball have the same x-value\n"
362 "4: (links-3)th and (links-2)th ball have the same z-value")
363 addSpaceOption(parser)
364 addPlannerOption(parser)
365 addConstrainedOptions(parser)
366 addAtlasOptions(parser)
367
368 chainPlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:76
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:50
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...