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

fixed TypeError (#414)

Browse Source 
* fixed TypeError

* updated global variables

* updated global variables

* fix comment

* Change try-except to if
This commit is contained in:
Raghuram Shankar
2020-10-10 03:51:16 +02:00
committed by GitHub
parent b3ab809890
commit 1b1e649b7d
1 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -34,9 +34,14 @@ if show_animation:
def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
"""
Computes the inverse kinematics for a planar 2DOF arm
When out of bounds, rewrite x and y with last correct values
"""
global x, y
while True:
try:
if x is not None and y is not None:
x_prev = x
y_prev = y
if np.sqrt(x**2 + y**2) > (l1 + l2):
theta2_goal = 0
else:
@@ -54,11 +59,15 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt
except ValueError as e:
print("Unreachable goal")
except TypeError:
x = x_prev
y = y_prev
wrist = plot_arm(theta1, theta2, x, y)
# check goal
d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
if x is not None and y is not None:
d2goal = np.hypot(wrist[0] - x, wrist[1] - y)
if abs(d2goal) < GOAL_TH and x is not None:
return theta1, theta2
@@ -118,8 +127,8 @@ def main(): # pragma: no cover
fig = plt.figure()
fig.canvas.mpl_connect("button_press_event", click)
# for stopping simulation with the esc key.
fig.canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
fig.canvas.mpl_connect('key_release_event', lambda event: [
exit(0) if event.key == 'escape' else None])
two_joint_arm()