def test_evaluate_state_difficulty_4(self): difficulty = 4 pose_origin = move_cuboid.Pose() pose_trans = move_cuboid.Pose(position=[1, 2, 3]) pose_rot = move_cuboid.Pose( orientation=Rotation.from_euler("z", 0.42).as_quat()) pose_both = move_cuboid.Pose([1, 2, 3], Rotation.from_euler("z", 0.42).as_quat()) # needs to be zero for exact match cost = move_cuboid.evaluate_state(pose_origin, pose_origin, difficulty) self.assertEqual(cost, 0) # None-zero if there is translation, rotation or both self.assertNotEqual( move_cuboid.evaluate_state(pose_origin, pose_trans, difficulty), 0) self.assertNotEqual( move_cuboid.evaluate_state(pose_origin, pose_rot, difficulty), 0) self.assertNotEqual( move_cuboid.evaluate_state(pose_origin, pose_both, difficulty), 0)
def test_evaluate_state_difficulty_4_rotation_around_y(self): difficulty = 4 pose_origin = move_cuboid.Pose() pose_rot_only_y = move_cuboid.Pose( orientation=Rotation.from_euler("y", 0.42).as_quat()) pose_rot_without_y = move_cuboid.Pose( orientation=Rotation.from_euler("yz", [0.0, 0.42]).as_quat()) pose_rot_with_y = move_cuboid.Pose( orientation=Rotation.from_euler("yz", [0.2, 0.42]).as_quat()) # needs to be zero for exact match cost = move_cuboid.evaluate_state(pose_origin, pose_origin, difficulty) self.assertEqual(cost, 0) cost_only_y = move_cuboid.evaluate_state(pose_origin, pose_rot_only_y, difficulty) self.assertEqual(cost_only_y, 0) cost_without_y = move_cuboid.evaluate_state(pose_origin, pose_rot_without_y, difficulty) cost_with_y = move_cuboid.evaluate_state(pose_origin, pose_rot_with_y, difficulty) self.assertAlmostEqual(cost_without_y, cost_with_y)
def test_validate_goal(self): on_ground_height = move_cuboid._CUBOID_HALF_SIZE[2] yaw_rotation = Rotation.from_euler("z", 0.42).as_quat() full_rotation = Rotation.from_euler("zxz", [0.42, 0.1, -2.3]).as_quat() # test some valid goals try: move_cuboid.validate_goal( move_cuboid.Pose([0, 0, on_ground_height], [0, 0, 0, 1])) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cuboid.validate_goal( move_cuboid.Pose([0.05, -0.1, on_ground_height], yaw_rotation)) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) try: move_cuboid.validate_goal( move_cuboid.Pose([-0.12, 0.0, 0.06], full_rotation)) except Exception as e: self.fail("Valid goal was considered invalid because %s" % e) # test some invalid goals # invalid values with self.assertRaises(ValueError): move_cuboid.validate_goal(move_cuboid.Pose([0, 0], [0, 0, 0, 1])) with self.assertRaises(ValueError): move_cuboid.validate_goal(move_cuboid.Pose([0, 0, 0], [0, 0, 1])) # invalid positions with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal( move_cuboid.Pose([0.3, 0, on_ground_height], [0, 0, 0, 1])) with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal( move_cuboid.Pose([0, -0.3, on_ground_height], [0, 0, 0, 1])) with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal( move_cuboid.Pose([0, 0, 0.3], [0, 0, 0, 1])) with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal(move_cuboid.Pose([0, 0, 0], [0, 0, 0, 1])) with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal( move_cuboid.Pose([0, 0, -0.01], [0, 0, 0, 1])) # valid CoM position but rotation makes it reach out of valid range with self.assertRaises(move_cuboid.InvalidGoalError): move_cuboid.validate_goal( move_cuboid.Pose([0, 0, on_ground_height], full_rotation))
def test_get_cube_corner_positions(self): # cube half width chw = move_cuboid._CUBOID_HALF_SIZE # no transformation expected_origin_corners = np.array([ [-chw[0], -chw[1], -chw[2]], [-chw[0], -chw[1], +chw[2]], [-chw[0], +chw[1], -chw[2]], [-chw[0], +chw[1], +chw[2]], [+chw[0], -chw[1], -chw[2]], [+chw[0], -chw[1], +chw[2]], [+chw[0], +chw[1], -chw[2]], [+chw[0], +chw[1], +chw[2]], ]) origin = move_cuboid.Pose() origin_corners = move_cuboid.get_cube_corner_positions(origin) np.testing.assert_array_almost_equal(expected_origin_corners, origin_corners) # only translation expected_translated_corners = np.array([ [-chw[0] + 1, -chw[1] + 2, -chw[2] + 3], [-chw[0] + 1, -chw[1] + 2, +chw[2] + 3], [-chw[0] + 1, +chw[1] + 2, -chw[2] + 3], [-chw[0] + 1, +chw[1] + 2, +chw[2] + 3], [+chw[0] + 1, -chw[1] + 2, -chw[2] + 3], [+chw[0] + 1, -chw[1] + 2, +chw[2] + 3], [+chw[0] + 1, +chw[1] + 2, -chw[2] + 3], [+chw[0] + 1, +chw[1] + 2, +chw[2] + 3], ]) translated = move_cuboid.get_cube_corner_positions( move_cuboid.Pose([1, 2, 3], [0, 0, 0, 1])) np.testing.assert_array_almost_equal(expected_translated_corners, translated) # only rotation rot_z90 = Rotation.from_euler("z", 90, degrees=True).as_quat() expected_rotated_corners = np.array([ [+chw[1], -chw[0], -chw[2]], [+chw[1], -chw[0], +chw[2]], [-chw[1], -chw[0], -chw[2]], [-chw[1], -chw[0], +chw[2]], [+chw[1], +chw[0], -chw[2]], [+chw[1], +chw[0], +chw[2]], [-chw[1], +chw[0], -chw[2]], [-chw[1], +chw[0], +chw[2]], ]) rotated = move_cuboid.get_cube_corner_positions( move_cuboid.Pose([0, 0, 0], rot_z90)) np.testing.assert_array_almost_equal(expected_rotated_corners, rotated) # both rotation and translation expected_both_corners = np.array([ [+chw[1] + 1, -chw[0] + 2, -chw[2] + 3], [+chw[1] + 1, -chw[0] + 2, +chw[2] + 3], [-chw[1] + 1, -chw[0] + 2, -chw[2] + 3], [-chw[1] + 1, -chw[0] + 2, +chw[2] + 3], [+chw[1] + 1, +chw[0] + 2, -chw[2] + 3], [+chw[1] + 1, +chw[0] + 2, +chw[2] + 3], [-chw[1] + 1, +chw[0] + 2, -chw[2] + 3], [-chw[1] + 1, +chw[0] + 2, +chw[2] + 3], ]) both = move_cuboid.get_cube_corner_positions( move_cuboid.Pose([1, 2, 3], rot_z90)) np.testing.assert_array_almost_equal(expected_both_corners, both)