def getTrajectoryPoint(self, inputs): gripperLeft = inputs[0] gripperRight = inputs[1] mode = inputs[2] tfListener = inputs[3] jointPosition = inputs[4] (worldToBase, _) = tfListener.lookupTransform('/world', '/yumi_base_link', rospy.Time(0)) targertHeightBase = self.targetHeight - worldToBase[2] trajectoryPoint = Trajectory_point() if mode == 'individual': positionLeft = gripperLeft.getPosition() positionRight = gripperRight.getPosition() if abs(jointPosition[6]) > np.pi / 0.7 or abs( jointPosition[13]) > np.pi / 0.7: orientationRight = tf.transformations.quaternion_from_euler( -jointPosition[6] / 2, 0, np.pi, 'rzyx') orientationLeft = tf.transformations.quaternion_from_euler( -jointPosition[13] / 2, 0, np.pi, 'rzyx') else: orientationLeft = np.array([1, 0, 0, 0]) orientationRight = np.array([1, 0, 0, 0]) positionRight[2] = targertHeightBase[0] positionLeft[2] = targertHeightBase[1] self.pointTime = utils.getPointTime(gripperRight=gripperRight,\ gripperLeft=gripperLeft,\ posTargetRight=positionRight,\ posTargetLeft=positionLeft, \ rotTargetRight=orientationRight,\ rotTargetLeft=orientationLeft) trajectoryPoint.positionRight = positionRight.tolist() trajectoryPoint.positionLeft = positionLeft.tolist() trajectoryPoint.orientationLeft = orientationLeft.tolist() trajectoryPoint.orientationRight = orientationRight.tolist() trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]] trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]] trajectoryPoint.pointTime = self.pointTime gripperLeft.update(positionLeft, orientationLeft) gripperRight.update(positionRight, orientationRight) else: print('Error, mode not valid in subtask') return [] return [trajectoryPoint]
def getTrajectoryPoint(self, inputs): DLO = inputs[0] gripperLeft = inputs[1] gripperRight = inputs[2] tfListener = inputs[3] (worldToBase, _) = tfListener.lookupTransform('/world', '/yumi_base_link', rospy.Time(0)) targertHeightBase = self.targetHeight - worldToBase[2] rightPos, leftPos, quatRight, quatLeft = self.individual.getRightLeftPosQuat( np.array([0.03, 0.03])) if self.individual.pickupRightValid == 1: positionRight = rightPos positionRight[2] = targertHeightBase[0] else: positionRight = np.array([0.1, -0.35, 0.1]) # gripperRight.getPosition() quatRight = np.array([1, 0, 0, 0]) #gripperRight.getQuaternion() if self.individual.pickupLeftValid == 1: positionLeft = leftPos positionLeft[2] = targertHeightBase[1] else: positionLeft = np.array([0.1, 0.35, 0.1]) # gripperLeft.getPosition() quatLeft = np.array([1, 0, 0, 0]) #gripperLeft.getQuaternion() # 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): 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 getTrajectoryPoint(self, inputs): gripperLeft = inputs[0] gripperRight = inputs[1] mode = inputs[2] tfListener = inputs[3] (worldToBase, _) = tfListener.lookupTransform('/world', '/yumi_base_link', rospy.Time(0)) targertHeightBase = self.targetHeight - worldToBase[2] trajectoryPoint = Trajectory_point() if mode == 'individual': positionLeft = gripperLeft.getPosition() positionRight = gripperRight.getPosition() orientationLeft = gripperLeft.getQuaternion() orientationRight = gripperRight.getQuaternion() positionRight[2] = targertHeightBase[0] positionLeft[2] = targertHeightBase[1] self.pointTime = utils.getPointTime(gripperRight=gripperRight,\ gripperLeft=gripperLeft,\ posTargetRight=positionRight,\ posTargetLeft=positionLeft, \ rotTargetRight=orientationRight,\ rotTargetLeft=orientationLeft,\ avgSpeed=self.avgVelocity) trajectoryPoint.positionRight = positionRight.tolist() trajectoryPoint.positionLeft = positionLeft.tolist() trajectoryPoint.orientationLeft = orientationLeft.tolist() trajectoryPoint.orientationRight = orientationRight.tolist() trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]] trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]] trajectoryPoint.pointTime = self.pointTime gripperLeft.update(positionLeft, orientationLeft) gripperRight.update(positionRight, orientationRight) else: print('Error, mode not valid in subtask') return [] return [trajectoryPoint]
def getTrajectoryPoint(self, inputs): relative = inputs[0] absolute = inputs[1] map_ = inputs[2] previousFixture = inputs[3] targetFixture = inputs[4] tfListener = inputs[5] DLO = inputs[6] targetFixtureObj = map_[targetFixture] absPos = targetFixtureObj.getClippPosition() absPos[2] += self.targetHeight rot = targetFixtureObj.getOrientation() if absolute.flipped == 1: # rotate z 180 rot = tf.transformations.quaternion_multiply( rot, np.array([0, 0, 1, 0])) absRot = utils.rotateX180(rot) relRot = np.array([0, 0, 0, 1]) relPos = np.array([0, self.gripperWidth, 0]) self.pointTime = utils.getPointTime(gripperRight=absolute,\ gripperLeft=relative,\ posTargetRight=absPos,\ posTargetLeft=relPos, \ rotTargetRight=absRot,\ rotTargetLeft=relRot) trajectoryPoint = Trajectory_point() trajectoryPoint.positionAbsolute = absPos.tolist() trajectoryPoint.positionRelative = relPos.tolist() trajectoryPoint.orientationRelative = relRot.tolist() trajectoryPoint.orientationAbsolute = absRot.tolist() trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]] trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]] trajectoryPoint.pointTime = self.pointTime relative.update(relPos, relRot) absolute.update(absPos, absRot) return [trajectoryPoint]
def getTrajectoryPoint(self, inputs): absolute = inputs[0] relative = inputs[1] mode = inputs[2] tfListener = inputs[3] (worldToBase, _) = tfListener.lookupTransform('/world', '/yumi_base_link', rospy.Time(0)) targertHeightBase = self.targetHeight - worldToBase[2] trajectoryPoint = Trajectory_point() if mode == 'combined': absPos = absolute.getPosition() absRot = absolute.getQuaternion() relPos = relative.getPosition() relRot = relative.getQuaternion() absPos[2] = targertHeightBase[0] self.pointTime = utils.getPointTime(gripperRight=absolute,\ gripperLeft=relative,\ posTargetRight=absPos,\ posTargetLeft=relPos, \ rotTargetRight=absRot,\ rotTargetLeft=relRot) trajectoryPoint.positionAbsolute = absPos.tolist() trajectoryPoint.positionRelative = relPos.tolist() trajectoryPoint.orientationRelative = relRot.tolist() trajectoryPoint.orientationAbsolute = absRot.tolist() trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]] trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]] trajectoryPoint.pointTime = self.pointTime relative.update(relPos, relRot) absolute.update(absPos, absRot) else: print('Error, mode not valid in subtask') return [] return [trajectoryPoint]
def getTrajectoryPoint(self, inputs): relative = inputs[0] absolute = inputs[1] tfListener = inputs[2] (worldToBase, _) = tfListener.lookupTransform('/world', '/yumi_base_link', rospy.Time(0)) absPos = np.zeros(3) absPos[0] = self.absPosXY[0] absPos[1] = self.absPosXY[1] absPos[2] = self.targetHeight - worldToBase[2] absRot = tf.transformations.quaternion_from_euler( self.absRotZ, 0, 180 * np.pi / 180, 'rzyx') relRot = np.array([0, 0, 0, 1]) relPos = np.array([0, self.gripperWidth, 0]) self.pointTime = utils.getPointTime(gripperRight=absolute,\ gripperLeft=relative,\ posTargetRight=absPos,\ posTargetLeft=relPos, \ rotTargetRight=absRot,\ rotTargetLeft=relRot) trajectoryPoint = Trajectory_point() trajectoryPoint.positionAbsolute = absPos.tolist() trajectoryPoint.positionRelative = relPos.tolist() trajectoryPoint.orientationRelative = relRot.tolist() trajectoryPoint.orientationAbsolute = absRot.tolist() trajectoryPoint.gripperLeft = [self.gripper[1], self.gripper[1]] trajectoryPoint.gripperRight = [self.gripper[0], self.gripper[0]] trajectoryPoint.pointTime = self.pointTime relative.update(relPos, relRot) absolute.update(absPos, absRot) return [trajectoryPoint]