def distConfAbs(self, q1, q2, moveChains=None, dmax=1.0e6):
total = 10*abs(hu.angleDiff(q1.baseConf()[-1], q2.baseConf()[-1]))
for chainName in moveChains or self.chainNames:
if chainName in q1.conf and chainName in q2.conf:
td = self.chains.chainsByName[chainName].distAbsLess(q1[chainName],
q2[chainName],
dmax-total)
if td is None: return None
total += td
return total
def graspClass(wrist):
# is wrist close enough to horizontal or vertical approach?
euler = euler_from_matrix(wrist.matrix)
eps = math.pi / 10
if abs(hu.angleDiff(math.pi / 2, euler[1])) < eps:
return 'v', (euler[0], math.pi / 2, euler[2])
elif abs(euler[0]) < eps and abs(euler[1]) < eps:
return 'h', (0., 0., euler[2])
else:
return None, None
def distance(self, other, chains = None):
d = 0.
if chains is None: chains = self.keys()
for c in chains:
if not other.get(c): continue
if isinstance(self.get(c), hu.Transform):
strans = self.get(c)
otrans = other.get(c)
d += np.linalg.norm(otrans.matrix[:3,3] - strans.matrix[:3,3])
dot = np.dot(otrans.quat().matrix, strans.quat().matrix)
dot = max(-1.0, min(1.0, dot))
d += 2*abs(math.acos(abs(dot)))
else:
d += sum(abs(hu.angleDiff(x,y)) for (x,y) in zip(self.get(c),other.get(c)))
return d
def tangentSolOld(x, y, x0, y0):
# print 'X', (x,y), 'X0', (x0, y0)
alpha = math.atan2(y,x)
theta0 = math.atan2(y0,x0)
r = math.sqrt(x0*x0 + y0*y0)
d = math.sqrt(x*x + y*y)
theta1 = math.pi/2
ac = math.acos((r/d)*math.cos(theta0-theta1))
# print 'alpha', alpha, 'theta0', theta0, 'ac', ac
values = [alpha + theta1 + ac,
alpha + theta1 - ac,
alpha - theta1 + ac,
alpha - theta1 - ac]
keep = []
for angle in values:
v = (x - r*math.cos(angle + theta0),
y - r*math.sin(angle + theta0))
vangle = math.atan2(v[1], v[0])
# print 'angle', angle, 'atan', vangle
if hu.angleDiff(angle, vangle) < 0.001:
keep.append(vangle)
# This generally returns two answers, but they may not be within limits.
# print 'keep', keep
return keep
def distance(self, other, chains = None):
if chains is None: chains = self.keys()
return sum(sum([abs(hu.angleDiff(x,y)) for (x,y) in zip(self.conf[c],other.conf[c])]) \
for c in chains if c in other.conf)