def __init__(self, port, rate=115200, timeout=0.025, debug=False, plot=False, fake=False): self._debugOn = debug self._fake = fake self._driver = DobotDriver(port, rate) if fake: self._driver._ramps = True self._driver._stepCoeff = 20000 self._driver._stopSeq = 0 self._driver._stepCoeffOver2 = self._driver._stepCoeff / 2 self._driver._freqCoeff = self._driver._stepCoeff * 25 else: self._driver.Open(timeout) self._plot = plot if plot: import matplotlib.pyplot as plt self._plt = plt self._kinematics = DobotKinematics(debug=debug) self._toolRotation = 0 self._gripper = 480 # Last directions to compensate backlash. self._lastBaseDirection = 0 self._lastRearDirection = 0 self._lastFrontDirection = 0 # Initialize arms current configuration from accelerometers if fake: self._baseSteps = long(0) self._rearSteps = long(0) self._frontSteps = long(0) else: self.InitializeAccelerometers()
def __init__(self, port, rate=115200, timeout=0.025, debug=False, fake=False): self._debugOn = debug self._fake = fake self._driver = DobotDriver(port, rate) if not fake: self._driver.Open(timeout) self._ik = DobotInverseKinematics(debug=debug) # Initialize arms current configuration from accelerometers if fake: self._baseSteps = long(0) self._rearSteps = long(0) self._foreSteps = long(0) else: # self._baseSteps = long(0) # self._rearSteps = long(0) # self._foreSteps = long(0) accels = self._driver.GetAccelerometers() accelRear = accels[1] accelFore = accels[2] rearAngle = math.pi / 2 - self._driver.accelToRadians(accelRear, accelOffsets[0]) foreAngle = self._driver.accelToRadians(accelFore, accelOffsets[1]) self._baseSteps = long(0) self._rearSteps = long((rearAngle / math.pi / 2.0) * rearArmActualStepsPerRevolution + 0.5) self._foreSteps = long((foreAngle / math.pi / 2.0) * foreArmActualStepsPerRevolution + 0.5) self._driver.SetCounters(self._baseSteps, self._rearSteps, self._foreSteps) print "Initializing with steps:", self._baseSteps, self._rearSteps, self._foreSteps print "Reading back what was set:", self._driver.GetCounters() print "--=========--"
def __init__(self,
port,
rate=115200,
timeout=0.025,
debug=False,
fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if not fake:
self._driver.Open(timeout)
self._ik = DobotInverseKinematics(debug=debug)
self._toolRotation = 0
self._gripper = 480
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._foreSteps = long(0)
else:
self._initializeAccelerometers()
def __init__(self,
port,
rate=115200,
timeout=0.025,
debug=False,
fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if not fake:
self._driver.Open(timeout)
self._ik = DobotInverseKinematics(debug=debug)
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._foreSteps = long(0)
else:
# self._baseSteps = long(0)
# self._rearSteps = long(0)
# self._foreSteps = long(0)
accels = self._driver.GetAccelerometers()
accelRear = accels[1]
accelFore = accels[2]
rearAngle = math.pi / 2 - self._driver.accelToRadians(
accelRear, accelOffsets[0])
foreAngle = self._driver.accelToRadians(accelFore, accelOffsets[1])
self._baseSteps = long(0)
self._rearSteps = long((rearAngle / math.pi / 2.0) *
rearArmActualStepsPerRevolution + 0.5)
self._foreSteps = long((foreAngle / math.pi / 2.0) *
foreArmActualStepsPerRevolution + 0.5)
self._driver.SetCounters(self._baseSteps, self._rearSteps,
self._foreSteps)
print "Initializing with steps:", self._baseSteps, self._rearSteps, self._foreSteps
print "Reading back what was set:", self._driver.GetCounters()
print "--=========--"
def __init__(self,
port,
rate=115200,
timeout=0.025,
debug=False,
plot=False,
fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if fake:
self._driver._ramps = True
self._driver._stepCoeff = 20000
self._driver._stopSeq = 0
self._driver._stepCoeffOver2 = self._driver._stepCoeff / 2
self._driver._freqCoeff = self._driver._stepCoeff * 25
else:
self._driver.Open(timeout)
self._plot = plot
if plot:
import matplotlib.pyplot as plt
self._plt = plt
self._kinematics = DobotKinematics(debug=debug)
self._toolRotation = 0
self._gripper = 480
# Last directions to compensate backlash.
self._lastBaseDirection = 0
self._lastRearDirection = 0
self._lastFrontDirection = 0
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._frontSteps = long(0)
else:
self.InitializeAccelerometers()
def __init__(self, port, rate=115200, timeout=0.025, debug=False, fake=False): self._debugOn = debug self._fake = fake self._driver = DobotDriver(port, rate) if not fake: self._driver.Open(timeout) self._ik = DobotInverseKinematics(debug=debug) self._toolRotation = 0 self._gripper = 480 # Initialize arms current configuration from accelerometers if fake: self._baseSteps = long(0) self._rearSteps = long(0) self._foreSteps = long(0) else: self._initializeAccelerometers()
class Dobot:
def __init__(self,
port,
rate=115200,
timeout=0.025,
debug=False,
plot=False,
fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if fake:
self._driver._ramps = True
self._driver._stepCoeff = 20000
self._driver._stopSeq = 0
self._driver._stepCoeffOver2 = self._driver._stepCoeff / 2
self._driver._freqCoeff = self._driver._stepCoeff * 25
else:
self._driver.Open(timeout)
self._plot = plot
if plot:
import matplotlib.pyplot as plt
self._plt = plt
self._kinematics = DobotKinematics(debug=debug)
self._toolRotation = 0
self._gripper = 480
# Last directions to compensate backlash.
self._lastBaseDirection = 0
self._lastRearDirection = 0
self._lastFrontDirection = 0
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._frontSteps = long(0)
else:
self.InitializeAccelerometers()
def _debug(self, *args):
if self._debugOn:
# Since "print" is not a function the expansion (*) cannot be used
# as it is not an operator. So this is a workaround.
for arg in args:
sys.stdout.write(str(arg))
sys.stdout.write(' ')
print('')
def InitializeAccelerometers(self):
#print("--=========--")
print("Initializing accelerometers")
if self._driver.isFpga():
# In FPGA v1.0 SPI accelerometers are read only when Arduino boots. The readings
# are already available, so read once.
ret = (0, 0, 0, 0, 0, 0, 0)
while not ret[0]:
ret = self._driver.GetAccelerometers()
accelRearX = ret[1]
accelFrontX = ret[4]
rearAngle = piHalf - self._driver.accelToRadians(
accelRearX, accelOffsets[0])
frontAngle = self._driver.accelToRadians(accelFrontX,
accelOffsets[1])
else:
# In RAMPS accelerometers are on I2C bus and can be read at any time. We need to
# read them multiple times to get average as MPU-6050 have greater resolution but are noisy.
# However, due to the interference from the way A4988 holds the motors if none of the
# recommended measures to suppress interference are in place (see open-dobot wiki), or
# in case accelerometers are not connected, we need to give up and assume some predefined pose.
accelRearX = 0
accelRearY = 0
accelRearZ = 0
accelFrontX = 0
accelFrontY = 0
accelFrontZ = 0
successes = 0
for i in range(20):
ret = (0, 0, 0, 0, 0, 0, 0)
attempts = 10
while attempts:
ret = self._driver.GetAccelerometers()
if ret[0]:
successes += 1
accelRearX += ret[1]
accelRearY += ret[2]
accelRearZ += ret[3]
accelFrontX += ret[4]
accelFrontY += ret[5]
accelFrontZ += ret[6]
break
attempts -= 1
if successes > 0:
divisor = float(successes)
rearAngle = piHalf - self._driver.accel3DXToRadians(
accelRearX / divisor, accelRearY / divisor,
accelRearZ / divisor)
frontAngle = -self._driver.accel3DXToRadians(
accelFrontX / divisor, accelFrontY / divisor,
accelFrontZ / divisor)
else:
print(
'Failed to read accelerometers. Make sure they are connected and interference is suppressed. See open-dobot wiki'
)
print('Assuming rear arm vertical and front arm horizontal')
rearAngle = 0
frontAngle = -piHalf
self._baseSteps = long(0)
self._rearSteps = long((rearAngle / piTwo) *
rearArmActualStepsPerRevolution + 0.5)
self._frontSteps = long((frontAngle / piTwo) *
frontArmActualStepsPerRevolution + 0.5)
self._driver.SetCounters(self._baseSteps, self._rearSteps,
self._frontSteps)
#print("Initializing with steps:", self._baseSteps, self._rearSteps, self._frontSteps)
#print("Reading back what was set:", self._driver.GetCounters())
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
#print('Current estimated coordinates:', self._kinematics.coordinatesFromAngles(currBaseAngle, currRearAngle, currFrontAngle))
#print("--=========--")
def _moveArmToAngles(self, baseAngle, rearArmAngle, frontArmAngle,
duration):
self._baseAngle = baseAngle
self._rearAngle = rearArmAngle
self._frontAngle = frontArmAngle
dur = float(duration)
# baseStepLocation = long((baseAngle / 360.0) * baseActualStepsPerRevolution + 0.5)
# rearArmStepLocation = long((abs(rearArmAngle) / 360.0) * rearArmActualStepsPerRevolution + 0.5)
# frontArmStepLocation = long((abs(frontArmAngle) / 360.0) * frontArmActualStepsPerRevolution + 0.5)
baseStepLocation = long(baseAngle * baseActualStepsPerRevolution /
piTwo)
rearArmStepLocation = long(rearArmAngle *
rearArmActualStepsPerRevolution / piTwo)
frontArmStepLocation = long(frontArmAngle *
frontArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Frontarm Step Location", frontArmStepLocation)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
frontDiff = frontArmStepLocation - self._frontSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('frontDiff', frontDiff)
self._baseSteps = baseStepLocation
self._rearSteps = rearArmStepLocation
self._frontSteps = frontArmStepLocation
baseDir = 1
rearDir = 1
frontDir = 1
if (baseDiff < 1):
baseDir = 0
if (rearDiff < 1):
rearDir = 0
if (frontDiff > 1):
frontDir = 0
baseSliced = self._sliceStepsToValues(abs(baseDiff), dur)
rearSliced = self._sliceStepsToValues(abs(rearDiff), dur)
frontSliced = self._sliceStepsToValues(abs(frontDiff), dur)
for base, rear, front in zip(baseSliced, rearSliced, frontSliced):
ret = [0, 0]
# If ret[0] == 0 then command timed out or crc failed.
# If ret[1] == 0 then command queue was full.
while ret[0] == 0 or ret[1] == 0:
ret = self._driver.Steps(base, rear, front, baseDir, rearDir,
frontDir)
def _moveToAnglesSlice(self, baseAngle, rearArmAngle, frontArmAngle,
toolRotation):
baseStepLocation = baseAngle * baseActualStepsPerRevolution / piTwo
rearArmStepLocation = abs(rearArmAngle *
rearArmActualStepsPerRevolution / piTwo)
frontArmStepLocation = abs(frontArmAngle *
frontArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Front Arm Step Location", frontArmStepLocation)
self._debug('self._baseSteps', self._baseSteps)
self._debug('self._rearSteps', self._rearSteps)
self._debug('self._frontSteps', self._frontSteps)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
frontDiff = frontArmStepLocation - self._frontSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('frontDiff', frontDiff)
baseSign = 1
rearSign = 1
frontSign = -1
baseDir = 1
rearDir = 1
frontDir = 1
if (baseDiff < 1):
baseDir = 0
baseSign = -1
if (rearDiff < 1):
rearDir = 0
rearSign = -1
if (frontDiff > 1):
frontDir = 0
frontSign = 1
baseDiffAbs = abs(baseDiff)
rearDiffAbs = abs(rearDiff)
frontDiffAbs = abs(frontDiff)
cmdBaseVal, actualStepsBase, leftStepsBase = self._driver.stepsToCmdValFloat(
baseDiffAbs)
cmdRearVal, actualStepsRear, leftStepsRear = self._driver.stepsToCmdValFloat(
rearDiffAbs)
cmdFrontVal, actualStepsFront, leftStepsFront = self._driver.stepsToCmdValFloat(
frontDiffAbs)
# Compensate for backlash.
# For now compensate only backlash in the base motor as the backlash in the arm motors depends
# on specific task (a laser/brush or push-pull tasks).
if self._lastBaseDirection != baseDir and actualStepsBase > 0:
cmdBaseVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(
baseDiffAbs + backlash)
self._lastBaseDirection = baseDir
# if self._lastRearDirection != rearDir and actualStepsRear > 0:
# cmdRearVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(rearDiffAbs + backlash)
# self._lastRearDirection = rearDir
# if self._lastFrontDirection != frontDir and actualStepsFront > 0:
# cmdFrontVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(frontDiffAbs + backlash)
# self._lastFrontDirection = frontDir
if not self._fake:
# Repeat until the command is queued. May not be queued if queue is full.
ret = (0, 0)
while not ret[1]:
ret = self._driver.Steps(cmdBaseVal, cmdRearVal, cmdFrontVal,
baseDir, rearDir, frontDir,
self._gripper, int(toolRotation))
if self._plot:
self._toPlot1.append(baseDiff)
self._toPlot2.append(actualStepsBase * baseSign)
return (actualStepsBase * baseSign, actualStepsRear * rearSign, actualStepsFront * frontSign,\
leftStepsBase * baseSign, leftStepsRear * rearSign, leftStepsFront * frontSign)
def freqToCmdVal(self, freq):
'''
See DobotDriver.freqToCmdVal()
'''
return self._driver.freqToCmdVal(freq)
def _moveTo(self, x, y, z, duration):
'''
TEMPORARILY UNAVALAILABLE
Left for future refactor.
Moves the arm to the location with provided coordinates. Makes sure it takes exactly
the amount of time provided in "duration" argument to get end effector to the location.
All axes arrive at the same time.
SDK keeps track of the current end effector pose.
The origin is at the arm's base (the rotating base plate - joint1).
Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find
more about reference frame and arm names.
Current limitations:
- rear arm cannot go beyond initial 90 degrees (backwards from initial vertical position),
so plan carefully the coordinates
- front arm cannot go above initial horizontal position
Not yet implemented:
- the move is not linear as motors run the whole path at constant speed
- acceleration/deceleration
- rear arm cannot go beyond 90 degrees (see current limitations) and there are no checks
for that - it will simply go opposite direction instead
'''
xx = float(x)
yy = float(y)
zz = float(z)
radiansToDegrees = 180.0 / math.pi
b = 135.0
c = 160.0
b2 = pow(b, 2)
c2 = pow(c, 2)
self._debug('============================')
r = math.sqrt(pow(xx, 2) + pow(yy, 2))
self._debug('r', r)
z2 = zz - (80.0 + 23.0)
self._debug('z2', z2)
d2 = math.pow(z2, 2) + math.pow(r, 2)
d = math.sqrt(d2)
self._debug('d', d)
self._debug('d2', d2)
q1 = math.atan2(z2, r)
self._debug('q1', q1, q1 * radiansToDegrees)
q2 = math.acos((b2 - c2 + d2) / (2.0 * b * d))
self._debug('q2', q2, q2 * radiansToDegrees)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle, rearAngle * radiansToDegrees)
frontAngle = piHalf - (math.acos(
(b2 + c2 - d2) / (2.0 * b * c)) - rearAngle)
self._debug('ik front angle', frontAngle,
frontAngle * radiansToDegrees)
baseAngle = math.atan2(yy, xx)
self._debug('ik base angle', baseAngle, baseAngle * radiansToDegrees)
self._moveArmToAngles(baseAngle, rearAngle, frontAngle, duration)
def MoveWithSpeed(self, x, y, z, maxSpeed, accel=None, toolRotation=None):
'''
For toolRotation see DobotDriver.Steps() function description (servoRot parameter).
'''
if self._plot:
self._toPlot1 = []
self._toPlot2 = []
maxVel = float(maxSpeed)
xx = float(x)
yy = float(y)
zz = float(z)
if toolRotation == None:
toolRotation = self._toolRotation
elif toolRotation > 1024:
toolRotation = 1024
elif toolRotation < 0:
toolRotation = 0
accelf = None
# Set 100% acceleration to equal maximum velocity if it wasn't provided
if accel == None:
accelf = maxVel
else:
accelf = float(accel)
#print("--=========--")
self._debug('maxVel', maxVel)
self._debug('accelf', accelf)
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
currX, currY, currZ = self._kinematics.coordinatesFromAngles(
currBaseAngle, currRearAngle, currFrontAngle)
vectX = xx - currX
vectY = yy - currY
vectZ = zz - currZ
self._debug('moving from', currX, currY, currZ)
self._debug('moving to', xx, yy, zz)
self._debug('moving by', vectX, vectY, vectZ)
distance = math.sqrt(pow(vectX, 2) + pow(vectY, 2) + pow(vectZ, 2))
self._debug('distance to travel', distance)
# If half the distance is reached before reaching maxSpeed with the given acceleration, then actual
# maximum velocity will be lower, hence total number of slices is determined from half the distance
# and acceleration.
distToReachMaxSpeed = pow(maxVel, 2) / (2.0 * accelf)
if distToReachMaxSpeed * 2.0 >= distance:
timeToAccel = math.sqrt(distance / accelf)
accelSlices = timeToAccel * 50.0
timeFlat = 0
flatSlices = 0
maxVel = math.sqrt(distance * accelf)
# Or else number of slices when velocity does not change is greater than zero.
else:
timeToAccel = maxVel / accelf
accelSlices = timeToAccel * 50.0
timeFlat = (distance - distToReachMaxSpeed * 2.0) / maxVel
flatSlices = timeFlat * 50.0
slices = accelSlices * 2.0 + flatSlices
self._debug('slices to do', slices)
self._debug('accelSlices', accelSlices)
self._debug('flatSlices', flatSlices)
# Acceleration/deceleration in respective axes
accelX = (accelf * vectX) / distance
accelY = (accelf * vectY) / distance
accelZ = (accelf * vectZ) / distance
self._debug('accelXYZ', accelX, accelY, accelZ)
# Vectors in respective axes to complete acceleration/deceleration
segmentAccelX = accelX * pow(timeToAccel, 2) / 2.0
segmentAccelY = accelY * pow(timeToAccel, 2) / 2.0
segmentAccelZ = accelZ * pow(timeToAccel, 2) / 2.0
self._debug('segmentAccelXYZ', segmentAccelX, segmentAccelY,
segmentAccelZ)
# Maximum velocity in respective axes for the segment with constant velocity
maxVelX = (maxVel * vectX) / distance
maxVelY = (maxVel * vectY) / distance
maxVelZ = (maxVel * vectZ) / distance
self._debug('maxVelXYZ', maxVelX, maxVelY, maxVelZ)
# Vectors in respective axes for the segment with constant velocity
segmentFlatX = maxVelX * timeFlat
segmentFlatY = maxVelY * timeFlat
segmentFlatZ = maxVelZ * timeFlat
self._debug('segmentFlatXYZ', segmentFlatX, segmentFlatY, segmentFlatZ)
segmentToolRotation = (toolRotation - self._toolRotation) / slices
self._debug('segmentToolRotation', segmentToolRotation)
commands = 1
leftStepsBase = 0.0
leftStepsRear = 0.0
leftStepsFront = 0.0
if self._plot:
toPlot1 = []
toPlot2 = []
toPlot3 = []
toPlot4 = []
toPlot5 = []
toPlot6 = []
toPlot7 = []
toPlot8 = []
toPlot9 = []
while commands < slices:
self._debug('==============================')
self._debug('slice #', commands)
# If accelerating
if commands <= accelSlices:
t2half = pow(commands / 50.0, 2) / 2.0
nextX = currX + accelX * t2half
nextY = currY + accelY * t2half
nextZ = currZ + accelZ * t2half
# If decelerating
elif commands >= accelSlices + flatSlices:
t2half = pow((slices - commands) / 50.0, 2) / 2.0
nextX = currX + segmentAccelX * 2.0 + segmentFlatX - accelX * t2half
nextY = currY + segmentAccelY * 2.0 + segmentFlatY - accelY * t2half
nextZ = currZ + segmentAccelZ * 2.0 + segmentFlatZ - accelZ * t2half
# Or else moving at maxSpeed
else:
t = abs(commands - accelSlices) / 50.0
nextX = currX + segmentAccelX + maxVelX * t
nextY = currY + segmentAccelY + maxVelY * t
nextZ = currZ + segmentAccelZ + maxVelZ * t
self._debug('moving to', nextX, nextY, nextZ)
nextToolRotation = self._toolRotation + (segmentToolRotation *
commands)
self._debug('nextToolRotation', nextToolRotation)
(baseAngle, rearAngle,
frontAngle) = self._kinematics.anglesFromCoordinates(
nextX, nextY, nextZ)
movedStepsBase, movedStepsRear, movedStepsFront, leftStepsBase, leftStepsRear, leftStepsFront = \
self._moveToAnglesSlice(baseAngle, rearAngle, frontAngle, nextToolRotation)
self._debug('moved', movedStepsBase, movedStepsRear,
movedStepsFront, 'steps')
self._debug('leftovers', leftStepsBase, leftStepsRear,
leftStepsFront)
commands += 1
self._baseSteps += movedStepsBase
self._rearSteps += movedStepsRear
self._frontSteps += movedStepsFront
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
cX, cY, cZ = self._kinematics.coordinatesFromAngles(
currBaseAngle, currRearAngle, currFrontAngle)
if self._plot:
toPlot1.append(cX)
toPlot2.append(cY)
toPlot3.append(cZ)
toPlot4.append(nextX)
toPlot5.append(nextY)
toPlot6.append(nextZ)
toPlot7.append(cX - nextX)
toPlot8.append(cY - nextY)
toPlot9.append(cZ - nextZ)
self._toolRotation = toolRotation
if self._plot:
# linewidth = 1.0
# plt.subplot(131)
# line, = plt.plot(toPlot4, linewidth=linewidth)
# line, = plt.plot(toPlot5, linewidth=linewidth)
# line, = plt.plot(toPlot6, linewidth=linewidth)
# plt.subplot(132)
# line, = plt.plot(toPlot1, linewidth=linewidth)
# line, = plt.plot(toPlot2, linewidth=linewidth)
# line, = plt.plot(toPlot3, linewidth=linewidth)
# plt.subplot(133)
# line, = plt.plot(toPlot7, linewidth=linewidth, label='x')
# line, = plt.plot(toPlot8, linewidth=linewidth, label='y')
# line, = plt.plot(toPlot9, linewidth=linewidth, label='z')
# legend = plt.legend(loc='upper center', shadow=True)
# plt.show()
linewidth = 1.0
self._plt.plot(self._toPlot1, linewidth=linewidth)
self._plt.plot(self._toPlot2, linewidth=2.0)
# make the y ticks integers, not floats
yint = []
locs, labels = self._plt.yticks()
for each in locs:
yint.append(int(each))
self._plt.yticks(yint)
self._plt.show()
def Gripper(self, gripper):
if gripper > 480:
self._gripper = 480
elif gripper < 208:
self._gripper = 208
else:
self._gripper = gripper
self._driver.Steps(0, 0, 0, 0, 0, 0, self._gripper, self._toolRotation)
def Wait(self, waitTime):
'''
See description in DobotDriver.Wait()
'''
self._driver.Wait(waitTime)
def CalibrateJoint(self, joint, forwardCommand, backwardCommand, direction,
pin, pinMode, pullup):
'''
See DobotDriver.CalibrateJoint()
'''
return self._driver.CalibrateJoint(joint, forwardCommand,
backwardCommand, direction, pin,
pinMode, pullup)
def EmergencyStop(self):
'''
See DobotDriver.EmergencyStop()
'''
return self._driver.EmergencyStop()
def LaserOn(self, on):
return self._driver.LaserOn(on)
def PumpOn(self, on):
return self._driver.PumpOn(on)
def ValveOn(self, on):
return self._driver.ValveOn(on)
6. Note the "Raw" data from accelerometers reported on the console. Those are your accelerometers' offsets
7. Turn off power on the arm, disconnect USB cable, mount accelerometers back onto the arm
Author: maxosprojects (March 18 2016)
Additional Authors: <put your name here>
Version: 0.4.0
License: MIT
"""
import math
from DobotDriver import DobotDriver
# driver = DobotDriver('COM4')
driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
# driver = DobotDriver('/dev/tty.BT4-SPP-SerialPort')
# driver.Open(timeout=0.3)
# Offsets must be found using this tool for your Dobot once
# (rear arm, forearm)
offsets = (1024, 1024)
def toEndEffectorHeight(rear, fore):
return 88.0 - 160.0 * math.sin(fore) + 135.0 * math.sin(rear)
while True:
ret = driver.GetAccelerometers()
if ret[0]:
print("Rear arm: {0:10f} | Forearm: {1:10f} | End effector height: {2:10f} | Raw rear arm: {3:4d} | Raw forearm: {4:4d}".format(\
So, if you send only one command "50Hz" then dobot makes exacly one step and stops.
number_of_steps = 50Hz * 20ms = 50 * 0.02 = 1
For 600Hz:
number_of_steps = 600Hz * 20ms = 600 * 0.02 = 12
'''
from DobotDriver import DobotDriver
import time
# driver = DobotDriver('/dev/tty.BT4-SPP-SerialPort')
# driver = DobotDriver('COM4')
driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
successes = 0
i = 0
while True:
ret = driver.isReady()
if ret[0] and ret[1]:
successes += 1
if successes > 10:
print("Dobot ready!")
break
if i > 100:
raise Exception('Comm problem')
gripper = 480
toolRotation = 0
class Dobot:
def __init__(self, port, rate=115200, timeout=0.025, debug=False, fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if not fake:
self._driver.Open(timeout)
self._ik = DobotInverseKinematics(debug=debug)
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._foreSteps = long(0)
else:
# self._baseSteps = long(0)
# self._rearSteps = long(0)
# self._foreSteps = long(0)
accels = self._driver.GetAccelerometers()
accelRear = accels[1]
accelFore = accels[2]
rearAngle = math.pi / 2 - self._driver.accelToRadians(accelRear, accelOffsets[0])
foreAngle = self._driver.accelToRadians(accelFore, accelOffsets[1])
self._baseSteps = long(0)
self._rearSteps = long((rearAngle / math.pi / 2.0) * rearArmActualStepsPerRevolution + 0.5)
self._foreSteps = long((foreAngle / math.pi / 2.0) * foreArmActualStepsPerRevolution + 0.5)
self._driver.SetCounters(self._baseSteps, self._rearSteps, self._foreSteps)
print "Initializing with steps:", self._baseSteps, self._rearSteps, self._foreSteps
print "Reading back what was set:", self._driver.GetCounters()
print "--=========--"
def _debug(self, *args):
if self._debugOn:
# Since "print" is not a function the expansion (*) cannot be used
# as it is not an operator. So this is a workaround.
for arg in args:
print arg,
print
def _moveArmToAngles(self, baseAngle, rearArmAngle, foreArmAngle, duration):
self._baseAngle = baseAngle
self._rearAngle = rearArmAngle
self._foreAngle = foreArmAngle
dur = float(duration)
# baseStepLocation = long((baseAngle / 360.0) * baseActualStepsPerRevolution + 0.5)
# rearArmStepLocation = long((abs(rearArmAngle) / 360.0) * rearArmActualStepsPerRevolution + 0.5)
# foreArmStepLocation = long((abs(foreArmAngle) / 360.0) * foreArmActualStepsPerRevolution + 0.5)
baseStepLocation = long(baseAngle * baseActualStepsPerRevolution / piTwo)
rearArmStepLocation = long(rearArmAngle * rearArmActualStepsPerRevolution / piTwo)
foreArmStepLocation = long(foreArmAngle * foreArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Forearm Step Location", foreArmStepLocation)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
foreDiff = foreArmStepLocation - self._foreSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('foreDiff', foreDiff)
self._baseSteps = baseStepLocation
self._rearSteps = rearArmStepLocation
self._foreSteps = foreArmStepLocation
baseDir = 1
rearDir = 1
foreDir = 1
if (baseDiff < 1):
baseDir = 0
if (rearDiff < 1):
rearDir = 0
if (foreDiff > 1):
foreDir = 0
baseSliced = self._sliceStepsToValues(abs(baseDiff), dur)
rearSliced = self._sliceStepsToValues(abs(rearDiff), dur)
foreSliced = self._sliceStepsToValues(abs(foreDiff), dur)
for base, rear, fore in zip(baseSliced, rearSliced, foreSliced):
ret = [0, 0]
# If ret[0] == 0 then command timed out or crc failed.
# If ret[1] == 0 then command queue was full.
while ret[0] == 0 or ret[1] == 0:
ret = self._driver.Steps(base, rear, fore, baseDir, rearDir, foreDir)
def _moveToAnglesSlice(self, baseAngle, rearArmAngle, foreArmAngle, \
carryOverStepsBase, carryOverStepsRear, carryOverStepsFore):
baseStepLocation = baseAngle * baseActualStepsPerRevolution / piTwo
rearArmStepLocation = abs(rearArmAngle * rearArmActualStepsPerRevolution / piTwo)
foreArmStepLocation = abs(foreArmAngle * foreArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Forearm Step Location", foreArmStepLocation)
self._debug('self._baseSteps', self._baseSteps)
self._debug('self._rearSteps', self._rearSteps)
self._debug('self._foreSteps', self._foreSteps)
self._debug('carryOverStepsBase', carryOverStepsBase)
self._debug('carryOverStepsRear', carryOverStepsRear)
self._debug('carryOverStepsFore', carryOverStepsFore)
baseDiff = baseStepLocation - self._baseSteps + carryOverStepsBase
rearDiff = rearArmStepLocation - self._rearSteps + carryOverStepsRear
foreDiff = foreArmStepLocation - self._foreSteps + carryOverStepsFore
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('foreDiff', foreDiff)
# self._baseSteps = baseStepLocation
# self._rearSteps = rearArmStepLocation
# self._foreSteps = foreArmStepLocation
baseSign = 1
rearSign = 1
foreSign = -1
baseDir = 1
rearDir = 1
foreDir = 1
if (baseDiff < 1):
baseDir = 0
baseSign = -1
if (rearDiff < 1):
rearDir = 0
rearSign = -1
if (foreDiff > 1):
foreDir = 0
foreSign = 1
baseDiffAbs = abs(baseDiff)
rearDiffAbs = abs(rearDiff)
foreDiffAbs = abs(foreDiff)
cmdBaseVal, actualStepsBase, leftStepsBase = self._driver.stepsToCmdValFloat(baseDiffAbs)
cmdRearVal, actualStepsRear, leftStepsRear = self._driver.stepsToCmdValFloat(rearDiffAbs)
cmdForeVal, actualStepsFore, leftStepsFore = self._driver.stepsToCmdValFloat(foreDiffAbs)
if not self._fake:
# Repeat until the command is buffered. May not be buffered if buffer is full.
ret = (0, 0)
while not ret[1]:
ret = self._driver.Steps(cmdBaseVal, cmdRearVal, cmdForeVal, baseDir, rearDir, foreDir)
return (actualStepsBase * baseSign, actualStepsRear * rearSign, actualStepsFore * foreSign,\
leftStepsBase * baseSign, leftStepsRear * rearSign, leftStepsFore * foreSign)
def freqToCmdVal(self, freq):
'''
See DobotDriver.freqToCmdVal()
'''
return self._driver.freqToCmdVal(freq)
def moveWithSpeed(self, x, y, z, maxSpeed, accel=None):
maxVel = float(maxSpeed)
xx = float(x)
yy = float(y)
zz = float(z)
accelf = None
# Set 100% acceleration if it wasn't provided or exceeds 100%
if accel == None or accel > maxVel:
accelf = maxVel
else:
accelf = float(accel)
print "--=========--"
self._debug('maxVel', maxVel)
self._debug('accelf', accelf)
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currForeAngle = piTwo * self._foreSteps / foreArmActualStepsPerRevolution
currX, currY, currZ = self._getCoordinatesFromAngles(currBaseAngle, currRearAngle, currForeAngle)
vectX = xx - currX
vectY = yy - currY
vectZ = zz - currZ
self._debug('moving from', currX, currY, currZ)
self._debug('moving to', xx, yy, zz)
self._debug('moving by', vectX, vectY, vectZ)
distance = math.sqrt(pow(vectX, 2) + pow(vectY, 2) + pow(vectZ, 2))
self._debug('distance to travel', distance)
# If half the distance is reached before reaching maxSpeed with the given acceleration, then actual
# maximum velocity will be lower, hence total number of slices is determined from half the distance
# and acceleration.
distMaxSpeed = pow(maxVel, 2) / (2.0 * accelf)
if distMaxSpeed * 2.0 >= distance:
accelSlices = math.sqrt(distance / accelf) * 50.0
flatSlices = 0
# Or else number of slices when velocity does not change is greater than zero.
else:
accelSlices = maxVel / accelf * 50.0
flatSlices = (distance - distMaxSpeed) / maxVel * 50.0
slices = accelSlices * 2.0 + flatSlices
self._debug('slices to do', slices)
self._debug('accelSlices', accelSlices)
self._debug('flatSlices', flatSlices)
# Acceleration/deceleration in respective axes
accelX = accelf * vectX / distance
accelY = accelf * vectY / distance
accelZ = accelf * vectZ / distance
self._debug('accelXYZ', accelX, accelY, accelZ)
# Vectors in respective axes to complete acceleration/deceleration
segmentAccelX = accelX * pow(accelSlices / 50.0, 2) / 2.0
segmentAccelY = accelY * pow(accelSlices / 50.0, 2) / 2.0
segmentAccelZ = accelZ * pow(accelSlices / 50.0, 2) / 2.0
self._debug('segmentAccelXYZ', segmentAccelX, segmentAccelY, segmentAccelZ)
maxVelX = maxVel * vectX / distance
maxVelY = maxVel * vectY / distance
maxVelZ = maxVel * vectZ / distance
self._debug('maxVelXYZ', maxVelX, maxVelY, maxVelZ)
# Vectors in respective axes in the segment with constant velocity
segmentFlatX = maxVelX * flatSlices / 50.0
segmentFlatY = maxVelY * flatSlices / 50.0
segmentFlatZ = maxVelZ * flatSlices / 50.0
self._debug('segmentFlatXYZ', segmentFlatX, segmentFlatY, segmentFlatZ)
# sliceX = vectX / slices
# sliceY = vectY / slices
# sliceZ = vectZ / slices
# self._debug('slicing by', sliceX, sliceY, sliceZ)
commands = 1
leftStepsBase = 0.0
leftStepsRear = 0.0
leftStepsFore = 0.0
toPlot1 = []
toPlot2 = []
toPlot3 = []
toPlot4 = []
toPlot5 = []
toPlot6 = []
toPlot7 = []
toPlot8 = []
toPlot9 = []
while commands < slices:
self._debug('==============================')
self._debug('slice #', commands)
# If accelerating
if commands <= accelSlices:
nextX = currX + accelX * pow(commands / 50.0, 2) / 2.0
nextY = currY + accelY * pow(commands / 50.0, 2) / 2.0
nextZ = currZ + accelZ * pow(commands / 50.0, 2) / 2.0
# If decelerating
elif commands >= accelSlices + flatSlices:
cmd = (slices - commands) / 50.0
nextX = currX + segmentAccelX * 2.0 + segmentFlatX - accelX * pow(cmd, 2) / 2.0
nextY = currY + segmentAccelY * 2.0 + segmentFlatY - accelY * pow(cmd, 2) / 2.0
nextZ = currZ + segmentAccelZ * 2.0 + segmentFlatZ - accelZ * pow(cmd, 2) / 2.0
# Or else moving at maxSpeed
else:
cmd = abs(commands - accelSlices)
nextX = currX + segmentAccelX + maxVelX * cmd / 50.0
nextY = currY + segmentAccelY + maxVelY * cmd / 50.0
nextZ = currZ + segmentAccelZ + maxVelZ * cmd / 50.0
self._debug('moving to', nextX, nextY, nextZ)
'''
http://www.learnaboutrobots.com/inverseKinematics.htm
'''
# Radius to the center of the tool.
radiusTool = math.sqrt(pow(nextX, 2) + pow(nextY, 2))
self._debug('radiusTool', radiusTool)
# Radius to joint3.
radius = radiusTool - distanceTool
self._debug('radius', radius)
baseAngle = math.atan2(nextY, nextX)
self._debug('ik base angle', baseAngle)
# X coordinate of joint3.
jointX = radius * math.cos(baseAngle)
self._debug('jointX', jointX)
# Y coordinate of joint3.
jointY = radius * math.sin(baseAngle)
self._debug('jointY', jointY)
actualZ = nextZ - heightFromBase
self._debug('actualZ', actualZ)
# Imaginary segment connecting joint1 with joint2, squared.
hypotenuseSquared = pow(actualZ, 2) + pow(radius, 2)
hypotenuse = math.sqrt(hypotenuseSquared)
self._debug('hypotenuse', hypotenuse)
self._debug('hypotenuseSquared', hypotenuseSquared)
q1 = math.atan2(actualZ, radius)
self._debug('q1', q1)
q2 = math.acos((lengthRearSquared - lengthForeSquared + hypotenuseSquared) / (2.0 * lengthRearArm * hypotenuse))
self._debug('q2', q2)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle)
foreAngle = piHalf - (math.acos((lengthRearSquared + lengthForeSquared - hypotenuseSquared) / (2.0 * lengthRearArm * lengthForearm)) - rearAngle)
self._debug('ik fore angle', foreAngle)
movedStepsBase, movedStepsRear, movedStepsFore, leftStepsBase, leftStepsRear, leftStepsFore = \
self._moveToAnglesSlice(baseAngle, rearAngle, foreAngle, \
leftStepsBase, leftStepsRear, leftStepsFore)
self._debug('moved', movedStepsBase, movedStepsRear, movedStepsFore, 'steps')
self._debug('leftovers', leftStepsBase, leftStepsRear, leftStepsFore)
commands += 1
self._baseSteps += movedStepsBase
self._rearSteps += movedStepsRear
self._foreSteps += movedStepsFore
# currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
# currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
# currForeAngle = piTwo * self._foreSteps / foreArmActualStepsPerRevolution
# cX, cY, cZ = self.getCoordinatesFromAngles(currBaseAngle, currRearAngle, currForeAngle)
# toPlot1.append(cX)
# toPlot2.append(cY)
# toPlot3.append(cZ)
# toPlot4.append(nextX)
# toPlot5.append(nextY)
# toPlot6.append(nextZ)
# toPlot7.append(cX - nextX)
# toPlot8.append(cY - nextY)
# toPlot9.append(cZ - nextZ)
# linewidth = 1.0
# plt.subplot(131)
# line, = plt.plot(toPlot4, linewidth=linewidth)
# line, = plt.plot(toPlot5, linewidth=linewidth)
# line, = plt.plot(toPlot6, linewidth=linewidth)
# plt.subplot(132)
# line, = plt.plot(toPlot1, linewidth=linewidth)
# line, = plt.plot(toPlot2, linewidth=linewidth)
# line, = plt.plot(toPlot3, linewidth=linewidth)
# plt.subplot(133)
# line, = plt.plot(toPlot7, linewidth=linewidth)
# line, = plt.plot(toPlot8, linewidth=linewidth)
# line, = plt.plot(toPlot9, linewidth=linewidth)
# plt.show()
def moveTo(self, x, y, z, duration):
'''
Moves the arm to the location with provided coordinates. Makes sure it takes exactly
the amount of time provided in "duration" argument to get end effector to the location.
All axes arrive at the same time.
SDK keeps track of the current end effector pose.
The origin is at the arm's base (the rotating base plate - joint1).
Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find
more about reference frame and arm names.
Current limitations:
- rear arm cannot go beyond initial 90 degrees (backwards from initial vertical position),
so plan carefully the coordinates
- forearm cannot go above initial horizontal position
Not yet implemented:
- the move is not linear as motors run the whole path at constant speed
- acceleration/deceleration
- rear arm cannot go beyond 90 degrees (see current limitations) and there are no checks
for that - it will simply go opposite direction instead
'''
xx = float(x)
yy = float(y)
zz = float(z)
radiansToDegrees = 180.0 / math.pi
b = 135.0
c = 160.0
b2 = pow(b, 2)
c2 = pow(c, 2)
self._debug('============================')
r = math.sqrt(pow(xx, 2) + pow(yy, 2))
self._debug('r', r)
z2 = zz - (80.0 + 23.0)
self._debug('z2', z2)
d2 = math.pow(z2, 2) + math.pow(r, 2)
d = math.sqrt(d2)
self._debug('d', d)
self._debug('d2', d2)
q1 = math.atan2(z2, r)
self._debug('q1', q1, q1 * radiansToDegrees)
q2 = math.acos((b2 - c2 + d2) / (2.0 * b * d))
self._debug('q2', q2, q2 * radiansToDegrees)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle, rearAngle * radiansToDegrees)
foreAngle = piHalf - (math.acos((b2 + c2 - d2) / (2.0 * b * c)) - rearAngle)
self._debug('ik fore angle', foreAngle, foreAngle * radiansToDegrees)
baseAngle = math.atan2(yy, xx)
self._debug('ik base angle', baseAngle, baseAngle * radiansToDegrees)
self._moveArmToAngles(baseAngle, rearAngle, foreAngle, duration)
def _getCoordinatesFromAngles(self, baseAngle, rearArmAngle, foreArmAngle):
radius = lengthRearArm * math.cos(rearArmAngle) + lengthForearm * math.cos(foreArmAngle) + distanceTool
x = radius * math.cos(baseAngle)
y = radius * math.sin(baseAngle)
z = lengthRearArm * math.sin(rearArmAngle) + heightFromBase - lengthForearm * math.sin(foreArmAngle)
return (x, y, z)
def CalibrateJoint(self, joint, forwardCommand, backwardCommand, direction, pin, pinMode, pullup):
'''
See DobotDriver.CalibrateJoint()
'''
return self._driver.CalibrateJoint(joint, forwardCommand, backwardCommand, direction, pin, pinMode, pullup)
def EmergencyStop(self):
'''
See DobotDriver.EmergencyStop()
'''
return self._driver.EmergencyStop()
def LaserOn(self, on):
self._driver.LaserOn(on)
class Dobot:
def __init__(self,
port,
rate=115200,
timeout=0.025,
debug=False,
fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if not fake:
self._driver.Open(timeout)
self._ik = DobotInverseKinematics(debug=debug)
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._foreSteps = long(0)
else:
# self._baseSteps = long(0)
# self._rearSteps = long(0)
# self._foreSteps = long(0)
accels = self._driver.GetAccelerometers()
accelRear = accels[1]
accelFore = accels[2]
rearAngle = math.pi / 2 - self._driver.accelToRadians(
accelRear, accelOffsets[0])
foreAngle = self._driver.accelToRadians(accelFore, accelOffsets[1])
self._baseSteps = long(0)
self._rearSteps = long((rearAngle / math.pi / 2.0) *
rearArmActualStepsPerRevolution + 0.5)
self._foreSteps = long((foreAngle / math.pi / 2.0) *
foreArmActualStepsPerRevolution + 0.5)
self._driver.SetCounters(self._baseSteps, self._rearSteps,
self._foreSteps)
print "Initializing with steps:", self._baseSteps, self._rearSteps, self._foreSteps
print "Reading back what was set:", self._driver.GetCounters()
print "--=========--"
def _debug(self, *args):
if self._debugOn:
# Since "print" is not a function the expansion (*) cannot be used
# as it is not an operator. So this is a workaround.
for arg in args:
print arg,
print
def _moveArmToAngles(self, baseAngle, rearArmAngle, foreArmAngle,
duration):
self._baseAngle = baseAngle
self._rearAngle = rearArmAngle
self._foreAngle = foreArmAngle
dur = float(duration)
# baseStepLocation = long((baseAngle / 360.0) * baseActualStepsPerRevolution + 0.5)
# rearArmStepLocation = long((abs(rearArmAngle) / 360.0) * rearArmActualStepsPerRevolution + 0.5)
# foreArmStepLocation = long((abs(foreArmAngle) / 360.0) * foreArmActualStepsPerRevolution + 0.5)
baseStepLocation = long(baseAngle * baseActualStepsPerRevolution /
piTwo)
rearArmStepLocation = long(rearArmAngle *
rearArmActualStepsPerRevolution / piTwo)
foreArmStepLocation = long(foreArmAngle *
foreArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Forearm Step Location", foreArmStepLocation)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
foreDiff = foreArmStepLocation - self._foreSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('foreDiff', foreDiff)
self._baseSteps = baseStepLocation
self._rearSteps = rearArmStepLocation
self._foreSteps = foreArmStepLocation
baseDir = 1
rearDir = 1
foreDir = 1
if (baseDiff < 1):
baseDir = 0
if (rearDiff < 1):
rearDir = 0
if (foreDiff > 1):
foreDir = 0
baseSliced = self._sliceStepsToValues(abs(baseDiff), dur)
rearSliced = self._sliceStepsToValues(abs(rearDiff), dur)
foreSliced = self._sliceStepsToValues(abs(foreDiff), dur)
for base, rear, fore in zip(baseSliced, rearSliced, foreSliced):
ret = [0, 0]
# If ret[0] == 0 then command timed out or crc failed.
# If ret[1] == 0 then command queue was full.
while ret[0] == 0 or ret[1] == 0:
ret = self._driver.Steps(base, rear, fore, baseDir, rearDir,
foreDir)
def _moveToAnglesSlice(self, baseAngle, rearArmAngle, foreArmAngle, \
carryOverStepsBase, carryOverStepsRear, carryOverStepsFore):
baseStepLocation = baseAngle * baseActualStepsPerRevolution / piTwo
rearArmStepLocation = abs(rearArmAngle *
rearArmActualStepsPerRevolution / piTwo)
foreArmStepLocation = abs(foreArmAngle *
foreArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Forearm Step Location", foreArmStepLocation)
self._debug('self._baseSteps', self._baseSteps)
self._debug('self._rearSteps', self._rearSteps)
self._debug('self._foreSteps', self._foreSteps)
self._debug('carryOverStepsBase', carryOverStepsBase)
self._debug('carryOverStepsRear', carryOverStepsRear)
self._debug('carryOverStepsFore', carryOverStepsFore)
baseDiff = baseStepLocation - self._baseSteps + carryOverStepsBase
rearDiff = rearArmStepLocation - self._rearSteps + carryOverStepsRear
foreDiff = foreArmStepLocation - self._foreSteps + carryOverStepsFore
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('foreDiff', foreDiff)
# self._baseSteps = baseStepLocation
# self._rearSteps = rearArmStepLocation
# self._foreSteps = foreArmStepLocation
baseSign = 1
rearSign = 1
foreSign = -1
baseDir = 1
rearDir = 1
foreDir = 1
if (baseDiff < 1):
baseDir = 0
baseSign = -1
if (rearDiff < 1):
rearDir = 0
rearSign = -1
if (foreDiff > 1):
foreDir = 0
foreSign = 1
baseDiffAbs = abs(baseDiff)
rearDiffAbs = abs(rearDiff)
foreDiffAbs = abs(foreDiff)
cmdBaseVal, actualStepsBase, leftStepsBase = self._driver.stepsToCmdValFloat(
baseDiffAbs)
cmdRearVal, actualStepsRear, leftStepsRear = self._driver.stepsToCmdValFloat(
rearDiffAbs)
cmdForeVal, actualStepsFore, leftStepsFore = self._driver.stepsToCmdValFloat(
foreDiffAbs)
if not self._fake:
# Repeat until the command is buffered. May not be buffered if buffer is full.
ret = (0, 0)
while not ret[1]:
ret = self._driver.Steps(cmdBaseVal, cmdRearVal, cmdForeVal,
baseDir, rearDir, foreDir)
return (actualStepsBase * baseSign, actualStepsRear * rearSign, actualStepsFore * foreSign,\
leftStepsBase * baseSign, leftStepsRear * rearSign, leftStepsFore * foreSign)
def freqToCmdVal(self, freq):
'''
See DobotDriver.freqToCmdVal()
'''
return self._driver.freqToCmdVal(freq)
def moveWithSpeed(self, x, y, z, maxSpeed, accel=None):
maxVel = float(maxSpeed)
xx = float(x)
yy = float(y)
zz = float(z)
accelf = None
# Set 100% acceleration if it wasn't provided or exceeds 100%
if accel == None or accel > maxVel:
accelf = maxVel
else:
accelf = float(accel)
print "--=========--"
self._debug('maxVel', maxVel)
self._debug('accelf', accelf)
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currForeAngle = piTwo * self._foreSteps / foreArmActualStepsPerRevolution
currX, currY, currZ = self._getCoordinatesFromAngles(
currBaseAngle, currRearAngle, currForeAngle)
vectX = xx - currX
vectY = yy - currY
vectZ = zz - currZ
self._debug('moving from', currX, currY, currZ)
self._debug('moving to', xx, yy, zz)
self._debug('moving by', vectX, vectY, vectZ)
distance = math.sqrt(pow(vectX, 2) + pow(vectY, 2) + pow(vectZ, 2))
self._debug('distance to travel', distance)
# If half the distance is reached before reaching maxSpeed with the given acceleration, then actual
# maximum velocity will be lower, hence total number of slices is determined from half the distance
# and acceleration.
distMaxSpeed = pow(maxVel, 2) / (2.0 * accelf)
if distMaxSpeed * 2.0 >= distance:
accelSlices = math.sqrt(distance / accelf) * 50.0
flatSlices = 0
# Or else number of slices when velocity does not change is greater than zero.
else:
accelSlices = maxVel / accelf * 50.0
flatSlices = (distance - distMaxSpeed) / maxVel * 50.0
slices = accelSlices * 2.0 + flatSlices
self._debug('slices to do', slices)
self._debug('accelSlices', accelSlices)
self._debug('flatSlices', flatSlices)
# Acceleration/deceleration in respective axes
accelX = accelf * vectX / distance
accelY = accelf * vectY / distance
accelZ = accelf * vectZ / distance
self._debug('accelXYZ', accelX, accelY, accelZ)
# Vectors in respective axes to complete acceleration/deceleration
segmentAccelX = accelX * pow(accelSlices / 50.0, 2) / 2.0
segmentAccelY = accelY * pow(accelSlices / 50.0, 2) / 2.0
segmentAccelZ = accelZ * pow(accelSlices / 50.0, 2) / 2.0
self._debug('segmentAccelXYZ', segmentAccelX, segmentAccelY,
segmentAccelZ)
maxVelX = maxVel * vectX / distance
maxVelY = maxVel * vectY / distance
maxVelZ = maxVel * vectZ / distance
self._debug('maxVelXYZ', maxVelX, maxVelY, maxVelZ)
# Vectors in respective axes in the segment with constant velocity
segmentFlatX = maxVelX * flatSlices / 50.0
segmentFlatY = maxVelY * flatSlices / 50.0
segmentFlatZ = maxVelZ * flatSlices / 50.0
self._debug('segmentFlatXYZ', segmentFlatX, segmentFlatY, segmentFlatZ)
# sliceX = vectX / slices
# sliceY = vectY / slices
# sliceZ = vectZ / slices
# self._debug('slicing by', sliceX, sliceY, sliceZ)
commands = 1
leftStepsBase = 0.0
leftStepsRear = 0.0
leftStepsFore = 0.0
toPlot1 = []
toPlot2 = []
toPlot3 = []
toPlot4 = []
toPlot5 = []
toPlot6 = []
toPlot7 = []
toPlot8 = []
toPlot9 = []
while commands < slices:
self._debug('==============================')
self._debug('slice #', commands)
# If accelerating
if commands <= accelSlices:
nextX = currX + accelX * pow(commands / 50.0, 2) / 2.0
nextY = currY + accelY * pow(commands / 50.0, 2) / 2.0
nextZ = currZ + accelZ * pow(commands / 50.0, 2) / 2.0
# If decelerating
elif commands >= accelSlices + flatSlices:
cmd = (slices - commands) / 50.0
nextX = currX + segmentAccelX * 2.0 + segmentFlatX - accelX * pow(
cmd, 2) / 2.0
nextY = currY + segmentAccelY * 2.0 + segmentFlatY - accelY * pow(
cmd, 2) / 2.0
nextZ = currZ + segmentAccelZ * 2.0 + segmentFlatZ - accelZ * pow(
cmd, 2) / 2.0
# Or else moving at maxSpeed
else:
cmd = abs(commands - accelSlices)
nextX = currX + segmentAccelX + maxVelX * cmd / 50.0
nextY = currY + segmentAccelY + maxVelY * cmd / 50.0
nextZ = currZ + segmentAccelZ + maxVelZ * cmd / 50.0
self._debug('moving to', nextX, nextY, nextZ)
'''
http://www.learnaboutrobots.com/inverseKinematics.htm
'''
# Radius to the center of the tool.
radiusTool = math.sqrt(pow(nextX, 2) + pow(nextY, 2))
self._debug('radiusTool', radiusTool)
# Radius to joint3.
radius = radiusTool - distanceTool
self._debug('radius', radius)
baseAngle = math.atan2(nextY, nextX)
self._debug('ik base angle', baseAngle)
# X coordinate of joint3.
jointX = radius * math.cos(baseAngle)
self._debug('jointX', jointX)
# Y coordinate of joint3.
jointY = radius * math.sin(baseAngle)
self._debug('jointY', jointY)
actualZ = nextZ - heightFromBase
self._debug('actualZ', actualZ)
# Imaginary segment connecting joint1 with joint2, squared.
hypotenuseSquared = pow(actualZ, 2) + pow(radius, 2)
hypotenuse = math.sqrt(hypotenuseSquared)
self._debug('hypotenuse', hypotenuse)
self._debug('hypotenuseSquared', hypotenuseSquared)
q1 = math.atan2(actualZ, radius)
self._debug('q1', q1)
q2 = math.acos(
(lengthRearSquared - lengthForeSquared + hypotenuseSquared) /
(2.0 * lengthRearArm * hypotenuse))
self._debug('q2', q2)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle)
foreAngle = piHalf - (math.acos(
(lengthRearSquared + lengthForeSquared - hypotenuseSquared) /
(2.0 * lengthRearArm * lengthForearm)) - rearAngle)
self._debug('ik fore angle', foreAngle)
movedStepsBase, movedStepsRear, movedStepsFore, leftStepsBase, leftStepsRear, leftStepsFore = \
self._moveToAnglesSlice(baseAngle, rearAngle, foreAngle, \
leftStepsBase, leftStepsRear, leftStepsFore)
self._debug('moved', movedStepsBase, movedStepsRear,
movedStepsFore, 'steps')
self._debug('leftovers', leftStepsBase, leftStepsRear,
leftStepsFore)
commands += 1
self._baseSteps += movedStepsBase
self._rearSteps += movedStepsRear
self._foreSteps += movedStepsFore
# currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
# currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
# currForeAngle = piTwo * self._foreSteps / foreArmActualStepsPerRevolution
# cX, cY, cZ = self.getCoordinatesFromAngles(currBaseAngle, currRearAngle, currForeAngle)
# toPlot1.append(cX)
# toPlot2.append(cY)
# toPlot3.append(cZ)
# toPlot4.append(nextX)
# toPlot5.append(nextY)
# toPlot6.append(nextZ)
# toPlot7.append(cX - nextX)
# toPlot8.append(cY - nextY)
# toPlot9.append(cZ - nextZ)
# linewidth = 1.0
# plt.subplot(131)
# line, = plt.plot(toPlot4, linewidth=linewidth)
# line, = plt.plot(toPlot5, linewidth=linewidth)
# line, = plt.plot(toPlot6, linewidth=linewidth)
# plt.subplot(132)
# line, = plt.plot(toPlot1, linewidth=linewidth)
# line, = plt.plot(toPlot2, linewidth=linewidth)
# line, = plt.plot(toPlot3, linewidth=linewidth)
# plt.subplot(133)
# line, = plt.plot(toPlot7, linewidth=linewidth)
# line, = plt.plot(toPlot8, linewidth=linewidth)
# line, = plt.plot(toPlot9, linewidth=linewidth)
# plt.show()
def moveTo(self, x, y, z, duration):
'''
Moves the arm to the location with provided coordinates. Makes sure it takes exactly
the amount of time provided in "duration" argument to get end effector to the location.
All axes arrive at the same time.
SDK keeps track of the current end effector pose.
The origin is at the arm's base (the rotating base plate - joint1).
Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find
more about reference frame and arm names.
Current limitations:
- rear arm cannot go beyond initial 90 degrees (backwards from initial vertical position),
so plan carefully the coordinates
- forearm cannot go above initial horizontal position
Not yet implemented:
- the move is not linear as motors run the whole path at constant speed
- acceleration/deceleration
- rear arm cannot go beyond 90 degrees (see current limitations) and there are no checks
for that - it will simply go opposite direction instead
'''
xx = float(x)
yy = float(y)
zz = float(z)
radiansToDegrees = 180.0 / math.pi
b = 135.0
c = 160.0
b2 = pow(b, 2)
c2 = pow(c, 2)
self._debug('============================')
r = math.sqrt(pow(xx, 2) + pow(yy, 2))
self._debug('r', r)
z2 = zz - (80.0 + 23.0)
self._debug('z2', z2)
d2 = math.pow(z2, 2) + math.pow(r, 2)
d = math.sqrt(d2)
self._debug('d', d)
self._debug('d2', d2)
q1 = math.atan2(z2, r)
self._debug('q1', q1, q1 * radiansToDegrees)
q2 = math.acos((b2 - c2 + d2) / (2.0 * b * d))
self._debug('q2', q2, q2 * radiansToDegrees)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle, rearAngle * radiansToDegrees)
foreAngle = piHalf - (math.acos(
(b2 + c2 - d2) / (2.0 * b * c)) - rearAngle)
self._debug('ik fore angle', foreAngle, foreAngle * radiansToDegrees)
baseAngle = math.atan2(yy, xx)
self._debug('ik base angle', baseAngle, baseAngle * radiansToDegrees)
self._moveArmToAngles(baseAngle, rearAngle, foreAngle, duration)
def _getCoordinatesFromAngles(self, baseAngle, rearArmAngle, foreArmAngle):
radius = lengthRearArm * math.cos(
rearArmAngle) + lengthForearm * math.cos(
foreArmAngle) + distanceTool
x = radius * math.cos(baseAngle)
y = radius * math.sin(baseAngle)
z = lengthRearArm * math.sin(
rearArmAngle) + heightFromBase - lengthForearm * math.sin(
foreArmAngle)
return (x, y, z)
def CalibrateJoint(self, joint, forwardCommand, backwardCommand, direction,
pin, pinMode, pullup):
'''
See DobotDriver.CalibrateJoint()
'''
return self._driver.CalibrateJoint(joint, forwardCommand,
backwardCommand, direction, pin,
pinMode, pullup)
def EmergencyStop(self):
'''
See DobotDriver.EmergencyStop()
'''
return self._driver.EmergencyStop()
def LaserOn(self, on):
self._driver.LaserOn(on)
#! /usr/bin/env python
'''
An example running a calibration procedure using a limit switch/photointerrupter connected to a any
of the unused pins on Arduino, enabling/disabling pullup, specifying the switch type (normal LOW or
normal HIGH), forward joint rotation direction (towards the switch), forward and backward speeds.
Refer to DobotDriver.CalibrateJoint() for details.
This example works well with a photointerrupter shown on open-dobot/docs/images/interrupter*.jpg
It is cheap and easily mounted and yet sturdy and reliably. Requires minimum effort to get accurate
calibration for Joint1 (base), which originally has no sensors.
You can get an interrupter here:
https://www.sparkfun.com/products/9299
https://www.sparkfun.com/products/9322
'''
from DobotDriver import DobotDriver
driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
# Rotate base CW at 400 steps/s until limit switch is hit. Then retract CCW at 50 steps/s
# until switch is released and stop.
# Switch is expected to be connected (soldered) to pin D8 and pulled up (HIGH) externally (with a
# resistor to 5V supply) or be an active device (like a photointerrupter). Pullup is not enabled on that pin.
print driver.CalibrateJoint(1, driver.freqToCmdVal(1000),
driver.freqToCmdVal(50), 1, 5, 1, 0)
So, if you send only one command "50Hz" then dobot makes exacly one step and stops.
number_of_steps = 50Hz * 20ms = 50 * 0.02 = 1
For 600Hz:
number_of_steps = 600Hz * 20ms = 600 * 0.02 = 12
'''
from DobotDriver import DobotDriver
import time
# driver = DobotDriver('/dev/tty.BT4-SPP-SerialPort')
# driver = DobotDriver('COM4')
driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
successes = 0
i = 0
while True:
ret = driver.isReady()
if ret[0] and ret[1]:
successes += 1
if successes > 10:
print("Dobot ready!")
break
if i > 100:
raise Exception('Comm problem')
gripper = 480
toolRotation = 0
#! /usr/bin/env python
'''
An example running a calibration procedure using a limit switch/photointerrupter connected to a any
of the unused pins on Arduino, enabling/disabling pullup, specifying the switch type (normal LOW or
normal HIGH), forward joint rotation direction (towards the switch), forward and backward speeds.
Refer to DobotDriver.CalibrateJoint() for details.
This example works well with a photointerrupter shown on open-dobot/docs/images/interrupter*.jpg
It is cheap and easily mounted and yet sturdy and reliably. Requires minimum effort to get accurate
calibration for Joint1 (base), which originally has no sensors.
You can get an interrupter here:
https://www.sparkfun.com/products/9299
https://www.sparkfun.com/products/9322
'''
from DobotDriver import DobotDriver
driver = DobotDriver('/dev/tty.usbmodem1421')
driver.Open()
# Rotate base CW at 400 steps/s until limit switch is hit. Then retract CCW at 50 steps/s
# until switch is released and stop.
# Switch is expected to be connected (soldered) to pin D8 and pulled up (HIGH) externally (with a
# resistor to 5V supply) or be an active device (like a photointerrupter). Pullup is not enabled on that pin.
print driver.CalibrateJoint(1, driver.freqToCmdVal(1000), driver.freqToCmdVal(50), 1, 5, 1, 0)
class Dobot:
def __init__(self, port, rate=115200, timeout=0.025, debug=False, plot=False, fake=False):
self._debugOn = debug
self._fake = fake
self._driver = DobotDriver(port, rate)
if fake:
self._driver._ramps = True
self._driver._stepCoeff = 20000
self._driver._stopSeq = 0
self._driver._stepCoeffOver2 = self._driver._stepCoeff / 2
self._driver._freqCoeff = self._driver._stepCoeff * 25
else:
self._driver.Open(timeout)
self._plot = plot
if plot:
import matplotlib.pyplot as plt
self._plt = plt
self._kinematics = DobotKinematics(debug=debug)
self._toolRotation = 0
self._gripper = 480
# Last directions to compensate backlash.
self._lastBaseDirection = 0
self._lastRearDirection = 0
self._lastFrontDirection = 0
# Initialize arms current configuration from accelerometers
if fake:
self._baseSteps = long(0)
self._rearSteps = long(0)
self._frontSteps = long(0)
else:
self.InitializeAccelerometers()
def _debug(self, *args):
if self._debugOn:
# Since "print" is not a function the expansion (*) cannot be used
# as it is not an operator. So this is a workaround.
for arg in args:
sys.stdout.write(str(arg))
sys.stdout.write(' ')
print('')
def InitializeAccelerometers(self):
print("--=========--")
print("Initializing accelerometers")
if self._driver.isFpga():
# In FPGA v1.0 SPI accelerometers are read only when Arduino boots. The readings
# are already available, so read once.
ret = (0, 0, 0, 0, 0, 0, 0)
while not ret[0]:
ret = self._driver.GetAccelerometers()
accelRearX = ret[1]
accelFrontX = ret[4]
rearAngle = piHalf - self._driver.accelToRadians(accelRearX, accelOffsets[0])
frontAngle = self._driver.accelToRadians(accelFrontX, accelOffsets[1])
else:
# In RAMPS accelerometers are on I2C bus and can be read at any time. We need to
# read them multiple times to get average as MPU-6050 have greater resolution but are noisy.
# However, due to the interference from the way A4988 holds the motors if none of the
# recommended measures to suppress interference are in place (see open-dobot wiki), or
# in case accelerometers are not connected, we need to give up and assume some predefined pose.
accelRearX = 0
accelRearY = 0
accelRearZ = 0
accelFrontX = 0
accelFrontY = 0
accelFrontZ = 0
successes = 0
for i in range(20):
ret = (0, 0, 0, 0, 0, 0, 0)
attempts = 10
while attempts:
ret = self._driver.GetAccelerometers()
if ret[0]:
successes += 1
accelRearX += ret[1]
accelRearY += ret[2]
accelRearZ += ret[3]
accelFrontX += ret[4]
accelFrontY += ret[5]
accelFrontZ += ret[6]
break
attempts -= 1
if successes > 0:
divisor = float(successes)
rearAngle = piHalf - self._driver.accel3DXToRadians(accelRearX / divisor, accelRearY / divisor, accelRearZ / divisor)
frontAngle = -self._driver.accel3DXToRadians(accelFrontX / divisor, accelFrontY / divisor, accelFrontZ / divisor)
else:
print('Failed to read accelerometers. Make sure they are connected and interference is suppressed. See open-dobot wiki')
print('Assuming rear arm vertical and front arm horizontal')
rearAngle = 0
frontAngle = -piHalf
self._baseSteps = long(0)
self._rearSteps = long((rearAngle / piTwo) * rearArmActualStepsPerRevolution + 0.5)
self._frontSteps = long((frontAngle / piTwo) * frontArmActualStepsPerRevolution + 0.5)
self._driver.SetCounters(self._baseSteps, self._rearSteps, self._frontSteps)
print("Initializing with steps:", self._baseSteps, self._rearSteps, self._frontSteps)
print("Reading back what was set:", self._driver.GetCounters())
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
print('Current estimated coordinates:', self._kinematics.coordinatesFromAngles(currBaseAngle, currRearAngle, currFrontAngle))
print("--=========--")
def _moveArmToAngles(self, baseAngle, rearArmAngle, frontArmAngle, duration):
self._baseAngle = baseAngle
self._rearAngle = rearArmAngle
self._frontAngle = frontArmAngle
dur = float(duration)
# baseStepLocation = long((baseAngle / 360.0) * baseActualStepsPerRevolution + 0.5)
# rearArmStepLocation = long((abs(rearArmAngle) / 360.0) * rearArmActualStepsPerRevolution + 0.5)
# frontArmStepLocation = long((abs(frontArmAngle) / 360.0) * frontArmActualStepsPerRevolution + 0.5)
baseStepLocation = long(baseAngle * baseActualStepsPerRevolution / piTwo)
rearArmStepLocation = long(rearArmAngle * rearArmActualStepsPerRevolution / piTwo)
frontArmStepLocation = long(frontArmAngle * frontArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Frontarm Step Location", frontArmStepLocation)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
frontDiff = frontArmStepLocation - self._frontSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('frontDiff', frontDiff)
self._baseSteps = baseStepLocation
self._rearSteps = rearArmStepLocation
self._frontSteps = frontArmStepLocation
baseDir = 1
rearDir = 1
frontDir = 1
if (baseDiff < 1):
baseDir = 0
if (rearDiff < 1):
rearDir = 0
if (frontDiff > 1):
frontDir = 0
baseSliced = self._sliceStepsToValues(abs(baseDiff), dur)
rearSliced = self._sliceStepsToValues(abs(rearDiff), dur)
frontSliced = self._sliceStepsToValues(abs(frontDiff), dur)
for base, rear, front in zip(baseSliced, rearSliced, frontSliced):
ret = [0, 0]
# If ret[0] == 0 then command timed out or crc failed.
# If ret[1] == 0 then command queue was full.
while ret[0] == 0 or ret[1] == 0:
ret = self._driver.Steps(base, rear, front, baseDir, rearDir, frontDir)
def _moveToAnglesSlice(self, baseAngle, rearArmAngle, frontArmAngle, toolRotation):
baseStepLocation = baseAngle * baseActualStepsPerRevolution / piTwo
rearArmStepLocation = abs(rearArmAngle * rearArmActualStepsPerRevolution / piTwo)
frontArmStepLocation = abs(frontArmAngle * frontArmActualStepsPerRevolution / piTwo)
self._debug("Base Step Location", baseStepLocation)
self._debug("Rear Arm Step Location", rearArmStepLocation)
self._debug("Front Arm Step Location", frontArmStepLocation)
self._debug('self._baseSteps', self._baseSteps)
self._debug('self._rearSteps', self._rearSteps)
self._debug('self._frontSteps', self._frontSteps)
baseDiff = baseStepLocation - self._baseSteps
rearDiff = rearArmStepLocation - self._rearSteps
frontDiff = frontArmStepLocation - self._frontSteps
self._debug('baseDiff', baseDiff)
self._debug('rearDiff', rearDiff)
self._debug('frontDiff', frontDiff)
baseSign = 1
rearSign = 1
frontSign = -1
baseDir = 1
rearDir = 1
frontDir = 1
if (baseDiff < 1):
baseDir = 0
baseSign = -1
if (rearDiff < 1):
rearDir = 0
rearSign = -1
if (frontDiff > 1):
frontDir = 0
frontSign = 1
baseDiffAbs = abs(baseDiff)
rearDiffAbs = abs(rearDiff)
frontDiffAbs = abs(frontDiff)
cmdBaseVal, actualStepsBase, leftStepsBase = self._driver.stepsToCmdValFloat(baseDiffAbs)
cmdRearVal, actualStepsRear, leftStepsRear = self._driver.stepsToCmdValFloat(rearDiffAbs)
cmdFrontVal, actualStepsFront, leftStepsFront = self._driver.stepsToCmdValFloat(frontDiffAbs)
# Compensate for backlash.
# For now compensate only backlash in the base motor as the backlash in the arm motors depends
# on specific task (a laser/brush or push-pull tasks).
if self._lastBaseDirection != baseDir and actualStepsBase > 0:
cmdBaseVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(baseDiffAbs + backlash)
self._lastBaseDirection = baseDir
# if self._lastRearDirection != rearDir and actualStepsRear > 0:
# cmdRearVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(rearDiffAbs + backlash)
# self._lastRearDirection = rearDir
# if self._lastFrontDirection != frontDir and actualStepsFront > 0:
# cmdFrontVal, _ignore, _ignore = self._driver.stepsToCmdValFloat(frontDiffAbs + backlash)
# self._lastFrontDirection = frontDir
if not self._fake:
# Repeat until the command is queued. May not be queued if queue is full.
ret = (0, 0)
while not ret[1]:
ret = self._driver.Steps(cmdBaseVal, cmdRearVal, cmdFrontVal, baseDir, rearDir, frontDir, self._gripper, int(toolRotation))
if self._plot:
self._toPlot1.append(baseDiff)
self._toPlot2.append(actualStepsBase * baseSign)
return (actualStepsBase * baseSign, actualStepsRear * rearSign, actualStepsFront * frontSign,\
leftStepsBase * baseSign, leftStepsRear * rearSign, leftStepsFront * frontSign)
def freqToCmdVal(self, freq):
'''
See DobotDriver.freqToCmdVal()
'''
return self._driver.freqToCmdVal(freq)
def _moveTo(self, x, y, z, duration):
'''
TEMPORARILY UNAVALAILABLE
Left for future refactor.
Moves the arm to the location with provided coordinates. Makes sure it takes exactly
the amount of time provided in "duration" argument to get end effector to the location.
All axes arrive at the same time.
SDK keeps track of the current end effector pose.
The origin is at the arm's base (the rotating base plate - joint1).
Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find
more about reference frame and arm names.
Current limitations:
- rear arm cannot go beyond initial 90 degrees (backwards from initial vertical position),
so plan carefully the coordinates
- front arm cannot go above initial horizontal position
Not yet implemented:
- the move is not linear as motors run the whole path at constant speed
- acceleration/deceleration
- rear arm cannot go beyond 90 degrees (see current limitations) and there are no checks
for that - it will simply go opposite direction instead
'''
xx = float(x)
yy = float(y)
zz = float(z)
radiansToDegrees = 180.0 / math.pi
b = 135.0
c = 160.0
b2 = pow(b, 2)
c2 = pow(c, 2)
self._debug('============================')
r = math.sqrt(pow(xx, 2) + pow(yy, 2))
self._debug('r', r)
z2 = zz - (80.0 + 23.0)
self._debug('z2', z2)
d2 = math.pow(z2, 2) + math.pow(r, 2)
d = math.sqrt(d2)
self._debug('d', d)
self._debug('d2', d2)
q1 = math.atan2(z2, r)
self._debug('q1', q1, q1 * radiansToDegrees)
q2 = math.acos((b2 - c2 + d2) / (2.0 * b * d))
self._debug('q2', q2, q2 * radiansToDegrees)
rearAngle = piHalf - (q1 + q2)
self._debug('ik rear angle', rearAngle, rearAngle * radiansToDegrees)
frontAngle = piHalf - (math.acos((b2 + c2 - d2) / (2.0 * b * c)) - rearAngle)
self._debug('ik front angle', frontAngle, frontAngle * radiansToDegrees)
baseAngle = math.atan2(yy, xx)
self._debug('ik base angle', baseAngle, baseAngle * radiansToDegrees)
self._moveArmToAngles(baseAngle, rearAngle, frontAngle, duration)
def MoveWithSpeed(self, x, y, z, maxSpeed, accel=None, toolRotation=None):
'''
For toolRotation see DobotDriver.Steps() function description (servoRot parameter).
'''
if self._plot:
self._toPlot1 = []
self._toPlot2 = []
maxVel = float(maxSpeed)
xx = float(x)
yy = float(y)
zz = float(z)
if toolRotation == None:
toolRotation = self._toolRotation
elif toolRotation > 1024:
toolRotation = 1024
elif toolRotation < 0:
toolRotation = 0
accelf = None
# Set 100% acceleration to equal maximum velocity if it wasn't provided
if accel == None:
accelf = maxVel
else:
accelf = float(accel)
print("--=========--")
self._debug('maxVel', maxVel)
self._debug('accelf', accelf)
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
currX, currY, currZ = self._kinematics.coordinatesFromAngles(currBaseAngle, currRearAngle, currFrontAngle)
vectX = xx - currX
vectY = yy - currY
vectZ = zz - currZ
self._debug('moving from', currX, currY, currZ)
self._debug('moving to', xx, yy, zz)
self._debug('moving by', vectX, vectY, vectZ)
distance = math.sqrt(pow(vectX, 2) + pow(vectY, 2) + pow(vectZ, 2))
self._debug('distance to travel', distance)
# If half the distance is reached before reaching maxSpeed with the given acceleration, then actual
# maximum velocity will be lower, hence total number of slices is determined from half the distance
# and acceleration.
distToReachMaxSpeed = pow(maxVel, 2) / (2.0 * accelf)
if distToReachMaxSpeed * 2.0 >= distance:
timeToAccel = math.sqrt(distance / accelf)
accelSlices = timeToAccel * 50.0
timeFlat = 0
flatSlices = 0
maxVel = math.sqrt(distance * accelf)
# Or else number of slices when velocity does not change is greater than zero.
else:
timeToAccel = maxVel / accelf
accelSlices = timeToAccel * 50.0
timeFlat = (distance - distToReachMaxSpeed * 2.0) / maxVel
flatSlices = timeFlat * 50.0
slices = accelSlices * 2.0 + flatSlices
self._debug('slices to do', slices)
self._debug('accelSlices', accelSlices)
self._debug('flatSlices', flatSlices)
# Acceleration/deceleration in respective axes
accelX = (accelf * vectX) / distance
accelY = (accelf * vectY) / distance
accelZ = (accelf * vectZ) / distance
self._debug('accelXYZ', accelX, accelY, accelZ)
# Vectors in respective axes to complete acceleration/deceleration
segmentAccelX = accelX * pow(timeToAccel, 2) / 2.0
segmentAccelY = accelY * pow(timeToAccel, 2) / 2.0
segmentAccelZ = accelZ * pow(timeToAccel, 2) / 2.0
self._debug('segmentAccelXYZ', segmentAccelX, segmentAccelY, segmentAccelZ)
# Maximum velocity in respective axes for the segment with constant velocity
maxVelX = (maxVel * vectX) / distance
maxVelY = (maxVel * vectY) / distance
maxVelZ = (maxVel * vectZ) / distance
self._debug('maxVelXYZ', maxVelX, maxVelY, maxVelZ)
# Vectors in respective axes for the segment with constant velocity
segmentFlatX = maxVelX * timeFlat
segmentFlatY = maxVelY * timeFlat
segmentFlatZ = maxVelZ * timeFlat
self._debug('segmentFlatXYZ', segmentFlatX, segmentFlatY, segmentFlatZ)
segmentToolRotation = (toolRotation - self._toolRotation) / slices
self._debug('segmentToolRotation', segmentToolRotation)
commands = 1
leftStepsBase = 0.0
leftStepsRear = 0.0
leftStepsFront = 0.0
if self._plot:
toPlot1 = []
toPlot2 = []
toPlot3 = []
toPlot4 = []
toPlot5 = []
toPlot6 = []
toPlot7 = []
toPlot8 = []
toPlot9 = []
while commands < slices:
self._debug('==============================')
self._debug('slice #', commands)
# If accelerating
if commands <= accelSlices:
t2half = pow(commands / 50.0, 2) / 2.0
nextX = currX + accelX * t2half
nextY = currY + accelY * t2half
nextZ = currZ + accelZ * t2half
# If decelerating
elif commands >= accelSlices + flatSlices:
t2half = pow((slices - commands) / 50.0, 2) / 2.0
nextX = currX + segmentAccelX * 2.0 + segmentFlatX - accelX * t2half
nextY = currY + segmentAccelY * 2.0 + segmentFlatY - accelY * t2half
nextZ = currZ + segmentAccelZ * 2.0 + segmentFlatZ - accelZ * t2half
# Or else moving at maxSpeed
else:
t = abs(commands - accelSlices) / 50.0
nextX = currX + segmentAccelX + maxVelX * t
nextY = currY + segmentAccelY + maxVelY * t
nextZ = currZ + segmentAccelZ + maxVelZ * t
self._debug('moving to', nextX, nextY, nextZ)
nextToolRotation = self._toolRotation + (segmentToolRotation * commands)
self._debug('nextToolRotation', nextToolRotation)
(baseAngle, rearAngle, frontAngle) = self._kinematics.anglesFromCoordinates(nextX, nextY, nextZ)
movedStepsBase, movedStepsRear, movedStepsFront, leftStepsBase, leftStepsRear, leftStepsFront = \
self._moveToAnglesSlice(baseAngle, rearAngle, frontAngle, nextToolRotation)
self._debug('moved', movedStepsBase, movedStepsRear, movedStepsFront, 'steps')
self._debug('leftovers', leftStepsBase, leftStepsRear, leftStepsFront)
commands += 1
self._baseSteps += movedStepsBase
self._rearSteps += movedStepsRear
self._frontSteps += movedStepsFront
currBaseAngle = piTwo * self._baseSteps / baseActualStepsPerRevolution
currRearAngle = piHalf - piTwo * self._rearSteps / rearArmActualStepsPerRevolution
currFrontAngle = piTwo * self._frontSteps / frontArmActualStepsPerRevolution
cX, cY, cZ = self._kinematics.coordinatesFromAngles(currBaseAngle, currRearAngle, currFrontAngle)
if self._plot:
toPlot1.append(cX)
toPlot2.append(cY)
toPlot3.append(cZ)
toPlot4.append(nextX)
toPlot5.append(nextY)
toPlot6.append(nextZ)
toPlot7.append(cX - nextX)
toPlot8.append(cY - nextY)
toPlot9.append(cZ - nextZ)
self._toolRotation = toolRotation
if self._plot:
# linewidth = 1.0
# plt.subplot(131)
# line, = plt.plot(toPlot4, linewidth=linewidth)
# line, = plt.plot(toPlot5, linewidth=linewidth)
# line, = plt.plot(toPlot6, linewidth=linewidth)
# plt.subplot(132)
# line, = plt.plot(toPlot1, linewidth=linewidth)
# line, = plt.plot(toPlot2, linewidth=linewidth)
# line, = plt.plot(toPlot3, linewidth=linewidth)
# plt.subplot(133)
# line, = plt.plot(toPlot7, linewidth=linewidth, label='x')
# line, = plt.plot(toPlot8, linewidth=linewidth, label='y')
# line, = plt.plot(toPlot9, linewidth=linewidth, label='z')
# legend = plt.legend(loc='upper center', shadow=True)
# plt.show()
linewidth = 1.0
self._plt.plot(self._toPlot1, linewidth=linewidth)
self._plt.plot(self._toPlot2, linewidth=2.0)
# make the y ticks integers, not floats
yint = []
locs, labels = self._plt.yticks()
for each in locs:
yint.append(int(each))
self._plt.yticks(yint)
self._plt.show()
def Gripper(self, gripper):
if gripper > 480:
self._gripper = 480
elif gripper < 208:
self._gripper = 208
else:
self._gripper = gripper
self._driver.Steps(0, 0, 0, 0, 0, 0, self._gripper, self._toolRotation)
def Wait(self, waitTime):
'''
See description in DobotDriver.Wait()
'''
self._driver.Wait(waitTime)
def CalibrateJoint(self, joint, forwardCommand, backwardCommand, direction, pin, pinMode, pullup):
'''
See DobotDriver.CalibrateJoint()
'''
return self._driver.CalibrateJoint(joint, forwardCommand, backwardCommand, direction, pin, pinMode, pullup)
def EmergencyStop(self):
'''
See DobotDriver.EmergencyStop()
'''
return self._driver.EmergencyStop()
def LaserOn(self, on):
return self._driver.LaserOn(on)
def PumpOn(self, on):
return self._driver.PumpOn(on)
def ValveOn(self, on):
return self._driver.ValveOn(on)