def walkerExpandToward(self, n, x, delta=0.5, dt=0.1):
#hack for walker
d = self.cspace.distance(n.data.x, x)
if d > delta:
x = self.cspace.interpolate(n.data.x, x, delta / d)
u = [dt] + vectorops.sub(x[6:12], n.data.x.q[6:]) + vectorops.sub(
x[18:], n.data.x.v[6:])
if not self.problem.isUFeasible(n.data.x, u):
self.uInfeasible += 1
return None
xu = self.problem.f(n.data.x, u)
if not self.problem.isXFeasible(xu):
self.xInfeasible += 1
return None
return self.makeNode(xu, n, u)
def distance(self, a, b):
if hasattr(a, 'q'):
a = a.q + a.v
if hasattr(b, 'q'):
b = b.q + b.v
d = vectorops.sub(a, b)
for i in xrange(3, 6):
while d[i] > math.pi:
d[i] -= 2.0 * math.pi
while d[i] < -math.pi:
d[i] += 2.0 * math.pi
if self.norm == 2.0:
return math.sqrt(
sum(wi * di**2 for di, wi in zip(d, self.weights) if wi != 0))
elif self.norm == 'inf':
return max(wi * abs(di) for di, wi in zip(d, self.weights)
if wi != 0)
elif self.norm == 1:
return sum(wi * abs(di) for di, wi in zip(d, weights) if wi != 0)
else:
return pow(
sum(wi * pow(di, self.norm) for di, wi in zip(d, self.weights)
if wi != 0), 1.0 / self.norm)
def interpolate(self, a, b, u):
if hasattr(a, 'q'):
a = a.q + a.v
if hasattr(b, 'q'):
b = b.q + b.v
return vectorops.madd(a, vectorops.sub(b, a), u)