ConstrainedPlanningImplicitChain.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 from __future__ import print_function
39 from ConstrainedPlanningCommon import *
40 import argparse
41 import math
42 from functools import partial
43 import numpy as np
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 /
113  8., self.width, self.jointRadius, i % 2) for i in range(obstacles)]
114 
115  def function(self, x, out):
116  joint1 = np.zeros(3)
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
120  joint1 = joint2
121 
122  if self.extra >= 1:
123  out[self.links] = np.linalg.norm(x[-3:]) - self.radius
124 
125  o = self.links - 5
126 
127  if self.extra >= 2:
128  out[self.links + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2]
129  if self.extra >= 3:
130  out[self.links + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0]
131  if self.extra >= 4:
132  out[self.links + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2]
133 
134  def jacobian(self, x, out):
135  out[:, :] = np.zeros(
136  (self.getCoDimension(), self.getAmbientDimension()), dtype=np.float64)
137 
138  plus = np.zeros(3 * (self.links + 1))
139  plus[:(3 * self.links)] = x[:(3 * self.links)]
140 
141  minus = np.zeros(3 * (self.links + 1))
142  minus[-(3 * self.links):] = x[:(3 * self.links)]
143 
144  diagonal = plus - minus
145 
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)]
150 
151  if self.extra >= 1:
152  out[self.links, -3:] = -normalize(diagonal[-3:])
153 
154  o = self.links - 5
155 
156  if self.extra >= 2:
157  out[self.links + 1, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
158  if self.extra >= 3:
159  out[self.links + 2, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
160  if self.extra >= 4:
161  out[self.links + 3, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
162 
163  # Checks if there are no self-collisions (of the joints themselves) or
164  # collisions with the extra obstacles on the surface of the sphere.
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)]
169  if joint[2] < 0:
170  return False
171  if np.linalg.norm(joint) >= self.radius - self.jointRadius:
172  for wall in self.walls:
173  if not wall.checkJoint(joint):
174  return False
175 
176  for i in range(self.links):
177  joint1 = x[(3 * i):(3 * i + 3)]
178  if np.max(np.absolute(joint1)) < self.jointRadius:
179  return False
180 
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:
184  return False
185 
186  return True
187 
188  def createSpace(self):
189  rvss = ob.RealVectorStateSpace(3 * self.links)
190  bounds = ob.RealVectorBounds(3 * self.links)
191 
192  for i in range(self.links):
193  bounds.setLow(3 * i + 0, -i - 1)
194  bounds.setHigh(3 * i + 0, i + 1)
195 
196  bounds.setLow(3 * i + 1, -i - 1)
197  bounds.setHigh(3 * i + 1, i + 1)
198 
199  bounds.setLow(3 * i + 2, -i - 1)
200  bounds.setHigh(3 * i + 2, i + 1)
201 
202  rvss.setBounds(bounds)
203  return rvss
204 
205  def getStartAndGoalStates(self):
206  start = np.zeros(3 * self.links)
207  goal = np.zeros(3 * self.links)
208 
209  for i in range(self.links - 3):
210  start[3 * i] = i + 1
211  start[3 * i + 1] = 0
212  start[3 * i + 2] = 0
213 
214  goal[3 * i] = -(i + 1)
215  goal[3 * i + 1] = 0
216  goal[3 * i + 2] = 0
217 
218  i = self.links - 3
219 
220  start[3 * i] = i
221  start[3 * i + 1] = -1
222  start[3 * i + 2] = 0
223 
224  goal[3 * i] = -i
225  goal[3 * i + 1] = 1
226  goal[3 * i + 2] = 0
227 
228  i += 1
229 
230  start[3 * i] = i
231  start[3 * i + 1] = -1
232  start[3 * i + 2] = 0
233 
234  goal[3 * i] = -i
235  goal[3 * i + 1] = 1
236  goal[3 * i + 2] = 0
237 
238  i += 1
239 
240  start[3 * i] = i - 1
241  start[3 * i + 1] = 0
242  start[3 * i + 2] = 0
243 
244  goal[3 * i] = -(i - 1)
245  goal[3 * i + 1] = 0
246  goal[3 * i + 2] = 0
247 
248  return start, goal
249 
250  # Create a projection evaluator for the chain constraint. Finds the
251  # spherical coordinates of the end-effector on the surface of the sphere of
252  # radius equal to that of the constraint (when extra = 1). */
253  def getProjection(self, space):
254  class ChainProjection(ob.ProjectionEvaluator):
255 
256  def __init__(self, space, links, radius):
257  super(ChainProjection, self).__init__(space)
258  self.links = links # Number of chain links.
259  # Radius of sphere end-effector lies on (for extra = 1)
260  self.radius = radius
261  self.defaultCellSizes()
262 
263  def getDimension(self):
264  return 2
265 
266  def defaultCellSizes(self):
267  self.cellSizes_ = list2vec([.1, .1])
268 
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)
273 
274  return ChainProjection(space, self.links, self.radius)
275 
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)
284 
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))
290 
291 
292 def chainPlanningOnce(cp, planner, output):
293  cp.setPlanner(planner, "chain")
294 
295  # Solve the problem
296  stat = cp.solveOnce(output, "chain")
297 
298  if output:
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)
303 
304  cp.atlasStats()
305  return stat
306 
307 
308 def chainPlanningBench(cp, planners):
309  cp.setupBenchmark(planners, "chain")
310  cp.constraint.addBenchmarkParameters(cp.bench)
311  cp.runBenchmark()
312 
313 
314 def chainPlanning(options):
315  # Create our constraint.
316  constraint = ChainConstraint(
317  options.links, options.obstacles, options.extra)
318 
319  cp = ConstrainedProblem(
320  options.space, constraint.createSpace(), constraint, options)
321 
322  cp.css.registerProjection("chain", constraint.getProjection(cp.css))
323 
324  start, goal = constraint.getStartAndGoalStates()
325  sstart = ob.State(cp.css)
326  sgoal = ob.State(cp.css)
327  for i in range(cp.css.getDimension()):
328  sstart[i] = start[i]
329  sgoal[i] = goal[i]
330  cp.setStartAndGoalStates(sstart, sgoal)
331  cp.ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(
332  ChainConstraint.isValid, constraint)))
333 
334  planners = options.planner.split(",")
335  if not options.bench:
336  chainPlanningOnce(cp, planners[0], options.output)
337  else:
338  chainPlanningBench(cp, planners)
339 
340 
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)
362 
363  chainPlanning(parser.parse_args())
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:49
bool project(State *state) const
Project a state state given the constraints. If a valid projection cannot be found, this method will return false. Even if this method fails, state will be modified.
Definition: Constraint.cpp:88
The lower and upper bounds for an Rn space.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...