def test_gp(data_folder):
good_model_states, bad_model_states = setup_stilde_training_data(data_folder)
#pt = [ 0.41101035, 0.01295252, 0.05013599, -0.01704243, 0.99902863,
# 0.03855948, -0.01282647]
pt = RigidTransform()
pt.translation = bad_model_states[2,:3]
in_region_with_modelfailure(pt, good_model_states, bad_model_states)
def visualize_test():
I = RigidTransform()
g_ab = RigidTransform()
g_ab.translation = np.array([0.05, 0, 0])
g_ab.rotation = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
q_a = np.array([0., 0., 0.])
p_b = np.array([0., 0., 0.])
p_a = apply_transform(p_b, g_ab)
print('g_ab = \n{}'.format(g_ab.matrix))
vis3d.pose(I, alpha=0.01, tube_radius=0.001, center_scale=0.001)
vis3d.points(q_a, color=(1, 0, 0), scale=0.005)
vis3d.pose(g_ab, alpha=0.01, tube_radius=0.001, center_scale=0.001)
vis3d.points(p_a, color=(0, 1, 0), scale=0.005)
vis3d.show()
