def path_from_weld_lines(wlines, dist_res, ang_res, shrink_dist): paths = [] for wl in wlines: if shrink_dist != None: wl["start"], wl["goal"] = shrink_path(wl["start"], wl["goal"], shrink_dist) trans, rots = tf_interpolations(wl["start"], wl["goal"], max_cart_step=dist_res) constraints = cons_list_to_dict(wl["con"]) path = [] for p, R in zip(trans, rots): if "xyz" in constraints.keys(): pos_tol = parse_constraints(constraints["xyz"], dist_res, ang_res) else: pos_tol = [NoTolerance()] * 3 if "rpy" in constraints.keys(): rot_tol = parse_constraints( constraints["rpy"], dist_res, ang_res, angular=True, convert_angles=True, ) else: rot_tol = [NoTolerance()] * 3 path.append( TolEulerPt(p, Quaternion(matrix=R.as_matrix()), pos_tol, rot_tol)) paths.append(path) return paths
def test_grid(self): pos = [1, 2, 3] pos_tol = [NoTolerance()] * 3 rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(np.pi, 3)] pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol) tf_samples = pt.sample_grid() assert_almost_equal(tf_samples[0], tf_samples[2]) assert_almost_equal(tf_samples[1][:3, :3], np.eye(3))
def test_tol_quat_pt_with_weights(): path_ori_free = [] for s in np.linspace(0, 1, 3): xi = 0.8 yi = s * 0.2 + (1 - s) * (-0.2) zi = 0.2 path_ori_free.append( TolQuatPt( [xi, yi, zi], Quaternion(axis=[1, 0, 0], angle=np.pi), [NoTolerance(), NoTolerance(), NoTolerance()], QuaternionTolerance(2.0), ) ) table = Box(0.5, 0.5, 0.1) table_tf = np.array( [[1, 0, 0, 0.80], [0, 1, 0, 0.00], [0, 0, 1, 0.12], [0, 0, 0, 1]] ) scene1 = Scene([table], [table_tf]) robot = Kuka() # robot.tool = torch setup = PlanningSetup(robot, path_ori_free, scene1) # weights to express the importance of the joints in the cost function joint_weights = [10.0, 5.0, 1.0, 1.0, 1.0, 1.0] settings = SamplingSetting( SearchStrategy.INCREMENTAL, sample_method=SampleMethod.random_uniform, num_samples=500, iterations=2, tolerance_reduction_factor=2, weights=joint_weights, ) solve_set = SolverSettings( SolveMethod.sampling_based, CostFuntionType.weighted_sum_squared, sampling_settings=settings, ) sol = solve(setup, solve_set) assert sol.success for qi, s in zip(sol.joint_positions, np.linspace(0, 1, 3)): xi = 0.8 yi = s * 0.2 + (1 - s) * (-0.2) zi = 0.2 fk = robot.fk(qi) pos_fk = fk[:3, 3] assert_almost_equal(pos_fk, np.array([xi, yi, zi]))
def test_incremental(self): pos = [1, 2, 3] pos_tol = [NoTolerance()] * 3 rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(np.pi, 3)] pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol) tf_samples = pt.sample_incremental(10, SampleMethod.random_uniform) euler = [rotation_matrix_to_rpy(tf[:3, :3]) for tf in tf_samples] for i in range(10): assert_almost_equal(euler[i][0], 0) assert_almost_equal(euler[i][1], 0) assert_in_range(euler[i][2], -np.pi, np.pi)
def test_sample_grid_2(self): """ Rotate with pi / 2 around z, give a tolerance along x, expect a grid sampled along y """ tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()] pt = TolPositionPt( [1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol ) tf_samples = pt.sample_grid() g = [tf[:3, 3] for tf in tf_samples] g_desired = np.array([[1, 1.9, 3], [1, 2, 3], [1, 2.1, 3]]) assert_almost_equal(g, g_desired)
def test_complete_problem(): path_ori_free = [] for s in np.linspace(0, 1, 3): xi = 0.8 yi = s * 0.2 + (1 - s) * (-0.2) zi = 0.2 path_ori_free.append( FreeOrientationPt( [xi, yi, zi], [NoTolerance(), NoTolerance(), NoTolerance()])) table = Box(0.5, 0.5, 0.1) table_tf = np.array([[1, 0, 0, 0.80], [0, 1, 0, 0.00], [0, 0, 1, 0.12], [0, 0, 0, 1]]) scene1 = ShapeSoup([table], [table_tf]) robot = Kuka() # robot.tool = torch settings = SamplingSetting( SearchStrategy.INCREMENTAL, 1, SampleMethod.random_uniform, 500, tolerance_reduction_factor=2, ) solve_set = SolverSettings( SolveMethod.sampling_based, CostFuntionType.sum_squared, sampling_settings=settings, ) setup = PlanningSetup(robot, path_ori_free, scene1) sol1 = solve(setup, solve_set) assert sol1.success s2 = SolverSettings( SolveMethod.optimization_based, CostFuntionType.sum_squared, opt_settings=OptSettings(q_init=np.array(sol1.joint_positions)), ) sol2 = solve(setup, s2) assert sol2.success
def test_to_joint_solutions(): for search_strategy in SearchStrategy: tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()] pt = TolPositionPt( [1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol ) robot = DummyRobot(is_colliding=True) settings = setting_generator(search_strategy) if search_strategy is not SearchStrategy.MIN_INCREMENTAL: joint_solutions = pt.to_joint_solutions(robot, settings) assert len(joint_solutions) == 0 else: with pytest.raises(Exception) as info: joint_solutions = pt.to_joint_solutions(robot, settings) msg = "Maximum iterations reached in to_joint_solutions." assert msg in str(info.value)
def test_transform_to_rel_tolerance_deviation(self): tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()] rot_tol = [NoTolerance(), NoTolerance(), SymmetricTolerance(0.1, 3)] quat = Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])) pt = TolEulerPt([1, 2, 3], quat, tol, rot_tol) quat2 = Quaternion(angle=np.pi / 2 - 0.05, axis=np.array([0, 0, 1])) tf = quat2.transformation_matrix tf[:3, 3] = np.array([1.06, 1.91, 3]) p_rel = pt.transform_to_rel_tolerance_deviation(tf) assert p_rel.shape == (6,) p_desired = np.array([-0.09, -0.06, 0.0, 0.0, 0.0, -0.05]) assert_almost_equal(p_rel, p_desired)
def test_state_cost(): robot = Kuka() table = Box(0.5, 0.5, 0.1) table_tf = np.array( [[1, 0, 0, 0.80], [0, 1, 0, 0.00], [0, 0, 1, 0.00], [0, 0, 0, 1]] ) scene1 = Scene([table], [table_tf]) # create path quat = Quaternion(axis=np.array([1, 0, 0]), angle=-3 * np.pi / 4) pos_tol = 3 * [NoTolerance()] # rot_tol = 3 * [NoTolerance()] rot_tol = [ NoTolerance(), SymmetricTolerance(np.pi / 4, 20), SymmetricTolerance(np.pi, 20), ] first_point = TolEulerPt(np.array([0.9, -0.1, 0.2]), quat, pos_tol, rot_tol) # end_position = np.array([0.9, 0.1, 0.2]) # path = create_line(first_point, end_position, 5) path = create_arc( first_point, np.array([0.9, 0.0, 0.2]), np.array([0, 0, 1]), 2 * np.pi, 5 ) planner_settings = SamplingSetting( SearchStrategy.GRID, iterations=1, tolerance_reduction_factor=2, use_state_cost=True, state_cost_weight=10.0, ) solver_settings = SolverSettings( SolveMethod.sampling_based, CostFuntionType.sum_squared, sampling_settings=planner_settings, ) setup = PlanningSetup(robot, path, scene1) sol = solve(setup, solver_settings) assert sol.success
def test_sample_incremental_1(self): """ Rotate with pi / 2 around z, give a tolerance along x, expect a grid sampled along y """ tol = [SymmetricTolerance(0.1, 3), NoTolerance(), NoTolerance()] pt = TolPositionPt( [1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol ) g = pt.sample_incremental(10, SampleMethod.random_uniform) pos_samples = [tf[:3, 3] for tf in g] for sample in pos_samples: assert_almost_equal(sample[0], 1) assert_almost_equal(sample[2], 3) assert_in_range(sample[1], 1.9, 2.1)
def test_sample_grid(self): q = Quaternion() pos_tol = [Tolerance(-0.5, 0.5, 2), Tolerance(-0.5, 0.5, 2), NoTolerance()] point = TolPositionPt([0.5, 0.5, 1], q, pos_tol) grid = point.sample_grid() position_samples = [[0, 0, 1], [1, 0, 1], [0, 1, 1], [1, 1, 1]] for pos, T in zip(position_samples, grid): assert_almost_equal(pos, T[:3, 3])
def test_tol_position_pt_planning_problem(): robot = Kuka() table = Box(0.5, 0.5, 0.1) table_tf = np.array( [[1, 0, 0, 0.80], [0, 1, 0, 0.00], [0, 0, 1, 0.12], [0, 0, 0, 1]] ) scene1 = Scene([table], [table_tf]) # create path quat = Quaternion(axis=np.array([1, 0, 0]), angle=np.pi) tolerance = [NoTolerance(), SymmetricTolerance(0.05, 10), NoTolerance()] first_point = TolPositionPt(np.array([0.9, -0.2, 0.2]), quat, tolerance) # end_position = np.array([0.9, 0.2, 0.2]) # path = create_line(first_point, end_position, 5) path = create_arc( first_point, np.array([0.9, 0.0, 0.2]), np.array([0, 0, 1]), 2 * np.pi, 5 ) planner_settings = SamplingSetting(SearchStrategy.GRID, iterations=1) solver_settings = SolverSettings( SolveMethod.sampling_based, CostFuntionType.sum_squared, sampling_settings=planner_settings, ) setup = PlanningSetup(robot, path, scene1) sol = solve(setup, solver_settings) assert sol.success for qi, pt in zip(sol.joint_positions, path): fk = robot.fk(qi) pos_fk = fk[:3, 3] pos_pt = pt.pos R_pt = pt.rotation_matrix pos_error = R_pt.T @ (pos_fk - pos_pt) assert_almost_equal(pos_error[0], 0) assert_almost_equal(pos_error[2], 0) assert pos_error[1] <= (0.05 + 1e-6) assert pos_error[1] >= -(0.05 + 1e-6)
def create_path(start, stop, n_path, rx_samples=5, ry_samples=5, rz_samples=90): R_pt = rot_z(np.pi / 2) @ rot_x(3 * np.pi / 4) pos_tol = 3 * [NoTolerance()] rot_tol = [ SymmetricTolerance(np.deg2rad(10), rx_samples), SymmetricTolerance(np.deg2rad(15), ry_samples), Tolerance(-np.pi, np.pi, rz_samples), ] start_pt = TolEulerPt(start, Quaternion(matrix=R_pt), pos_tol, rot_tol) return create_line(start_pt, stop, n_path)
def test_to_transform(self): distance = 0.1 q = Quaternion() pos_tol = 3 * [NoTolerance()] distance = 0.1 point = TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(distance)) tf = point.to_transform([1, 2, 3], q) assert_almost_equal( [[1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]], tf )
def test_sample_incremental(self): method = SampleMethod.random_uniform q = Quaternion() pos_tol = 3 * [NoTolerance()] distance = 0.1 point = TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(distance)) samples = point.sample_incremental(100, method) for tf in samples: newquat = Quaternion(matrix=tf) assert Quaternion.distance(q, newquat) <= distance
def test_transform_to_rel_tolerance_deviation(self): tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()] quat = Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])) pt = TolPositionPt([1, 2, 3], quat, tol) tf = quat.transformation_matrix tf[:3, 3] = np.array([1.06, 1.91, 3]) p_rel = pt.transform_to_rel_tolerance_deviation(tf) p_desired = np.array([-0.09, -0.06, 0.0]) assert_almost_equal(p_rel, p_desired)
def test_sample_incremental_2(self): tol = [SymmetricTolerance(0.1, 3), Tolerance(0.2, 1.0, 3), NoTolerance()] pt = TolPositionPt( [1, 2, 3], Quaternion(angle=np.pi / 2, axis=np.array([0, 0, 1])), tol ) g = pt.sample_incremental(10, SampleMethod.random_uniform) pos_samples = [tf[:3, 3] for tf in g] for sample in pos_samples: assert_almost_equal(sample[2], 3) assert_in_range(sample[0], 0, 3) # 1 + (-1, 2) assert_in_range(sample[1], 1.9, 2.1) # 2 + (-0.1, 0.1)
def test_create_arc(self): pos = [1, 0, 3] pos_tol = [NoTolerance(), NoTolerance(), NoTolerance()] rot_tol = [ SymmetricTolerance(0.5, 10), SymmetricTolerance(0.5, 10), SymmetricTolerance(0.5, 10), ] start_pt = TolEulerPt(pos, Quaternion(), pos_tol, rot_tol) path = create_arc(start_pt, np.zeros(3), np.array([0, 0, 1]), np.pi / 2, 3) assert len(path) == 3 p1, p2, p3 = path[0], path[1], path[2] assert_almost_equal(p1.pos[0], 1) assert_almost_equal(p1.pos[1], 0) assert_almost_equal(p1.pos[2], 3) assert_almost_equal(p2.pos[0], np.sqrt(2) / 2) assert_almost_equal(p2.pos[1], np.sqrt(2) / 2) assert_almost_equal(p2.pos[2], 3) assert_almost_equal(p3.pos[0], 0) assert_almost_equal(p3.pos[1], 1) assert_almost_equal(p3.pos[2], 3)
def parse_constraints(c, dist_res, ang_res, angular=False, convert_angles=False): tol = [] for lower, upper in zip(c["min"], c["max"]): if lower == upper: tol.append(NoTolerance()) else: if convert_angles: lower = np.deg2rad(lower) upper = np.deg2rad(upper) if angular: num_samples = int( np.ceil((upper - lower) / np.deg2rad(ang_res))) else: num_samples = int(np.ceil((upper - lower) / dist_res)) tol.append(Tolerance(lower, upper, num_samples)) return tol
def test_exceptions(): settings = SamplingSetting( SearchStrategy.INCREMENTAL, sample_method=SampleMethod.random_uniform, num_samples=500, iterations=2, tolerance_reduction_factor=2, ) solve_set = SolverSettings( SolveMethod.sampling_based, CostFuntionType.weighted_sum_squared, sampling_settings=settings, ) setup = PlanningSetup(None, None, None) with pytest.raises(Exception) as e: solve(setup, solve_set) assert ( str(e.value) == "No weights specified in SamplingSettings for the weighted cost function." ) robot = Kuka() scene = Scene([], []) pos = np.array([1000, 0, 0]) quat = Quaternion(axis=np.array([1, 0, 0]), angle=-3 * np.pi / 4) path = [TolPositionPt(pos, quat, 3 * [NoTolerance()])] solve_set2 = SolverSettings( SolveMethod.sampling_based, CostFuntionType.sum_squared, sampling_settings=settings, ) setup2 = PlanningSetup(robot, path, scene) with pytest.raises(Exception) as e: solve(setup2, solve_set2) assert str(e.value) == f"No valid joint solutions for path point {0}."
def test_create(self): TolPositionPt([1, 2, 3], Quaternion(), 3 * [NoTolerance()])
def test_create(self): q = Quaternion() pos_tol = 3 * [NoTolerance()] TolQuatPt([1, 2, 3], q, pos_tol, QuaternionTolerance(0.5))
from acrobotics.path.sampling import SampleMethod from acrobotics.path.tolerance import ( Tolerance, NoTolerance, SymmetricTolerance, QuaternionTolerance, ) from acrobotics.path.path_pt import TolPositionPt, TolEulerPt, TolQuatPt # ================================================================================== # Position samples # ================================================================================== R_pt = rot_z(np.deg2rad(30)) pos_pt = np.array([0.5, 0.5, 0.0]) pos_tol = [Tolerance(-0.1, 0.1, 3), Tolerance(-0.3, 0.3, 5), NoTolerance()] pt_pos = TolPositionPt(pos_pt, Quaternion(matrix=R_pt), pos_tol) # ================================================================================== # Euler constraints samples # ================================================================================== R_pt = np.eye(3) pos_tol = 3 * [NoTolerance()] rot_tol = [ SymmetricTolerance(np.deg2rad(15), 5), NoTolerance(), Tolerance(0, np.pi / 2, 10), ] pt_eul = TolEulerPt(np.zeros(3), Quaternion(matrix=R_pt), pos_tol, rot_tol)