ConstrainedPlanningImplicitChain.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 from __future__ import print_function
39 import argparse
40 import math
41 from functools import partial
42 import numpy as np
43 from ConstrainedPlanningCommon import *
44 
45 
46 def 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 
293 def 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 
309 def chainPlanningBench(cp, planners):
310  cp.setupBenchmark(planners, "chain")
311  cp.constraint.addBenchmarkParameters(cp.bench)
312  cp.runBenchmark()
313 
314 
315 def chainPlanning(options):
316  # Create our constraint.
317  constraint = ChainConstraint(
318  options.links, options.obstacles, options.extra)
319 
320  cp = ConstrainedProblem(
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 
342 if __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:107
Definition of an abstract state.
Definition: State.h:113
A state space representing Rn. The distance function is the L2 norm.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found,...
Definition: Constraint.cpp:88
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
The lower and upper bounds for an Rn space.