github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-18 12:31:19 -05:00
Code Issues Packages Projects Releases Wiki Activity

robustified 2 joint arm example (#402)

Browse Source 
two joint arm point control example will no longer fail if target point is out of reach
This commit is contained in:
ajwahab
2020-09-27 00:10:46 -04:00
committed by GitHub
parent d7cc364b2f
commit 468be047aa
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -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)))