def test_get_normals(self): b = Box(1, 2, 3) n = b.get_normals(tf_identity) desired = np.array( [[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]] ) assert_almost_equal(n, desired)
def test_polyhedron(self): b = Box(1, 2, 3) A, b = b.get_polyhedron(np.eye(4)) Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]) ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5]) assert_almost_equal(A, Aa) assert_almost_equal(b, ba)
def test_get_edges(self): b = Box(1, 2, 3) e = b.get_edges(tf_identity) row, col = e.shape assert row == 12 assert col == 6 v = b.get_vertices(tf_identity) # check only one edge v0 = np.hstack((v[0], v[1])) assert_almost_equal(v0, e[0])
def test_polyhedron_transformed(self): b = Box(1, 2, 3) tf = pose_z(0.3, 0.1, 0.2, -0.3) A, b = b.get_polyhedron(tf) Aa = np.array([[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]) ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5]) Aa = np.dot(Aa, tf[:3, :3].T) ba = ba + np.dot(Aa, tf[:3, 3]) assert_almost_equal(A, Aa) assert_almost_equal(b, ba)
def test_plot(self): tf2 = pose_z(np.pi / 6, 9, 0, 0) soup = Scene([Box(1, 1, 1), Box(2, 2, 0.5)], [tf_identity, tf2]) fig = plt.figure() ax = fig.gca(projection="3d") ax.set_xlim([0, 10]) ax.set_ylim([-5, 5]) ax.set_zlim([-5, 5]) soup.plot(ax, c="g") tf3 = pose_z(-np.pi / 4, 0, 3, 0) soup.plot(ax, tf=tf3, c="r")
def test_get_vertices(self): b = Box(1, 2, 3) v = b.get_vertices(tf_identity) desired = np.array([ [-0.5, 1, 1.5], [-0.5, 1, -1.5], [-0.5, -1, 1.5], [-0.5, -1, -1.5], [0.5, 1, 1.5], [0.5, 1, -1.5], [0.5, -1, 1.5], [0.5, -1, -1.5], ]) assert_almost_equal(v, desired)
def test_is_in_collision(self): b1 = Box(1, 1, 1) b2 = Box(1, 1, 2) actual = b1.is_in_collision(tf_identity, b2, tf_identity) assert actual == True b3 = Box(1, 2, 1) T3 = pose_z(np.pi / 4, 0.7, 0.7, 0) assert b1.is_in_collision(tf_identity, b3, T3) == True b4 = Box(1, 1, 1) b5 = Box(1, 1, 2) T4 = pose_z(0, -1, -1, 0) T5 = pose_z(np.pi / 4, -2, -2, 0) assert b4.is_in_collision(T4, b5, T5) == False
def test_kuka_collision(self): bot = Kuka() q0 = [0, np.pi / 2, 0, 0, 0, 0] obj1 = ShapeSoup( [Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)], [pose_x(0, 0.75, 0, 0.5), pose_x(0, 0.75, 0.5, 0.5)], ) obj2 = ShapeSoup( [Box(0.2, 0.3, 0.5), Box(0.1, 0.3, 0.1)], [pose_x(0, 0.3, -0.7, 0.5), pose_x(0, 0.75, 0.5, 0.5)], ) a1 = bot.is_in_collision(q0, obj1) assert a1 is True a2 = bot.is_in_collision(q0, obj2) assert a2 is False
def test_ccd_3(): table = Box(2, 2, 0.1) T_table = translation(0, 0, -0.2) obstacle = Box(0.01, 0.2, 0.2) T_obs = translation(0, 1.2, 0) scene = Scene([table, obstacle], [T_table, T_obs]) q_start = np.array([1.0, 1.2, -0.5, 0, 0, 0]) q_goal = np.array([2.0, 1.2, -0.5, 0, 0, 0]) res = robot.is_path_in_collision(q_start, q_goal, scene) assert res if DEBUG: print("resut test 3: ", res) show_animation(robot, scene, q_start, q_goal)
def test_set_transform(self): b = Box(1, 2, 3) tf = np.eye(4) tf[0, 3] = 10.5 v = b.get_vertices(tf) desired = np.array([ [10, 1, 1.5], [10, 1, -1.5], [10, -1, 1.5], [10, -1, -1.5], [11, 1, 1.5], [11, 1, -1.5], [11, -1, 1.5], [11, -1, -1.5], ]) assert_almost_equal(v, desired)
def test_init(self): dh_params = DHLink(0.1, np.pi / 4, -0.1, np.pi / 6) geometry = Scene([Box(1, 2, 3)], [np.eye(4)]) link1 = Link(dh_params, JointType.revolute, geometry) fig = plt.figure() ax = fig.gca(projection="3d") link1.plot(ax, np.eye(4))
def test_set_transform2(self): b = Box(1, 2, 3) tf = np.eye(4) # rotate pi / 2 around x-axis tf[1:3, 1:3] = np.array([[0, -1], [1, 0]]) v = b.get_vertices(tf) desired = np.array([ [-0.5, -1.5, 1], [-0.5, 1.5, 1], [-0.5, -1.5, -1], [-0.5, 1.5, -1], [0.5, -1.5, 1], [0.5, 1.5, 1], [0.5, -1.5, -1], [0.5, 1.5, -1], ]) assert_almost_equal(v, desired)
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_polyhedron(self): b = Box(1, 2, 3) tf = pose_z(0.3, 0.1, 0.2, -0.3) col = Scene([b], [tf]) polys = col.get_polyhedrons() assert len(polys) == 1 Aa = np.array( [[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]] ) ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5]) Aa = np.dot(Aa, tf[:3, :3].T) ba = ba + np.dot(Aa, tf[:3, 3]) assert_almost_equal(polys[0].A, Aa) assert_almost_equal(polys[0].b, ba)
def parse_link(link): """ Assume a link has only a single collision object. Assume this collision object is a box. Assume the link named "world" has no collision objects. Parameters ---------- link: a urdfpy.urdf.Link object """ assert len(link.collisions) == 1 c = link.collisions[0] assert c.geometry.box is not None size = c.geometry.box.size return Box(size[0], size[1], size[2])
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_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_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_scene(position): """ Welding case.""" obs_h = 0.03 obs_off = 0.04 L = 0.5 # length of all stuff in y-dir obs_l = L / 6 u_off = 0.005 # U-profile # obstacle # table # side U-profile 1 # side U-profile 2 dims = [ [0.27, L, 0.21], [0.2, obs_l, obs_h], [0.7, 0.7, 0.1], [0.2, 0.1, 0.055], [0.2, 0.1, 0.055], ] pos = [ [dims[0][0] / 2, 0, dims[0][2] / 2], [-(dims[1][0] / 2) - obs_off, 0, obs_h / 2], [0, 0, -0.10], [-dims[3][0] / 2, L / 2 - dims[3][1] / 2, dims[3][2] / 2], [-dims[3][0] / 2, -L / 2 + dims[3][1] / 2, dims[3][2] / 2], ] pos = [p + position for p in pos] shapes = [Box(d[0], d[1], d[2]) for d in dims] tfs = [translation(p[0], p[1], p[2]) for p in pos] path_start = np.array([0, -L / 2 + dims[3][1] + u_off, 0]) + position path_stop = np.array([0, L / 2 - dims[3][1] - u_off, 0]) + position return Scene(shapes, tfs), path_start, path_stop
def test_is_in_collision(self): soup_1 = Scene([Box(1, 1, 1)], [tf_identity]) soup_2 = Scene([Box(1, 1, 2)], [tf_identity]) assert soup_1.is_in_collision(soup_2) == True assert soup_1.is_in_collision(soup_2, tf_self=tf_far_away) == False assert soup_1.is_in_collision(soup_2, tf_other=tf_far_away) == False soup_3 = Scene( [Box(1, 1, 1), Box(0.3, 0.2, 0.1)], [pose_z(0.0, -1.0, -1.0, 0.0), -tf_far_away], ) soup_4 = Scene( [Box(1, 1, 2), Box(0.3, 0.2, 0.1)], [pose_z(np.pi / 4, -2, -2, 0), tf_far_away], ) assert soup_3.is_in_collision(soup_4) == False assert soup_4.is_in_collision(soup_3) == False
import time import numpy as np from numpy.random import uniform from acrobotics.shapes import Box from acrobotics.util import pose_x box1 = Box(1, 0.5, 2) box2 = Box(0.7, 1.5, 1) def check_random_collision(): tf1 = pose_x(uniform(-2, 2), uniform(0, 3), 0, 0) tf2 = pose_x(uniform(-2, 2), 0, uniform(0, 3), 0) return box1.is_in_collision(tf1, box2, tf2) N = 1000 run_times = np.zeros(N) results = np.zeros(N, dtype=bool) for i in range(N): start = time.time() res = check_random_collision() end = time.time() run_times[i] = end - start results[i] = res print(f"{N} runs took an average of {np.mean(run_times)*1000} ms per run") print(f"Not in collion average: {1000 * np.mean(run_times[results == False])}")
# ====================================================== from acrobotics.robot_examples import Kuka robot = Kuka() # ====================================================== # Create planning scene # ====================================================== from acrobotics.geometry import Scene from acrobotics.shapes import Box # A transform matrix is represented by a 4x4 numpy array # Here we create one using a helper function for readability from acrobotics.util import translation table = Box(2, 2, 0.1) T_table = translation(0, 0, -0.2) obstacle = Box(0.2, 0.2, 1.5) T_obs = translation(0, 0.5, 0.55) scene = Scene([table, obstacle], [T_table, T_obs]) # ====================================================== # Linear interpolation path from start to goal # ====================================================== import numpy as np q_start = np.array([0.5, 1.5, -0.3, 0, 0, 0]) q_goal = np.array([2.5, 1.5, 0.3, 0, 0, 0]) q_path = np.linspace(q_start, q_goal, 10)
def test_plot(self): b1 = Box(1, 2, 3) fig = plt.figure() ax = fig.gca(projection="3d") b1.plot(ax, tf_identity) assert True
def test_init(self): Box(1, 2, 3)