def test_Kuka_on_rail_robot(self):
bot = KukaOnRail()
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi[1:])
Tdesired = pose_x(PI / 2, 0, 0, qi[0]) @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_PlanarArm_base(self):
robot1.tf_base = pose_x(1.5, 0.3, 0.5, 1.2)
q_test = self.generate_random_configurations(robot1)
for qi in q_test:
Tactual = robot1.fk(qi)
Tdesired = fki.fk_PlanarArm(qi, robot1.links)
Tdesired = robot1.tf_base @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_kuka_collision(self):
bot = Kuka()
q0 = [0, np.pi / 2, 0, 0, 0, 0]
obj1 = Scene(
[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 = Scene(
[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_Kuka_base_robot(self):
bot = Kuka()
bot.tf_base = pose_x(0.5, 0.1, 0.2, 0.3)
q_test = self.generate_random_configurations(bot)
for qi in q_test:
Tactual = bot.fk(qi)
Tdesired = fki.fk_kuka(qi)
Tdesired = bot.tf_base @ Tdesired
assert_almost_equal(Tactual, Tdesired)
def test_sw_random_other_base(self):
bot = SphericalWrist()
bot.tf_base = pose_x(1.5, 0.3, 0.5, 1.2)
N = 20
q_rand = np.random.rand(N, 3) * 2 * PI - PI
for qi in q_rand:
T1 = bot.fk(qi)
resi = bot.ik(T1)
for q_sol in resi.solutions:
R2 = bot.fk(q_sol)[:3, :3]
assert_almost_equal(T1[:3, :3], R2, decimal=5)
def __init__(self, a1=0.18, a2=0.6, d4=0.62, d6=0.115):
s = [
Box(0.2, 0.2, 0.1),
Box(0.3, 0.2, 0.1),
Box(0.8, 0.2, 0.1),
Box(0.2, 0.1, 0.5),
Box(0.1, 0.2, 0.1),
Box(0.1, 0.1, 0.085),
Box(0.1, 0.1, 0.03),
]
tfs = [
pose_x(0, 0, 0, -0.15),
pose_x(0, -0.09, 0, 0.05),
pose_x(0, -0.3, 0, -0.05),
pose_x(0, 0, 0.05, 0.17),
pose_x(0, 0, 0.1, 0),
pose_x(0, 0, 0, 0.085 / 2),
pose_x(0, 0, 0, -0.03 / 2),
]
# create robot
super().__init__([
Link(
DHLink(0, PI / 2, 0, 0),
JointType.prismatic,
Scene([s[0]], [tfs[0]]),
),
Link(
DHLink(a1, PI / 2, 0, 0),
JointType.revolute,
Scene([s[1]], [tfs[1]]),
),
Link(DHLink(a2, 0, 0, 0), JointType.revolute,
Scene([s[2]], [tfs[2]])),
Link(
DHLink(0, PI / 2, 0, 0),
JointType.revolute,
Scene([s[3]], [tfs[3]]),
),
Link(
DHLink(0, -PI / 2, d4, 0),
JointType.revolute,
Scene([s[4]], [tfs[4]]),
),
Link(
DHLink(0, PI / 2, 0, 0),
JointType.revolute,
Scene([s[5]], [tfs[5]]),
),
Link(DHLink(0, 0, d6, 0), JointType.revolute,
Scene([s[6]], [tfs[6]])),
])
self.kuka = Kuka(a1=0.18, a2=0.6, d4=0.62, d6=0.115)
def __init__(self, a1=0.18, a2=0.6, d4=0.62, d6=0.115):
# define kuka collision shapes
s = [
Box(0.3, 0.2, 0.1),
Box(0.8, 0.2, 0.1),
Box(0.2, 0.1, 0.5),
Box(0.1, 0.2, 0.1),
Box(0.1, 0.1, 0.085),
Box(0.1, 0.1, 0.03),
]
# define transforms for collision shapes
tfs = [
pose_x(0, -0.09, 0, 0.05),
pose_x(0, -0.3, 0, -0.05),
pose_x(0, 0, 0.05, 0.17),
pose_x(0, 0, 0.1, 0),
pose_x(0, 0, 0, 0.085 / 2),
pose_x(0, 0, 0, -0.03 / 2),
]
dh_links = [
DHLink(a1, PI / 2, 0, 0),
DHLink(a2, 0, 0, 0),
DHLink(0, PI / 2, 0, 0),
DHLink(0, -PI / 2, d4, 0),
DHLink(0, PI / 2, 0, 0),
DHLink(0, 0, d6, 0),
]
geoms = [Scene([shape], [tf]) for shape, tf in zip(s, tfs)]
jls = [
JointLimit(np.deg2rad(-155), np.deg2rad(155)),
JointLimit(np.deg2rad(-180), np.deg2rad(65)),
JointLimit(np.deg2rad(-15), np.deg2rad(158)),
JointLimit(np.deg2rad(-350), np.deg2rad(350)),
JointLimit(np.deg2rad(-130), np.deg2rad(130)),
JointLimit(np.deg2rad(-350), np.deg2rad(350)),
]
# create robot
super().__init__([
Link(dh_links[i], JointType.revolute, geoms[i]) for i in range(6)
], jls)
self.arm = Arm2(a1=a1, a2=a2, a3=d4)
self.wrist = SphericalWrist(d3=d6)
def test_pose_x(self):
A = pose_x(0.6, 1, 2, 3)
assert_almost_equal(A[0, :3], np.array([1, 0, 0]))
assert_almost_equal(A[:3, 0], np.array([1, 0, 0]))
assert_almost_equal(A[:3, 3], np.array([1, 2, 3]))
assert_almost_equal(A[3, :], np.array([0, 0, 0, 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)