def errors(self, x, rot_mat, tan60, e_v, n_v, h, l, n):
theta, px, py, pz = x
# Because n_v is fixed, after finding p0, a precomputed
# rotation matrix can be used to find p1 and p2.
p0_v = rotate_vector_fast(l * e_v, n_v, -theta)
p1_v = np.dot(rot_mat, p0_v)
p2_v = np.dot(rot_mat, p1_v)
pc = np.array([px, py, pz])
p0 = p0_v + pc
p1 = p1_v + pc
p2 = p2_v + pc
n_p = pc + n * n_v
return (point_line_distance_fast(p0, 0),
point_line_distance_fast(p1, -tan60),
point_line_distance_fast(p2, tan60),
n_p[2] - h)
def solve(self, e_v, f_v, h):
self.e_v = e_v
self.f_v = f_v
self.h = h
n_v = np.cross(e_v, f_v)
n_v = n_v / np.linalg.norm(n_v)
self.n_v = n_v
theta_init = np.arctan2(e_v[1], e_v[0])
pz_init = self.h - self.n
estimate = [theta_init, 0, 0, pz_init]
rot_mat = rotation_matrix(n_v, radians(120))
tan60 = np.tan(radians(60))
x = optimize.fsolve(self.errors, estimate, args=(
rot_mat, tan60, e_v, n_v, self.h, self.l, self.n
))
theta = x[0]
self.theta = theta
platform_centroid = np.array([x[1], x[2], x[3]])
self.p_c = platform_centroid
self.n_p = self.p_c + self.n * n_v
self.e_p = self.n_p + self.e_v
# If necessary, these points are calculated like they
# are in the errors function.
p0_v = rotate_vector_fast(self.l * self.e_v, self.n_v, -self.theta)
p1_v = np.dot(rot_mat, p0_v)
p2_v = np.dot(rot_mat, p1_v)
p0 = p0_v + self.p_c
p1 = p1_v + self.p_c
p2 = p2_v + self.p_c
self.platform_points = np.array([p0, p1, p2]).T
return theta, platform_centroid, self.platform_points
point_line_distance_fast(p1, -tan60),
point_line_distance_fast(p2, tan60),
n_p[2] - h)
if __name__ == "__main__":
np.set_printoptions(suppress=True)
gx_v = np.array([1., 0., 0.])
gy_v = np.array([0., 1., 0.])
gz_v = np.array([0., 0., 1.])
e_v = np.array([1., 0., 0.])
f_v = np.array([0., 1., 0.])
e_v = rotate_vector_fast(e_v, gz_v, radians(0))
f_v = rotate_vector_fast(f_v, gz_v, radians(0))
e_v = rotate_vector_fast(e_v, gy_v, radians(-10))
f_v = rotate_vector_fast(f_v, gy_v, radians(-10))
e_v = rotate_vector_fast(e_v, gx_v, radians(-10))
f_v = rotate_vector_fast(f_v, gx_v, radians(-10))
pik = PlatformIK(l=1, n=0.1)
theta, centroid, points = pik.solve(e_v, f_v, 1)
print "Solution:"
print "theta:", degrees(theta)
print "p_c:", centroid
print "p:"
