def absPosAngleGrippPoints(task):
posRight = task.gripperRight.getPosition()
posLeft = task.gripperLeft.getPosition()
dy = posLeft[1] - posRight[1]
dx = posLeft[0] - posRight[0]
initAngle = np.arctan2(dy, dx) - np.pi / 2
initAbsPos = 0.5 * (posRight + posLeft)
minDist, point, minIndex = utils.closesPointDLO(task.DLO, posRight)
rightGrippPoint = point
minDist, point, minIndex = utils.closesPointDLO(task.DLO, posLeft)
leftGrippPoint = point
return rightGrippPoint, leftGrippPoint, initAbsPos, initAngle
def pickupRangeAndAngle(task, rightGrippPoint, leftGrippPoint):
endDLOMargin = 0.05
if task.previousFixture < 0:
start = endDLOMargin
else:
minDist, point, minIndex = utils.closesPointDLO(DLO=task.DLO,\
pos=task.map[task.previousFixture].getClippPosition())
start = point
if leftGrippPoint > rightGrippPoint:
leftStart = leftGrippPoint
leftEnd = task.DLO.getLength() - endDLOMargin
rightStart = start
rightEnd = rightGrippPoint
else:
rightStart = rightGrippPoint
rightEnd = task.DLO.getLength() - endDLOMargin
leftStart = start
leftEnd = leftGrippPoint
pointRight = task.DLO.getCoord(rightGrippPoint)
pointLeft = task.DLO.getCoord(leftGrippPoint)
dy = pointLeft[1] - pointRight[1]
dx = pointLeft[0] - pointRight[0]
angle = np.arctan2(dy, dx) - np.pi / 2
rightPickupRange = np.array([rightStart, rightEnd])
leftPickupRange = np.array([leftStart, leftEnd])
return rightPickupRange, leftPickupRange, angle
def ropeConstraintCombined(task, individual, rightGrippPoint, leftGrippPoint):
valid = True
score = 0
if task.previousFixture == -1:
pass
else:
fixtureClippPosition = task.map[
task.previousFixture].getClippPosition()
minDist, point, minIndex = utils.closesPointDLO(
DLO=task.DLO, pos=fixtureClippPosition)
rightPos, leftPos, quatRight, quatLeft = individual.getRightLeftPosQuat(
np.array([0.03, 0.03]))
lengthRight = abs(point - rightGrippPoint)
lengthLeft = abs(point - leftGrippPoint)
if lengthRight < lengthLeft:
closesPoint = rightPos
lengthRope = lengthRight
else:
closesPoint = leftPos
lengthRope = lengthLeft
closestDist = np.linalg.norm(fixtureClippPosition - closesPoint)
if closestDist > lengthLeft:
score -= 2 + (closestDist - lengthRope)
valid = False
return score, valid
def ropeConstraint(task, individual):
margin = 0.02
valid = True
score = 0
if task.previousFixture == -1:
return 0, valid
else:
rightPos, leftPos, quatRight, quatLeft = individual.getRightLeftPosQuat(np.array([0.03, 0.03]))
fixtureClippPosition = task.map[task.previousFixture].getClippPosition()
minDist, point, minIndex = utils.closesPointDLO(DLO=task.DLO, pos=fixtureClippPosition)
pickupPoints = individual.getPickupPoints()
lengthRight = abs(point - pickupPoints[0])
lengthLeft = abs(point - pickupPoints[1])
if individual.pickupLeftValid and individual.pickupRightValid:
if lengthRight < lengthLeft:
closesPoint = rightPos
lengthRope = lengthRight
else:
closesPoint = leftPos
lengthRope = lengthLeft
elif individual.pickupLeftValid:
closesPoint = leftPos
lengthRope = lengthLeft
elif individual.pickupRightValid:
closesPoint = rightPos
lengthRope = lengthRight
else:
closesPoint = np.zeros(3)
lengthRope = 1
closestDist = np.linalg.norm(fixtureClippPosition - closesPoint)
if closestDist > lengthRope-margin:
score -= 2 + (closestDist - lengthRope)
valid = False
return score, valid
def verification(self, input_):
DLO = input_[0]
gripperRight = input_[1]
gripperLeft = input_[2]
minDistLeft = 0
minDistRight = 0
if gripperRight.getPosition()[2] >= 0.08 and gripperLeft.getPosition(
)[2] >= 0.08:
minDistRight, pointRight, minIndex = utils.closesPointDLO(
DLO, gripperRight.getPosition())
minDistLeft, pointLeft, minIndex = utils.closesPointDLO(
DLO, gripperLeft.getPosition())
if minDistLeft < self.tol and minDistRight < self.tol:
return True
else:
return False
def getMsg(self):
self.newTrajectory = 0
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = self.mode
self.trajectory = []
if self.mode == 'combined':
absolutePosition, absoluteOrientation, relativePosition, relativeOrientation = \
utils.calcAbsoluteAndRelative(self.gripperRight, self.gripperLeft, self.transformer)
absoluteTemp = self.gripperRight.copyClass()
relativeTemp = self.gripperLeft.copyClass()
absoluteTemp.update(absolutePosition, absoluteOrientation)
relativeTemp.update(relativePosition, relativeOrientation)
absoluteTemp.flipped = -1
minDist, lengthRight, minIndex = utils.closesPointDLO(
self.DLO, self.gripperRight.getPosition())
minDist, lengthLeft, minIndex = utils.closesPointDLO(
self.DLO, self.gripperLeft.getPosition())
if lengthRight > lengthLeft:
absoluteTemp.flipped = 1
else:
gripperRightTemp = self.gripperRight.copyClass()
gripperLeftTemp = self.gripperLeft.copyClass()
for i in range(self.startSubTaskIdx, len(self.subTasks)):
inputArgs = self.subTasks[i].inputArgs
input_ = []
for j in range(len(inputArgs)):
input_.append(eval(inputArgs[j]))
trajectoryPoint = self.subTasks[i].getTrajectoryPoint(input_)
self.trajectory.extend(trajectoryPoint)
msg.trajectory = self.trajectory
return msg
def verification(self, input_):
setTaskStep = input_[0]
map_ = input_[1]
DLO = input_[2]
targetFixture = input_[3]
fixture = map_[targetFixture]
fixturePosition = fixture.getClippPosition()
minDist, point, minIndex = utils.closesPointDLO(DLO, fixturePosition)
if minDist < self.MaxDLODist:
setTaskStep(1)
else:
print('DLO not in Fixture, distance = ', minDist)
setTaskStep(-1)
return True