def infer_dh_parameter(last_frame, this_frame):
last_z = geo.Line(k=last_frame.z, p=last_frame.o)
this_z = geo.Line(k=this_frame.z, p=this_frame.o)
alpha, a = geo.angle_distance(last_z, this_z, last_frame.x)
last_x = geo.Line(k=last_frame.x, p=last_frame.o)
this_x = geo.Line(k=this_frame.x, p=this_frame.o)
theta, d = geo.angle_distance(last_x, this_x, this_frame.z)
return DHRow(a=a, alpha=alpha, d=d, theta=theta)
def absolute_axis(axes, frames):
assert len(axes) == len(frames)
ret = []
for a, f in zip(axes, frames):
line = ge.Line(k=np.matmul(f[:3, :3], a), p=f[:3, -1])
ret.append(line)
return ret
def infer_initial_frame(joints):
p_ref, d_ref = search_reference_axis(joints)
z = joints[0]
# when all axis are the same
if d_ref is None:
dx = geo.pick_perpendicular_direction(z.k)
return geo.Line(p=z.p, k=dx)
dx = geo.common_normal(z, d_ref)
if p_ref is None:
origin = z.p
else:
cn = geo.common_normal(z, p_ref)
origin = geo.common_normal_intersection(z, p_ref, cn)
return geo.Line(p=origin, k=dx)
def assign_dh_frame(last_x, this_z, next_z):
# same line case
if geo.same_line(this_z, next_z):
return last_x
dx = geo.common_normal(this_z, next_z)
# intersect case, select dx has lesser angle with last-x
if geo.coplanar(this_z, next_z):
sign = np.sign(geo.cosine(dx, last_x.k))
if sign != 0:
dx = sign * dx
# determine the origin
if geo.parallel(this_z.k, next_z.k):
origin = geo.planar_intersection(last_x, this_z)
else:
origin = geo.common_normal_intersection(this_z, next_z, dx)
return geo.Line(p=origin, k=dx)