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