Permalink
Branch: master
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
114 lines (93 sloc) 3.83 KB
import numpy as np
import math
import matplotlib.pyplot as plt
from RobotArm import *
# Instantiate robot arm class.
Arm = RobotArm2D()
# Add desired number of joints/links to robot arm object.
Arm.add_revolute_link(length=3, thetaInit=math.radians(10))
Arm.add_revolute_link(length=3, thetaInit=math.radians(15))
Arm.add_revolute_link(length=3, thetaInit=math.radians(20))
Arm.update_joint_coords()
# Initialize target coordinates to current end effector position.
target = Arm.joints[:,[-1]]
# Initialize plot and line objects for target, end effector, and arm.
fig, ax = plt.subplots(figsize=(5,5))
fig.subplots_adjust(left=0, bottom=0, right=1, top=1)
targetPt, = ax.plot([], [], marker='o', c='r')
endEff, = ax.plot([], [], marker='o', markerfacecolor='w', c='g', lw=2)
armLine, = ax.plot([], [], marker='o', c='g', lw=2)
# Determine maximum reach of arm.
reach = sum(Arm.lengths)
# Set axis limits based on reach from root joint.
ax.set_xlim(Arm.xRoot - 1.2 * reach, Arm.xRoot + 1.2 * reach)
ax.set_ylim(Arm.yRoot - 1.2 * reach, Arm.yRoot + 1.2 * reach)
# Add dashed circle to plot indicating reach.
circle = plt.Circle((Arm.xRoot, Arm.yRoot), reach, ls='dashed', fill=False)
ax.add_artist(circle)
def update_plot():
'''Update arm and end effector line objects with current x and y
coordinates from arm object.
'''
armLine.set_data(Arm.joints[0,:-1], Arm.joints[1,:-1])
endEff.set_data(Arm.joints[0,-2::], Arm.joints[1,-2::])
update_plot()
def move_to_target():
'''Run Jacobian inverse routine to move end effector toward target.'''
global Arm, target, reach
# Set distance to move end effector toward target per algorithm iteration.
distPerUpdate = 0.02 * reach
if np.linalg.norm(target - Arm.joints[:,[-1]]) > 0.02 * reach:
targetVector = (target - Arm.joints[:,[-1]])[:3]
targetUnitVector = targetVector / np.linalg.norm(targetVector)
deltaR = distPerUpdate * targetUnitVector
J = Arm.get_jacobian()
JInv = np.linalg.pinv(J)
deltaTheta = JInv.dot(deltaR)
Arm.update_theta(deltaTheta)
Arm.update_joint_coords()
update_plot()
# "mode" can be toggled with the Shift key between 1 (click to set
# target location) and -1 (target moves in predefined motion).
mode = 1
def on_button_press(event):
'''Mouse button press event to set target at the location in the
plot where the left mousebutton is clicked.
'''
global target, targetPt
xClick = event.xdata
yClick = event.ydata
# Ensure that the x and y click coordinates are within the axis limits
# by checking that they are floats.
if (mode == 1 and event.button == 1 and isinstance(xClick, float)
and isinstance(yClick, float)):
targetPt.set_data(xClick, yClick)
target = np.array([[xClick, yClick, 0, 1]]).T
fig.canvas.mpl_connect('button_press_event', on_button_press)
# Use "exitFlag" to halt while loop execution and terminate script.
exitFlag = False
def on_key_press(event):
'''Key press event to stop script execution if Enter is pressed,
or toggle mode if Shift is pressed.
'''
global exitFlag, mode
if event.key == 'enter':
exitFlag = True
elif event.key == 'shift':
mode *= -1
fig.canvas.mpl_connect('key_press_event', on_key_press)
# Turn on interactive plotting and show plot.
plt.ion()
plt.show()
print('Select plot window and press Shift to toggle mode or press Enter to quit.')
# Variable "t" is used for moving target mode.
t = 0.
while not exitFlag:
if mode == -1:
targetX = Arm.xRoot + 1.1 * (math.cos(0.12*t) * reach) * math.cos(t)
targetY = Arm.yRoot + 1.1 * (math.cos(0.2*t) * reach) * math.sin(t)
targetPt.set_data(targetX, targetY)
target = np.array([[targetX, targetY, 0, 1]]).T
t += 0.025
move_to_target()
fig.canvas.get_tk_widget().update()