def circleArcAroundPivot(pivot, start, end):
pivotToStart = vector.translate(vector.minus(pivot))(start)
pivotToEnd = vector.translate(vector.minus(pivot))(end)
normalToPlane = vector.crossproduct(pivotToStart,pivotToEnd)
firstRadialDirection = vector.crossproduct(normalToPlane, pivotToStart)
planeCoefficients = pivotToEnd
def dotByPlaneCoefficients(vec):
output = 0
for i in range(0,3):
output += planeCoefficients[i] * vec[i]
return output
t = (dotByPlaneCoefficients(end) - dotByPlaneCoefficients(start))/(dotByPlaneCoefficients(firstRadialDirection))
center = vector.translate(start)(vector.scale(firstRadialDirection,t))
return circleArc(center, start, end)
def calc_pos(self, pos, grip_angle=0.0):
"""Calculate servo values for arm position, returns dict(servos)"""
position = Vector(pos)
grip_angle = float(grip_angle)
# unit vector translation of position on xy plane
xy_unit = Vector(position.x, position.y, 0).unit
# get a grip... vector
gripper = (xy_unit * self.beams['gripper'].mag)
# ... and rotate to angle specified
gripper = rotate(gripper, crossproduct(gripper, Z),
radians(grip_angle))
# Subtract to get Sub Arm (sum of vectors 0 and 1)
composite = position - gripper
# Calculate sub-vectors
# Get angle betweens
try:
arm2compangle = trisss([
self.beams['arm'].mag,
self.beams['forearm'].mag,
composite.mag,
])[1]
except ValueError, m:
raise ValueError("Position is beyond range of motion")
def calc_pos(self, pos, grip_angle=0.0):
"""Calculate servo values for arm position, returns dict(servos)"""
position = Vector(pos)
grip_angle = float(grip_angle)
# unit vector translation of position on xy plane
xy_unit = Vector(position.x, position.y, 0).unit
# get a grip... vector
gripper = (xy_unit * self.beams['gripper'].mag)
# ... and rotate to angle specified
gripper = rotate(gripper, crossproduct(gripper, Z),
radians(grip_angle))
# Subtract to get Sub Arm (sum of vectors 0 and 1)
composite = position - gripper
# Calculate sub-vectors
# Get angle betweens
try:
arm2compangle = trisss([
self.beams['arm'].mag,
self.beams['forearm'].mag,
composite.mag,
])[1]
except ValueError, m:
raise ValueError("Position is beyond range of motion")
class Al5x(object):
"""Represents an AL5x robot arm from Lynxmotion
The state of the arm is represented with by a dict with these keys:
pos: the gripper position in 3-dimensions
gripper_angle: the grippers angle from horizon in degrees
grip: the grippers distance between fingers
wrist_rotate: the angle from center in degrees
Initiation and usage:
arm = Al5x(AL5D, servo_controller=None,
parked_state=dict(pos=(0,8,3)), dt=0.010)
a.move(dict(pos=(-4,6,6), grip_angle=15.0, grip=0.0))
a.move(dict(pos=(4,4,10), grip_angle=0.0, grip=0.5))
a.park()
"""
def __init__(self,
beams,
servo_controller=None,
parked_state=None,
servo_map=None,
avg_speed=15.0,
dt=0.007):
self.beams = dict(
zip(['arm', 'forearm', 'gripper'], map(Vector, beams)))
if servo_controller is not None:
self.sc = servo_controller
else:
self.sc = NullServo()
self.current_state = dict()
# set up parked_state
p = sum(self.beams.values())
ga = 0.0
g = 0.0
wr = 0.0
self.parked_state = dict(pos=p, grip_angle=ga, grip=g, wrist_rotate=wr)
if parked_state is not None:
self.parked_state.update(parked_state)
if servo_map is not None:
self.servo_map = servo_map
else:
self.servo_map = SERVO_MAP
self.avg_speed = avg_speed
self.dt = dt
self.immediate_move(self.parked_state)
def get_state(self):
"""Return current state"""
return dict(self.current_state)
def __zip_state(self, new_state):
"""Returns combined state of current_state and new_state"""
state = dict(self.current_state)
state.update(new_state)
return state
def immediate_move(self, new_state, time=0):
"""Move arm to new_state without interpolation
This bypasses the straight-line and accelleration calculations and
simply finds the servo positions for the new state and steps servos to
it. This will cause a non-linear gripper movement.
"""
state = self.__zip_state(new_state)
servos = dict()
servos.update(self.calc_pos(state['pos'], state['grip_angle']))
servos.update(self.calc_grip(state['grip']))
self.sc.servos(servos, time)
self.current_state.update(state)
def park(self):
"""Moves arm to parked state"""
self.move(self.parked_state)
def calc_grip(self, val):
"""Calculate grip servo value, returns dict(servo)"""
return dict({self.servo_map['grip']: val})
def set_grip(self, val):
self.sc.servos(self.calc_grip(val))
self.current_state.update(dict(grip=val))
def calc_pos(self, pos, grip_angle=0.0):
"""Calculate servo values for arm position, returns dict(servos)"""
position = Vector(pos)
grip_angle = float(grip_angle)
# unit vector translation of position on xy plane
xy_unit = Vector(position.x, position.y, 0).unit
# get a grip... vector
gripper = (xy_unit * self.beams['gripper'].mag)
# ... and rotate to angle specified
gripper = rotate(gripper, crossproduct(gripper, Z),
radians(grip_angle))
# Subtract to get Sub Arm (sum of vectors 0 and 1)
composite = position - gripper
# Calculate sub-vectors
# Get angle betweens
try:
arm2compangle = trisss([
self.beams['arm'].mag,
self.beams['forearm'].mag,
composite.mag,
])[1]
except ValueError, m:
raise ValueError("Position is beyond range of motion")
# get arm vector
arm = composite.unit * self.beams['arm'].mag
# ... and rotate to calculated angle
arm = rotate(arm, crossproduct(arm, Z), arm2compangle)
# the easy part...
forearm = composite - arm
# set servo values
servo_values = dict()
servo_values[self.servo_map['base']] = rad2float(angle(X, xy_unit))
servo_values[self.servo_map['shoulder']] = rad2float(
angle(xy_unit, arm))
servo_values[self.servo_map['elbow']] = rad2float(angle(arm, forearm))
servo_values[self.servo_map['wrist']] = rad2float(
pi / 2 -
angle(forearm, gripper) * sign(forearm.unit.z - gripper.unit.z))
return servo_values