From 468be047aa5975265cbdc5b89740bc6b61079a9e Mon Sep 17 00:00:00 2001 From: ajwahab <1449672+ajwahab@users.noreply.github.com> Date: Sun, 27 Sep 2020 00:10:46 -0400 Subject: [PATCH] robustified 2 joint arm example (#402) 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 75e7cf30..85ed6123 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)))