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",