mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 16:57:58 -05:00
84 lines
2.1 KiB
Python
84 lines
2.1 KiB
Python
"""
|
|
Inverse kinematics of a two-joint arm
|
|
Left-click the plot to set the goal position of the end effector
|
|
|
|
Author: Daniel Ingram (daniel-s-ingram)
|
|
"""
|
|
from __future__ import print_function, division
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
from math import sin, cos, atan2, acos, pi
|
|
|
|
Kp = 15
|
|
dt = 0.01
|
|
|
|
#Link lengths
|
|
l1 = l2 = 1
|
|
|
|
shoulder = np.array([0, 0])
|
|
|
|
#Set initial goal position to the initial end-effector position
|
|
x = 2
|
|
y = 0
|
|
|
|
plt.ion()
|
|
|
|
def two_joint_arm():
|
|
"""
|
|
Computes the inverse kinematics for a planar 2DOF arm
|
|
"""
|
|
theta1 = theta2 = 0
|
|
while True:
|
|
try:
|
|
theta2_goal = acos((x**2 + y**2 - l1**2 -l2**2)/(2*l1*l2))
|
|
theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
|
|
|
|
if theta1_goal < 0:
|
|
theta2_goal = -theta2_goal
|
|
theta1_goal = atan2(y, x) - atan2(l2*sin(theta2_goal), (l1 + l2*cos(theta2_goal)))
|
|
|
|
theta1 = theta1 + Kp*ang_diff(theta1_goal, theta1)*dt
|
|
theta2 = theta2 + Kp*ang_diff(theta2_goal, theta2)*dt
|
|
except ValueError as e:
|
|
print("Unreachable goal")
|
|
|
|
plot_arm(theta1, theta2, x, y)
|
|
|
|
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)
|
|
|
|
def ang_diff(theta1, theta2):
|
|
#Returns the difference between two angles in the range -pi to +pi
|
|
return (theta1-theta2+pi)%(2*pi)-pi
|
|
|
|
def click(event):
|
|
global x, y
|
|
x = event.xdata
|
|
y = event.ydata
|
|
|
|
if __name__ == "__main__":
|
|
fig = plt.figure()
|
|
fig.canvas.mpl_connect("button_press_event", click)
|
|
two_joint_arm()
|