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]
