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
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
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):
106  self.width = ChainConstraint.WALL_WIDTH
109  self.obstacles = obstacles
110  self.extra = extra
111  step = 2. * self.radius / (obstacles + 1.)
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)
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:
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))
141
142  minus = np.zeros(3 * (self.links + 1))
144
145  diagonal = plus - minus
146
148  out[i, (3 * i):(3 * i + 3)] = normalize(diagonal[(3 * i):(3 * i + 3)])
151
152  if self.extra >= 1:
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())])
169  joint = x[(3 * i):(3 * i + 3)]
170  if joint[2] < 0:
171  return False
173  for wall in self.walls:
174  if not wall.checkJoint(joint):
175  return False
176
178  joint1 = x[(3 * i):(3 * i + 3)]
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
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
258  super(ChainProjection, self).__init__(space)
260  # Radius of sphere end-effector lies on (for extra = 1)
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
276
277  def dump(self, outfile):
279  print(self.obstacles, file=outfile)
280  print(self.extra, file=outfile)
282  print(self.length, file=outfile)
284  print(self.width, file=outfile)
285
289  "obstacles", "INTEGER", str(self.obstacles))
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")
312  cp.runBenchmark()
313
314
315 def chainPlanning(options):
316  # Create our constraint.
317  constraint = ChainConstraint(
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()
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.")
349  help="Do benchmarking on provided planner list.")
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]")
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 "
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.