From 4b3284d8652c4c30a1e47ff743087803f625a1eb Mon Sep 17 00:00:00 2001 From: ajwahab <1449672+ajwahab@users.noreply.github.com> Date: Mon, 14 Sep 2020 12:01:36 -0400 Subject: [PATCH] robustified 2 joint arm example two joint arm point control example will no longer fail if target point is out of reach --- .../two_joint_arm_to_point_control.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py index 75e7cf301f..85ed612360 100644 --- a/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py +++ b/ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py @@ -37,8 +37,11 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): """ while True: try: - theta2_goal = np.arccos( - (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) + if np.sqrt(x**2 + y**2) > (l1 + l2): + theta2_goal = 0 + else: + theta2_goal = np.arccos( + (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))