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