if edgeFound and loopFound:
break
if not edgeFound: raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n2))
if not loopFound: raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n1))
return edges, loops
# infinite norm between vectors
dC = lambda q1,q2: reduce (lambda x,y : x if fabs (y [0]- y [1]) < x \
else fabs (y [0]- y [1]), zip (q1, q2), 0)
display = False
if display:
v = vf.createViewer()
pp = PathPlayer(v)
else:
v = lambda x: None
## Initial configuration of manipulator arms
q0_r0 = [
pi / 6,
-pi / 2,
pi / 2,
0,
0,
0,
]
q0_r1 = q0_r0[::]
loopFound = True
if edgeFound and loopFound:
break
if not edgeFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n2))
if not loopFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n1))
return edges, loops
# infinite norm between vectors
dC = lambda q1,q2: reduce (lambda x,y : x if fabs (y [0]- y [1]) < x \
else fabs (y [0]- y [1]), zip (q1, q2), 0)
display = False
if display:
v = vf.createViewer ()
pp = PathPlayer (v)
else:
v = lambda x: None
## Initial configuration of manipulator arms
q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0,]
q0_r1 = q0_r0 [::]
## Generate initial configurations of spheres
q0_spheres = list ()
i = 0
y = -0.04
while i < nSphere:
q0_spheres.append ([-0.45 - .1*(i/2), y, 0.025, 0, 0, 0, 1])
i+=1; y = -y