def run_task_space_plan():
"""Test task space plan."""
frame_E = zmq_client.plant.GetFrameByName('iiwa_link_7')
X_ET = RigidTransform()
X_ET.set_translation([0.1, 0, 0])
X_WE0 = zmq_client.get_current_ee_pose(frame_E)
X_WT0 = X_WE0.multiply(X_ET)
X_WT1 = RigidTransform(X_WT0.rotation(),
X_WT0.translation() + np.array([0, 0.2, 0]))
plan_msg = calc_task_space_plan_msg(X_ET, [X_WT0, X_WT1], [0, 5])
zmq_client.send_plan(plan_msg)
def least_squares_transform(scene, model):
X_BA = RigidTransform()
mu_m = np.mean(model, axis=0)
mu_s = np.mean(scene, axis=0)
W = (scene - mu_s).T.dot(model - mu_m)
U, Sigma, Vh = np.linalg.svd(W)
R_star = U.dot(Vh)
if np.linalg.det(R_star) < 0:
Vh[-1] *= -1
R_star = U.dot(Vh)
t_star = mu_s - R_star.dot(mu_m)
X_BA.set_rotation(RotationMatrix(R_star))
X_BA.set_translation(t_star)
return X_BA
