Beispiel #1
0
    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
Beispiel #2
0
    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]
Beispiel #3
0
    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]
Beispiel #4
0
    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
Beispiel #5
0
    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