def test_out_of_reach_exception(self):
a = 2
b = 1
c = 1
pos = [4, 0, 0]
limits = [[-1, 1], [-2, 2], [-1, 1]]
with self.assertRaises(ValueError):
scara.inverse_kinematics(pos, a, b, c, limits)
def test_limit_exception(self):
a = 2
b = 1
c = 1
pos = [2, 1, 0]
limits = [[-0.1, 0.1], [-2, 1], [-1, 1]]
with self.assertRaises(ValueError):
print(scara.inverse_kinematics(pos, a, b, c, limits))
def test_invariant(self):
a = 2
b = 1
c = 1
pos = [2, 0.5, 0]
joints = scara.inverse_kinematics(pos, a, b, c, [[-1, 1], [-2, 2], [-1, 1]])
pos_res = scara.forward_kinematics(joints, a, b, c)
self.assertAlmostEqual(pos[0], pos_res[0])
self.assertAlmostEqual(pos[1], pos_res[1])
self.assertAlmostEqual(pos[2], pos_res[2])
def move_hand(self, x, y, z, theta):
limits = [self.joint_limits['shoulder'], self.joint_limits['elbow'], self.joint_limits['z']]
shoulder, elbow, z_actuator = scara.inverse_kinematics([x, y, z],
self.upper_arm,
self.forearm,
self.z_meter_per_rad,
limits,
self.negative_elbow_angle)
self.joints['shoulder'] = shoulder
self.joints['elbow'] = elbow
self.joints['z'] = z_actuator
self.joints['wrist'] = theta - shoulder - elbow