def test_icp(self):
"""Test icp implementation"""
f = self.notebook_locals["icp"]
nearest_neighbors = self.notebook_locals["nearest_neighbors"]
# It should be sufficient to test only one for ICP.
X_BA = generate_random_transform(7)
self.scene = X_BA.multiply(self.model.T).T
X_BA_test, mean_error_test, num_iters_test = f(self.scene, self.model)
num_iters = 0
mean_error = 0.0
max_iterations = 20
tolerance = 1e-3
prev_error = 0
X_BA = RigidTransform()
while True:
num_iters += 1
distances, indices = nearest_neighbors(
self.scene,
X_BA.multiply(self.model.T).T)
X_BA = least_squares_transform(self.scene, self.model[indices])
mean_error = np.mean(distances)
if abs(mean_error -
prev_error) < tolerance or num_iters >= max_iterations:
break
prev_error = mean_error
result = X_BA.multiply(X_BA_test.inverse())
self.assertTrue(np.allclose(result.GetAsMatrix4(), np.eye(4)),
'icp implementation is incorrect')
def test_X_WB(self):
"""Testing X_WB"""
f = self.notebook_locals['compute_X_WB']
# construct a test case
theta1, theta2, theta3 = np.pi / 3.0, np.pi / 6.0, np.pi / 4.0
R_WA = RotationMatrix.MakeXRotation(theta1)
R_AB = RotationMatrix.MakeZRotation(theta2)
R_CB = RotationMatrix.MakeYRotation(theta3)
X_WA = RigidTransform(R_WA, [0.1, 0.2, 0.5])
X_AB = RigidTransform(R_AB, [0.3, 0.4, 0.1])
X_CB = RigidTransform(R_CB, [0.5, 0.9, 0.7])
test_X_WB = f(X_WA, X_AB, X_CB)
true_X_WB = X_WA.multiply(X_AB)
test_result = test_X_WB.multiply(true_X_WB.inverse())
test_result = test_result.GetAsMatrix4()
self.assertTrue(np.allclose(test_result, np.eye(4)))
def calc_x0(self, context, output):
x0 = np.zeros(6)
# Calc X_L_SP, the transform from the link to the setpoint
X_L_SP = RigidTransform() # For now, just make them the same
# Calc X_W_L
pose_L_rotational = self.GetInputPort("pose_L_rotational").Eval(
context)
pose_L_translational = self.GetInputPort("pose_L_translational").Eval(
context)
X_W_L = RigidTransform(p=pose_L_translational,
R=RotationMatrix(
RollPitchYaw(pose_L_rotational)))
# Calc X_W_SP
X_W_SP = X_W_L.multiply(X_L_SP)
translation = X_W_SP.translation()
rotation = RollPitchYaw(X_W_SP.rotation()).vector()
x0[:3] = rotation
x0[3:] = translation
output.SetFromVector(x0)
# Figure out where we're putting the camera for the scene by
# pointing it inwards, and then applying a random yaw around
# the origin.
cam_quat_base = RollPitchYaw(
68.*np.pi/180.,
0.*np.pi/180,
38.6*np.pi/180.).ToQuaternion()
cam_trans_base = np.array([0.47, -0.54, 0.31])
cam_tf_base = RigidTransform(quaternion=cam_quat_base,
p=cam_trans_base)
# Rotate camera around origin
cam_additional_rotation = RigidTransform(
quaternion=RollPitchYaw(0., 0., np.random.uniform(0., np.pi*2.)).ToQuaternion(),
p=[0, 0, 0]
)
cam_tf_base = cam_additional_rotation.multiply(cam_tf_base)
cam_tfs = [cam_tf_base]
# Set up the blender color camera using that camera, and specifying
# the environment map and a material to apply to the "ground" object.
# If you wanted to further rotate the scene (to e.g. rotate the background
# of the scene relative to the table coordinate system), you could apply an
# extra yaw here.
offset_quat_base = RollPitchYaw(0., 0., 0.).ToQuaternion().wxyz()
os.system("mkdir -p /tmp/ycb_scene_%03d" % scene_k)
blender_color_cam = builder.AddSystem(BlenderColorCamera(
scene_graph,
draw_period=0.03333/2.,
camera_tfs=cam_tfs,
zmq_url="tcp://127.0.0.1:5556",
env_map_path="data/env_maps/aerodynamics_workshop_4k.hdr",
