def test_get_robot_roll_rpy2(self):
robot, pose, internal_angle = self.init_robot_and_pose()
cam_angles = get_robot_horizon_r(robot, PyDataVector(-internal_angle, 0, 0)) * rtd()
self.assertEquals(internal_angle, -13)
self.assertTrue(abs(cam_angles[1] + internal_angle) < 1e-10)
def test_get_robot_pitch_rpy2(self):
robot, pose, internal_angle = self.init_robot_and_pose()
# robot.set_initial_angles(0, internal_angle, 0)
self.assertEquals(internal_angle, -13)
cam_angles = get_robot_horizon_r(robot, PyDataVector(0, -internal_angle, 0)) * rtd()
self.assertTrue(abs(cam_angles[1]) < 1e-10)
self.assertTrue(abs(cam_angles[0] + 2 * internal_angle) < 1e-10)
