github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 14:08:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix for test

Browse Source
This commit is contained in:
Atsushi Sakai
2018-08-15 17:53:43 +09:00
parent 9d1c906b84
commit d6b472cde5
1 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -48,8 +48,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
except ValueError as e:
print("Unreachable goal")
if show_animation:
wrist = plot_arm(theta1, theta2, x, y)
wrist = plot_arm(theta1, theta2, x, y)
# check goal
d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2)
@@ -64,23 +63,24 @@ def plot_arm(theta1, theta2, x, y):
wrist = elbow + \
np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
plt.cla()
if show_animation:
plt.cla()
plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-')
plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-')
plt.plot(shoulder[0], shoulder[1], 'ro')
plt.plot(elbow[0], elbow[1], 'ro')
plt.plot(wrist[0], wrist[1], 'ro')
plt.plot(shoulder[0], shoulder[1], 'ro')
plt.plot(elbow[0], elbow[1], 'ro')
plt.plot(wrist[0], wrist[1], 'ro')
plt.plot([wrist[0], x], [wrist[1], y], 'g--')
plt.plot(x, y, 'g*')
plt.plot([wrist[0], x], [wrist[1], y], 'g--')
plt.plot(x, y, 'g*')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.show()
plt.pause(dt)
plt.show()
plt.pause(dt)
return wrist