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

Planar 2DOF arm

Browse Source
This commit is contained in:
Daniel Ingram
2018-08-13 21:50:00 -04:00
parent 3988e21051
commit df8e91dea8
1 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
RoboticArm/two_joint_arm.py
View File

@@ -49,16 +49,22 @@ def two_joint_arm():
def plot_arm(theta1, theta2, x, y):
elbow = shoulder + np.array([l1*cos(theta1), l1*sin(theta1)])
wrist = elbow + np.array([l2*cos(theta1+theta2), l2*sin(theta1+theta2)])
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], 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.xlim(-2, 2)
plt.ylim(-2, 2)
plt.show()
plt.pause(dt)