def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) end_effector = position(1+link2, 1-link1) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, -pi / 2) self.assertAlmostEqual(joints.theta2, pi / 2) end_effector = position(1, 1+link1+link2) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, pi / 2) self.assertAlmostEqual(joints.theta2, 0.0)
def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) joints = jointangle(pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 - link2) self.assertAlmostEqual(end_effector.y, 1 + link1) joints = jointangle(-pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 + link2) self.assertAlmostEqual(end_effector.y, 1 - link1)