38 from __future__
import print_function
39 from ConstrainedPlanningCommon
import *
42 from functools
import partial
47 norm = np.linalg.norm(x)
48 if norm > 0
and np.isfinite(norm):
60 def __init__(self, offset, thickness, width, joint_radius, wallType):
63 self.
width = width + joint_radius
73 def checkJoint(self, v):
102 def __init__(self, links, obstacles=0, extra=1):
103 super(ChainConstraint, self).__init__(3 * links, links + extra)
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
111 step = 2. * self.radius / (obstacles + 1.)
113 8., self.
width, self.jointRadius, i % 2)
for i
in range(obstacles)]
115 def function(self, x, out):
117 for i
in range(self.links):
118 joint2 = x[(3 * i):(3 * i + 3)]
119 out[i] = np.linalg.norm(joint1 - joint2) - self.length
123 out[self.links] = np.linalg.norm(x[-3:]) - self.radius
128 out[self.links + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2]
130 out[self.links + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0]
132 out[self.links + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2]
134 def jacobian(self, x, out):
135 out[:, :] = np.zeros(
136 (self.getCoDimension(), self.getAmbientDimension()), dtype=np.float64)
138 plus = np.zeros(3 * (self.links + 1))
139 plus[:(3 * self.links)] = x[:(3 * self.links)]
141 minus = np.zeros(3 * (self.links + 1))
142 minus[-(3 * self.links):] = x[:(3 * self.links)]
144 diagonal = plus - minus
146 for i
in range(self.links):
147 out[i, (3 * i):(3 * i + 3)] = normalize(diagonal[(3 * i):(3 * i + 3)])
148 out[1:self.links, 0:(3 * self.links - 3)
149 ] -= out[1:self.links, 3:(3 * self.links)]
152 out[self.links, -3:] = -normalize(diagonal[-3:])
157 out[self.links + 1, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
159 out[self.links + 2, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
161 out[self.links + 3, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
165 def isValid(self, state):
166 x = np.array([state[i]
for i
in range(self.getAmbientDimension())])
167 for i
in range(self.links):
168 joint = x[(3 * i):(3 * i + 3)]
171 if np.linalg.norm(joint) >= self.radius - self.jointRadius:
172 for wall
in self.walls:
173 if not wall.checkJoint(joint):
176 for i
in range(self.links):
177 joint1 = x[(3 * i):(3 * i + 3)]
178 if np.max(np.absolute(joint1)) < self.jointRadius:
181 for j
in range(i + 1, self.links):
182 joint2 = x[(3 * j):(3 * j + 3)]
183 if np.max(np.absolute(joint1 - joint2)) < self.jointRadius:
188 def createSpace(self):
192 for i
in range(self.links):
193 bounds.setLow(3 * i + 0, -i - 1)
194 bounds.setHigh(3 * i + 0, i + 1)
196 bounds.setLow(3 * i + 1, -i - 1)
197 bounds.setHigh(3 * i + 1, i + 1)
199 bounds.setLow(3 * i + 2, -i - 1)
200 bounds.setHigh(3 * i + 2, i + 1)
202 rvss.setBounds(bounds)
205 def getStartAndGoalStates(self):
206 start = np.zeros(3 * self.links)
207 goal = np.zeros(3 * self.links)
209 for i
in range(self.links - 3):
214 goal[3 * i] = -(i + 1)
221 start[3 * i + 1] = -1
231 start[3 * i + 1] = -1
244 goal[3 * i] = -(i - 1)
253 def getProjection(self, space):
256 def __init__(self, space, links, radius):
257 super(ChainProjection, self).__init__(space)
261 self.defaultCellSizes()
263 def getDimension(self):
266 def defaultCellSizes(self):
267 self.cellSizes_ = list2vec([.1, .1])
269 def project(self, state, projection):
270 s = 3 * (self.links - 1)
271 projection[0] = math.atan2(state[s + 1], state[s])
272 projection[1] = math.acos(state[s + 2] / self.radius)
274 return ChainProjection(space, self.links, self.radius)
276 def dump(self, outfile):
277 print(self.links, file=outfile)
278 print(self.obstacles, file=outfile)
279 print(self.extra, file=outfile)
280 print(self.jointRadius, file=outfile)
281 print(self.length, file=outfile)
282 print(self.radius, file=outfile)
283 print(self.
width, file=outfile)
285 def addBenchmarkParameters(self, bench):
286 bench.addExperimentParameter(
"links",
"INTEGER", str(self.links))
287 bench.addExperimentParameter(
288 "obstacles",
"INTEGER", str(self.obstacles))
289 bench.addExperimentParameter(
"extra",
"INTEGER", str(self.extra))
292 def chainPlanningOnce(cp, planner, output):
293 cp.setPlanner(planner,
"chain")
296 stat = cp.solveOnce(output,
"chain")
299 ou.OMPL_INFORM(
"Dumping problem information to `chain_info.txt`.")
300 with open(
"chain_info.txt",
"w")
as infofile:
301 print(cp.spaceType, file=infofile)
302 cp.constraint.dump(infofile)
308 def chainPlanningBench(cp, planners):
309 cp.setupBenchmark(planners,
"chain")
310 cp.constraint.addBenchmarkParameters(cp.bench)
314 def chainPlanning(options):
317 options.links, options.obstacles, options.extra)
320 options.space, constraint.createSpace(), constraint, options)
322 cp.css.registerProjection(
"chain", constraint.getProjection(cp.css))
324 start, goal = constraint.getStartAndGoalStates()
327 for i
in range(cp.css.getDimension()):
330 cp.setStartAndGoalStates(sstart, sgoal)
332 ChainConstraint.isValid, constraint)))
334 planners = options.planner.split(
",")
335 if not options.bench:
336 chainPlanningOnce(cp, planners[0], options.output)
338 chainPlanningBench(cp, planners)
341 if __name__ ==
"__main__":
342 parser = argparse.ArgumentParser()
343 parser.add_argument(
"-o",
"--output", action=
"store_true",
344 help=
"Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
345 "`torus_path.txt` and `torus_graph.graphml` respectively.")
346 parser.add_argument(
"--bench", action=
"store_true",
347 help=
"Do benchmarking on provided planner list.")
348 parser.add_argument(
"-l",
"--links", type=int, default=5,
349 help=
"Number of links in the kinematic chain. Minimum is 4.")
350 parser.add_argument(
"-x",
"--obstacles", type=int, default=0, choices=[0, 1, 2],
351 help=
"Number of `wall' obstacles on the surface of the sphere. Ranges from [0, 2]")
352 parser.add_argument(
"-e",
"--extra", type=int, default=1,
353 help=
"Number of extra constraints to add to the chain. Extra constraints are as follows:\n"
354 "1: End-effector is constrained to be on the surface of a sphere of radius links - 2\n"
355 "2: (links-5)th and (links-4)th ball have the same z-value\n"
356 "3: (links-4)th and (links-3)th ball have the same x-value\n"
357 "4: (links-3)th and (links-2)th ball have the same z-value")
358 addSpaceOption(parser)
359 addPlannerOption(parser)
360 addConstrainedOptions(parser)
361 addAtlasOptions(parser)
363 chainPlanning(parser.parse_args())