def verification(self, input_):
DLO = input_[0]
gripperRight = input_[1]
gripperLeft = input_[2]
if gripperLeft.getPosition()[2] < 0.09 or gripperRight.getPosition(
)[2] < 0.09:
return True
positionRight, positionLeft,quatRight, quatLeft, inRange = utils.calcGrippPosRot(DLO,\
self.leftGrippPoint, self.rightGrippPoint, self.targetHeight[0], self.targetHeight[1])
if inRange == False:
print('Error, pickup points are outside the cable')
return False
if self.individal.pickupRightValid == 1:
checkRight = utils.checkIfWithinTol(positionRight, self.currentTarget[0], 0.04,\
quatRight, self.currentTarget[1], 0.4)
else:
checkRight = True
if self.individal.pickupLeftValid == 1:
checkLeft = utils.checkIfWithinTol(positionLeft, self.currentTarget[2], 0.04,\
quatLeft, self.currentTarget[3], 0.4)
else:
checkLeft = True
if checkRight == True and checkLeft == True:
return True
else:
return False
def getTrajectoryPoint(self, inputs):
map_ = inputs[0]
DLO = inputs[1]
gripperLeft = inputs[2]
gripperRight = inputs[3]
targetFixture = inputs[4]
previousFixture = inputs[5]
cableSlack = inputs[6]
tfListener = inputs[7]
clipPoint = utils.calcClipPoint(targetFixture, previousFixture, map_,
cableSlack, DLO, self.grippWidth)
self.leftGrippPoint, self.rightGrippPoint = utils.calcGrippPoints(
targetFixture, map_, DLO, self.grippWidth, clipPoint)
if self.leftGrippPoint == -1 or self.rightGrippPoint == -1:
print('Error, not in range')
return []
positionRight, positionLeft,quatRight, quatLeft, inRange = utils.calcGrippPosRot(DLO,\
self.leftGrippPoint, self.rightGrippPoint, self.targetHeight[0], self.targetHeight[1])
positionRight[2] = np.maximum(positionRight[2], 0.0023)
positionLeft[2] = np.maximum(positionLeft[2], 0.002)
if inRange == False:
print('Error, not in range')
return []
self.currentTarget = [positionRight, quatRight, positionLeft, quatLeft]
# calc time
self.pointTime = utils.getPointTime(gripperRight=gripperRight,\
gripperLeft=gripperLeft,\
posTargetRight=positionRight,\
posTargetLeft=positionLeft, \
rotTargetRight=quatRight,\
rotTargetLeft=quatLeft)
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = positionRight.tolist()
trajectoryPoint.positionLeft = positionLeft.tolist()
trajectoryPoint.orientationLeft = quatLeft.tolist()
trajectoryPoint.orientationRight = quatRight.tolist()
trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]]
trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]]
trajectoryPoint.pointTime = self.pointTime
gripperRight.update(positionRight, quatRight)
gripperLeft.update(positionLeft, quatLeft)
return [trajectoryPoint]
def getTrajectoryPoint(self, inputs):
DLO = inputs[0]
gripperLeft = inputs[1]
gripperRight = inputs[2]
pickupPoints = self.individal.getPickupPoints()
self.rightGrippPoint = pickupPoints[0]
self.leftGrippPoint = pickupPoints[1]
positionRight, positionLeft,quatRight, quatLeft, inRange = utils.calcGrippPosRot(DLO,\
self.leftGrippPoint, self.rightGrippPoint, self.targetHeight[0], self.targetHeight[1])
positionRight[2] = np.maximum(positionRight[2], 0.0023)
positionLeft[2] = np.maximum(positionLeft[2], 0.002)
if inRange == False:
print('Error, not in range')
return []
if self.individal.pickupRightValid == 0:
positionRight = np.array([0.1, -0.35,
0.1]) # gripperRight.getPosition()
quatRight = np.array([1, 0, 0, 0]) #gripperRight.getQuaternion()
if self.individal.pickupLeftValid == 0:
positionLeft = np.array([0.1, 0.35,
0.1]) # gripperLeft.getPosition()
quatLeft = np.array([1, 0, 0, 0]) #gripperLeft.getQuaternion()
self.currentTarget = [positionRight, quatRight, positionLeft, quatLeft]
# calc time
self.pointTime = utils.getPointTime(gripperRight=gripperRight,\
gripperLeft=gripperLeft,\
posTargetRight=positionRight,\
posTargetLeft=positionLeft, \
rotTargetRight=quatRight,\
rotTargetLeft=quatLeft)
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = positionRight.tolist()
trajectoryPoint.positionLeft = positionLeft.tolist()
trajectoryPoint.orientationLeft = quatLeft.tolist()
trajectoryPoint.orientationRight = quatRight.tolist()
trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]]
trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]]
trajectoryPoint.pointTime = self.pointTime
gripperRight.update(positionRight, quatRight)
gripperLeft.update(positionLeft, quatLeft)
return [trajectoryPoint]
def verification(self, input_):
DLO = input_[0]
positionRight, positionLeft,quatRight, quatLeft, inRange = utils.calcGrippPosRot(DLO,\
self.leftGrippPoint, self.rightGrippPoint, self.targetHeight[0], self.targetHeight[1])
if inRange == False:
print('Error, pickup points are outside the cable')
return False
checkRight = utils.checkIfWithinTol(positionRight, self.currentTarget[0], 0.02,\
quatRight, self.currentTarget[1], 0.4)
checkLeft = utils.checkIfWithinTol(positionLeft, self.currentTarget[2], 0.02,\
quatLeft, self.currentTarget[3], 0.4)
if checkRight == True and checkLeft == True:
return True
else:
return False
def evaluateIndividual(self, individual):
# init
score = 0
individual.pickupLeftValid = True
individual.pickupRightValid = True
# final positions
rightPos, leftPos, quatRight, quatLeft = individual.getRightLeftPosQuat(
np.array([0.03, 0.03]))
pickupPoints = individual.getPickupPoints()
# pickup points
tempRightPos, tempLeftPos, tempRightQuat, tempLeftQuat, valid_ = utils.calcGrippPosRot(self.DLO, leftGrippPoint=pickupPoints[1], rightGrippPoint=pickupPoints[0],\
targetHeightRight=0, targetHeightLeft=0)
# To close to fixture, for end points, penalty
scoreFixtureRight, validFixtureRight = utilsSolve.fixturePenalty(
position=rightPos, map_=self.map)
scoreFixtureLeft, validFixtureLeft = utilsSolve.fixturePenalty(
position=leftPos, map_=self.map)
score += scoreFixtureRight
score += scoreFixtureLeft
individual.pickupRightValid = individual.pickupRightValid and validFixtureRight
individual.pickupLeftValid = individual.pickupLeftValid and validFixtureLeft
# To close to fixture, pickup, penalty
scoreFixtureRight, validFixtureRight = utilsSolve.fixturePenalty(
position=tempRightPos, map_=self.map)
scoreFixtureLeft, validFixtureLeft = utilsSolve.fixturePenalty(
position=tempLeftPos, map_=self.map)
score += scoreFixtureRight
score += scoreFixtureLeft
individual.pickupRightValid = individual.pickupRightValid and validFixtureRight
individual.pickupLeftValid = individual.pickupLeftValid and validFixtureLeft
# check if pickup point is in reach
score_, valid_ = utilsSolve.outsideReachPenalty(position=tempRightPos,\
quat=np.array([1,0,0,0]), reachCentrum=self.reachRightCentrum, reach=self.reach)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.outsideReachPenalty(position=tempLeftPos,\
quat=np.array([1,0,0,0]), reachCentrum=self.reachLeftCentrum, reach=self.reach)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# check if end point is in reach
score_, valid_ = utilsSolve.outsideReachPenalty(position=rightPos, quat=quatRight,\
reachCentrum=self.reachRightCentrum, reach=self.reach)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.outsideReachPenalty(position=leftPos, quat=quatLeft,\
reachCentrum=self.reachLeftCentrum, reach=self.reach)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# penalty for crossing to far on x axis
score_, valid_ = utilsSolve.baseCollisionPenalty(position=rightPos)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.baseCollisionPenalty(position=leftPos)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
score_, valid_ = utilsSolve.baseCollisionPenalty(position=tempRightPos)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.baseCollisionPenalty(position=tempLeftPos)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# penalty for crossing to far on y axis
angle = individual.parametersIndividual[4]
if angle < -(np.pi / 2 +
(20 * np.pi / 180)) or angle > (np.pi / 2 +
(20 * np.pi / 180)):
score += -2
# penalty for crossing pickup
if not utilsSolve.checkCrossing(pointR=tempRightPos,
pointL=tempLeftPos):
score += -2
# penalty for outside side rope constraint
score_, valid_ = utilsSolve.ropeConstraint(task=self.task,
individual=individual)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# "Predict rope" ----------------
rightEndPickupPoint, leftEndPickupPoint = utilsSolve.predictRope(task=self.task, individual=individual,\
leftGrippPoint=self.leftGrippPoint, rightGrippPoint=self.rightGrippPoint)
# new pickup points to close to fixture
scoreFixtureRight, validFixtureRight = utilsSolve.fixturePenalty(
position=rightEndPickupPoint, map_=self.map)
scoreFixtureLeft, validFixtureLeft = utilsSolve.fixturePenalty(
position=leftEndPickupPoint, map_=self.map)
score += scoreFixtureRight
score += scoreFixtureLeft
individual.pickupRightValid = individual.pickupRightValid and validFixtureRight
individual.pickupLeftValid = individual.pickupLeftValid and validFixtureLeft
# penalty for crossing to far on x axis
score_, valid_ = utilsSolve.baseCollisionPenalty(
position=rightEndPickupPoint)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.baseCollisionPenalty(
position=leftEndPickupPoint)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# penalty for end pickup out of reach
score_, valid_ = utilsSolve.outsideReachPenalty(position=rightEndPickupPoint,\
quat=np.array([1,0,0,0]), reachCentrum=self.reachRightCentrum, reach=self.reach-0.04)
score += score_
individual.pickupRightValid = individual.pickupRightValid and valid_
score_, valid_ = utilsSolve.outsideReachPenalty(position=leftEndPickupPoint, quat=quatLeft,\
reachCentrum=self.reachLeftCentrum, reach=self.reach-0.04)
score += score_
individual.pickupLeftValid = individual.pickupLeftValid and valid_
# penalty for pickup points too close
if np.linalg.norm(rightEndPickupPoint - leftEndPickupPoint) < 0.12:
score += -2
if np.linalg.norm(tempRightPos - tempLeftPos) < 0.12:
score += -2
if individual.pickupRightValid and individual.pickupLeftValid:
individual.pickupRightValid = False
individual.pickupLeftValid = False
# penalty for distance from target pickup
score += -abs(pickupPoints[0] - self.rightGrippPoint)
score += -abs(pickupPoints[1] - self.leftGrippPoint)
# angle close to init angle reward
score += 0.5 / (
abs(self.initAngle - individual.parametersIndividual[4]) + 1)
# Distance moved penalty
score_, valid_ = utilsSolve.distanceMovedPenalty(
self.initRightGrippPos, rightEndPickupPoint)
score += score_
score_, valid_ = utilsSolve.distanceMovedPenalty(
self.initLeftGrippPos, leftEndPickupPoint)
score += score_
# if both not valid
#if not (individual.pickupRightValid or individual.pickupLeftValid):
# score += -5
return score