def gotoPointMaxDist(self, x, y, z, maxDist):
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
if dist < maxDist:
maxDist = dist
self.goDirectlyTo(x0 + (x - x0) * maxDist / dist, y0 + (y - y0) * maxDist / dist, z0 + (z - z0) * maxDist / dist)
def gotoPoint(self, x, y, z, step = 10, debugPrint=1):
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
i = 1
while i < dist:
self.goDirectlyTo(x0 + (x - x0) * i / dist, y0 + (y - y0) * i / dist, z0 + (z - z0) * i / dist, debugPrint)
time.sleep(0.03)
i += step
self.goDirectlyTo(x, y, z, debugPrint)
time.sleep(0.05)
def gotoPoint(self, x, y, z):
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
step = 10
i = 0
while i < dist:
self.goDirectlyTo(x0 + (x - x0) * i / dist, y0 + (y - y0) * i / dist, z0 + (z - z0) * i / dist)
time.sleep(0.05)
i += step
self.goDirectlyTo(x, y, z)
time.sleep(0.05)
def gotoPoint(self, x, y, z):
print("goto=> {},{},{}".format(x, y, z))
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
step = 10
i = 0
while i < dist:
self.goDirectlyTo(x0 + (x - x0) * i / dist,
y0 + (y - y0) * i / dist,
z0 + (z - z0) * i / dist)
time.sleep(0.05)
i += step
self.goDirectlyTo(x, y, z)
time.sleep(0.05)
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
step = 10
i = 0
while i < dist:
self.goDirectlyTo(x0 + (x - x0) * i / dist, y0 + (y - y0) * i / dist, z0 + (z - z0) * i / dist)
time.sleep(0.03)
i += step
self.goDirectlyTo(x, y, z)
time.sleep(0.05)
def getDistance(self, x, y, z)
dist = kinematics.distance(self.x, self.y, self.z, x, y, z)
return dist
def gotoPointMaxDist(self, x, y, z, maxDist):
"""Travel in a straight line from current position to a requested position"""
x0 = self.x
y0 = self.y
z0 = self.z
dist = kinematics.distance(x0, y0, z0, x, y, z)
if dist < maxDist:
maxDist = dist
self.goDirectlyTo(x0 + (x - x0) * maxDist / dist, y0 + (y - y0) * maxDist / dist, z0 + (z - z0) * maxDist / dist)
def openGripper(self):
"""Open the gripper, dropping whatever is being carried"""
def getDistanceBetween(self, x, y, z, x1, y1, z1): dist = kinematics.distance(x1, y1, z1, x, y, z) return dist
def getDistance(self, x, y, z): dist = kinematics.distance(self.x, self.y, self.z, x, y, z) return dist