def workspace_distance(a, b):
"""Returns a distance in R^n or SE(3) between points a and b. If len(a)<=3,
this will do cartesian distance. Otherwise, len(a)==6 is required and
this will do SE(3) distance (with a max orientation distance of 1)."""
if len(a) <= 3:
return vectorops.distance(a, b)
assert len(a) == 6, "Can only interpolate in R2, R3, or SE(3)"
dt = vectorops.distance(a[:3], b[:3])
dR = so3.distance(so3.from_moment(a[3:]), so3.from_moment(b[3:]))
return dt + dR / math.pi
def so3_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = so3_staggered_grid(N)
else:
G = so3_grid(N)
vispt = [1, 0, 0]
for n in G.nodes():
R = G.node[n]['params']
trans = so3.apply(R, vispt)
#trans = so3.axis_angle(R)[0]
#trans = vectorops.unit(so3.quaternion(R)[:3])
vis.add(str(n), [R, trans])
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
Ri = G.node[i]['params']
Rj = G.node[j]['params']
tmax = 9
times = range(tmax + 1)
milestones = []
for t in times:
u = float(t) / tmax
trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
milestones.append(trans)
vis.add(
str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
vis.hideLabel(str(i) + '-' + str(j))
dist = so3.distance(Ri, Rj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
return so3.rotation(axis, angle)
def interpolate_transform(T1, T2, u):
"""Interpolate linearly between the two transformations T1 and T2."""
return (interpolate_rotation(T1[0], T2[0],
u), interpolate_linear(T1[1], T2[1], u))
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print("Angle between")
print(so3.matrix(Ra))
print("and")
print(so3.matrix(Rb))
print("is", so3.distance(Ra, Rb))
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
vis.edit("end")
vis.setAttribute("world", "length", 0.25)
vis.setAttribute("interpolated", "fancy", True)
vis.setAttribute("interpolated", "width", 0.03)
vis.setAttribute("interpolated", "length", 1.0)
def orientation_dist(orient1, orient2):
rotMat1 = mathUtils.euler_zyx_mat(orient1)
rotMat2 = mathUtils.euler_zyx_mat(orient2)
dist = so3.distance(rotMat1, rotMat2)
return dist