def __init__(self, a2=1, a3=1):
geometry = [
Scene([Box(0.3, 0.3, 0.1)], [translation(0, 0, 0.05)]),
Scene([Box(a2, 0.1, 0.1)], [translation(-a2 / 2, 0, 0)]),
Scene([Box(a3, 0.1, 0.1)], [translation(-a3 / 2, 0, 0)]),
]
super().__init__([
Link(DHLink(0, PI / 2, 0, 0), JointType.revolute, geometry[0]),
Link(DHLink(a2, 0, 0, 0), JointType.revolute, geometry[1]),
Link(DHLink(a3, 0, 0, 0), JointType.revolute, geometry[2]),
])
def __init__(self, a1=1, a2=1, a3=1):
tf1 = translation(-a1 / 2, 0, 0)
tf2 = translation(-a2 / 2, 0, 0)
tf3 = translation(-a3 / 2, 0, 0)
geometry = [
Scene([Box(a1, 0.1, 0.1)], [tf1]),
Scene([Box(a2, 0.1, 0.1)], [tf2]),
Scene([Box(a3, 0.1, 0.1)], [tf3]),
]
super().__init__([
Link(DHLink(a1, 0, 0, 0), JointType.revolute, geometry[0]),
Link(DHLink(a2, 0, 0, 0), JointType.revolute, geometry[1]),
Link(DHLink(a3, 0, 0, 0), JointType.revolute, geometry[2]),
])
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_translation(self):
A = translation(1, 2, -3)
B = pose_z(0, 1, 2, -3)
assert_almost_equal(A, B)
import acrobotics as ab # ====================================================== # Get a ready to go robot implementation # ====================================================== robot = ab.Kuka() # ====================================================== # Create planning scene # ====================================================== # A transform matrix is represented by a 4x4 numpy array # Here we create one using a helper function for readability from acrolib.geometry import translation table = ab.Box(2, 2, 0.1) T_table = translation(0, 0, -0.2) obstacle = ab.Box(0.2, 0.2, 1.5) T_obs = translation(0, 0.5, 0.55) scene = ab.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)