def getTrajectoryPoint(self, inputs):
relative = inputs[0]
absolute = inputs[1]
mode = inputs[2]
trajectoryPoint = Trajectory_point()
if mode == 'combined':
absPos = absolute.getPosition()
absRot = absolute.getQuaternion()
relPos = relative.getPosition()
relRot = relative.getQuaternion()
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 pickup4_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# Move both grippers closer to each other
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.005, 0.25]
trajectoryPoint.positionLeft = [0.35, -0.005, 0.25]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('3: Moving arms to exchange location.')
def pickup3(x,y,z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#msg.forceControl = 0
#msg.maxForce = 0
#Move arms together
leftRot = tf.transformations.quaternion_from_euler(90*np.pi/180, 90*np.pi/180, 180*np.pi/180, 'rzyx')
rightRot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0*np.pi/180, 270*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint.positionRight = [0.355, 0.005, z+0.15]
trajectoryPoint.positionLeft = [0.35, -0.005, z+0.1375]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [20,20]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('3: Moving arms to exchange location.')
def getTrajectoryPoint(self, inputs):
gripperLeft = inputs[0]
gripperRight = inputs[1]
mode = inputs[2]
trajectoryPoint = Trajectory_point()
if mode == 'individual':
positionLeft = gripperLeft.getPosition()
positionRight = gripperRight.getPosition()
orientationLeft = gripperLeft.getQuaternion()
orientationRight = gripperRight.getQuaternion()
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):
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):
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]
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):
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 reset_position():
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print("Resetting position.")
def pickup2_left(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
# Close gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [x, y, 0.01]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('2: Close gripper.')
def calibrate_marker():
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
reset_position()
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.527, 0.005, 0.025]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print("Calibrating.")
def pickup2_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
# Close gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [x, y, 0.01]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('1: Picking up at x = ' + str(x) + ', y = ' + str(y))
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]
def main():
# initilize ros node
rospy.init_node('CheckFixturePostition', anonymous=True)
tfListener = tf.TransformListener()
# sleep for listener to have time to pick up the transformes
rospy.sleep(0.5)
# add existing fixtures in list
fixtureList = ['/Fixture1', '/Fixture2', '/Fixture3', '/Fixture4']
listOfObjects = []
for i in range(len(fixtureList)):
try:
(pos, rot) = tfListener.lookupTransform('/yumi_base_link',
fixtureList[i],
rospy.Time(0))
pos = np.asarray(pos)
rot = np.asarray(rot)
obj = utils.FixtureObject(position=pos,\
orientation=rot,\
fixtureHeight=0.06,\
fixtureRadius=0.06)
listOfObjects.extend([obj])
except:
break
pub = rospy.Publisher('/Trajectroy', Trajectory_msg, queue_size=1)
rospy.sleep(0.1)
# --------------------------------------------------
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
trajectory = []
for obj in listOfObjects:
pos = obj.getBasePosition()
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [pos[0], pos[1], 0.08]
trajectoryPoint.positionLeft = [0.05, 0.3, 0.15]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# add same point twice for the gripper to stop above the fixture
trajectory.append(trajectoryPoint)
pos = obj.getBasePosition()
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.3, -0.3, 0.1]
trajectoryPoint.positionLeft = [0.3, 0.3, 0.1]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
for obj in listOfObjects:
pos = obj.getBasePosition()
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.05, -0.3, 0.15]
trajectoryPoint.positionLeft = [pos[0], pos[1], 0.08]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# add same point twice for the gripper to stop above the fixture
trajectory.append(trajectoryPoint)
pos = obj.getBasePosition()
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.2, -0.3, 0.1]
trajectoryPoint.positionLeft = [0.2, 0.3, 0.1]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
rospy.sleep(0.1)
print('sent individual')
rospy.spin()
def pickup6_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
#Move arms away from each other
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.1, 0.25]
trajectoryPoint.positionLeft = [0.35, 0.1, 0.25]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
# Move left arm above drop location, move right arm to starting position
leftRot = tf.transformations.quaternion_from_euler(0 * np.pi / 180,
0 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 180 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.5, 0.2, z + 0.1]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# Move left arm down to drop location
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.5, 0.2, 0.005]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# Open left gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.5, 0.2, 0.005]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# Move left arm up from work table
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.5, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# Move left arm to starting position
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 180 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('5. Putting down cube and moving back to starting position.')
def pickup5_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# Close left claw
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.01, 0.25]
trajectoryPoint.positionLeft = [0.35, -0.01, 0.25]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 3.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
#Open right claw
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.01, 0.25]
trajectoryPoint.positionLeft = [0.35, -0.01, 0.25]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 3.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('4. Moving cube to the other gripper.')
def pickup4(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#msg.forceControl = 0
#msg.maxForce = 0
#Close right claw
leftRot = tf.transformations.quaternion_from_euler(90*np.pi/180, 90*np.pi/180, 180*np.pi/180, 'rzyx')
rightRot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0*np.pi/180, 270*np.pi/180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.355, 0.01, z+0.15]
trajectoryPoint.positionLeft = [0.35, -0.01, z+0.1375]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 3.0
trajectory = [trajectoryPoint]
#trajectory.append(trajectoryPoint)
#Open left claw
leftRot = tf.transformations.quaternion_from_euler(90*np.pi/180, 90*np.pi/180, 180*np.pi/180, 'rzyx')
rightRot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0*np.pi/180, 270*np.pi/180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.355, 0.01, z+0.15]
trajectoryPoint.positionLeft = [0.35, -0.01, z+0.1375]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20,20]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 3.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('4. Moving cube to the other gripper.')
def pickup3_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
# Move arm up from table
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [x, y, z + 0.2]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 4.0
trajectory = [trajectoryPoint]
#Move arms towards exchange location, turn both grippers 90 degrees inwards
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.05, 0.25]
trajectoryPoint.positionLeft = [0.35, 0.05, 0.25]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('2: Moving towards exchange location.')
def pickup1_right(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
# Move to starting position
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.15, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
# Move right arm above object
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [x, y, 0.05]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# Move arm down to object
trajectoryPoint = Trajectory_point()
#trajectoryPoint.positionRight = [x, y, -0.01]
trajectoryPoint.positionRight = [x, y, 0.01]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('1: Picking up at x = ' + str(x) + ', y = ' + str(y))
def pickup2(x,y,z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#Move arm upp from table
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z+0.1]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 4.0
trajectory = [trajectoryPoint]
#Move arms to echange location, turn the left-claw 90 degrees in the z-axis and open right claw
leftRot = tf.transformations.quaternion_from_euler(90*np.pi/180, 90*np.pi/180, 180*np.pi/180, 'rzyx')
rightRot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0*np.pi/180, 270*np.pi/180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.355, -0.1, z+0.15]
trajectoryPoint.positionLeft = [0.35, 0.1, z+0.1375]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [20,20]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('2: Moving towards exchange location.')
def pickup(x, y, z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#msg.forceControl = 0
#msg.maxForce = 0
#Move to start position
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#Move left arm above object and open left claw
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z + 0.1]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move arm down to object
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z + 0.03]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Close gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z + 0.03]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move arm upp from table
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z + 0.1]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 4.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
"""
leftRot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0*np.pi/180, 180*np.pi/180, 'rzyx')
#Test to se which axis-rotations we originaly have
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z+0.1]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 8.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
"""
#Move arms to echange location, turn the left-claw 90 degrees in the z-axis and open right claw
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.05, z + 0.15]
trajectoryPoint.positionLeft = [0.34, 0.05, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#delaykod start
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.05, z + 0.15]
trajectoryPoint.positionLeft = [0.34, 0.05, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#delay s**t
#Move arms together
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint.positionRight = [0.35, 0.03, z + 0.15]
trajectoryPoint.positionLeft = [0.34, -0.03, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#Close right claw
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.03, z + 0.15]
trajectoryPoint.positionLeft = [0.34, -0.03, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 3.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#Open left claw
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
# För att ymi inte ska krocka IRL så är denna ändrad
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.03, z + 0.15]
trajectoryPoint.positionLeft = [0.34, -0.03, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 3.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#Move arms away from each other
leftRot = tf.transformations.quaternion_from_euler(90 * np.pi / 180,
90 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 270 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.1, z + 0.15]
trajectoryPoint.positionLeft = [0.34, 0.1, z + 0.15]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
#trajectory = [trajectoryPoint]
trajectory.append(trajectoryPoint)
#Move right arm to above worktable, left arm to startpos and close left claw
leftRot = tf.transformations.quaternion_from_euler(0 * np.pi / 180,
0 * np.pi / 180,
180 * np.pi / 180,
'rzyx')
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 180 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.5, -0.2, z + 0.1]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move right arm down to worktable
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.5, -0.2, 0.03]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Open right gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.5, -0.2, 0.05]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move right arm up from object
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.5, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [20, 20]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move right arm to start position and close right gripper
rightRot = tf.transformations.quaternion_from_euler(
0 * np.pi / 180, 0 * np.pi / 180, 180 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = leftRot
trajectoryPoint.orientationRight = rightRot
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('Moving to start position...')
rospy.sleep(3)
def pickup1(x,y,z):
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#msg.forceControl = 0
#msg.maxForce = 0
#Move to start position
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
#Move left arm above object and open left claw
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z+0.1]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [20,20]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Move arm down to object
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z+0.017]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [20,20]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
#Close gripper
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [x, y, z+0.017]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0,0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('1: Moving to pick up location at x = ' + str(x) + ', y = ' + str(y))
def main():
# starting ROS node and subscribers
rospy.init_node('trajectory_test', anonymous=True)
pub = rospy.Publisher('/Trajectroy', Trajectory_msg, queue_size=1)
rospy.sleep(0.1)
# --------------------------------------------------
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
#msg.forceControl = 0
#msg.maxForce = 4.0
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.2]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [20, 20]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 12.0
trajectory = [trajectoryPoint]
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.1, 0.03]
trajectoryPoint.positionLeft = [0.35, 0.1, 0.03]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0, 0]
trajectoryPoint.gripperRight = [0, 0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
'''
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.1, 0.03]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.03]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [20,20]
trajectoryPoint.gripperRight = [20,20]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.1, 0.03]
trajectoryPoint.positionLeft = [0.35, 0.15, 0.03]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 2.0
trajectory.append(trajectoryPoint)
# ---------------
'''
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
print('sent individual')
rospy.sleep(31)
# --------------------------------------------
'''
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'combined'
msg.forceControl = 0
msg.maxForce = 8.0
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory = [trajectoryPoint]
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.45, 0.1, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.45, 0.1, 0.4]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
msg.trajectory = trajectory
pub.publish(msg)
print('sent msg 2, combined ')
rospy.sleep(33)
# --------------------------------------------
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'combined'
msg.forceControl = 0
msg.maxForce = 8.0
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 210*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory = [trajectoryPoint]
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 150*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(40*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(-40*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0,40*np.pi/180, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler( 0,-40*np.pi/180, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 16.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
rotrel = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 60*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = rotrel
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
rotrel = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 0*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = rotrel
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
msg.trajectory = trajectory
pub.publish(msg)
print('sent msg 2, combined ')
'''
'''
rospy.sleep(30)
# --------------------------------------------
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'combined'
msg.forceControl = 0
msg.maxForce = 4.0
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [1, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 6.0
trajectory = [trajectoryPoint]
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.3, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(110*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.4, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# ---------------
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.3, 0.0, 0.2]
trajectoryPoint.positionLeft = [0, 0.25, 0]
trajectoryPoint.orientationLeft = [0,0,0,1]
trajectoryPoint.orientationRight = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 6.0
trajectory.append(trajectoryPoint)
# ---------------
msg.trajectory = trajectory
pub.publish(msg)
print('sent msg 3, combined ')
rospy.sleep(14)
'''
rospy.spin()
def main():
# starting ROS node and subscribers
rospy.init_node('trajectory_test', anonymous=True)
pub = rospy.Publisher('/Trajectroy', Trajectory_msg, queue_size=1)
rospy.sleep(0.1)
# --------------------------------------------------
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = 'individual'
trajectoryPoint = Trajectory_point()
trajectory = []
# ---------------
trajectoryPoint.positionRight = [0.35, -0.25, 0.05]
trajectoryPoint.positionLeft = [0.35, 0.25, 0.05]
trajectoryPoint.orientationLeft = [1, 0, 0, 0]
trajectoryPoint.orientationRight = [1, 0, 0, 0]
trajectoryPoint.gripperLeft = [0.0, 0.0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 8.0
trajectory = [trajectoryPoint]
# ---------------
rotR = tf.transformations.quaternion_from_euler(0 * np.pi / 180,
-0 * np.pi / 180,
180 * np.pi / 180, 'rzyx')
rotL = tf.transformations.quaternion_from_euler(0 * np.pi / 180,
-0 * np.pi / 180,
180 * np.pi / 180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.25, 0.015]
trajectoryPoint.positionLeft = [0.35, 0.25, 0.015]
trajectoryPoint.orientationLeft = rotL
trajectoryPoint.orientationRight = rotR
trajectoryPoint.gripperLeft = [0.0, 0.0]
trajectoryPoint.gripperRight = [0.0, 0.0]
trajectoryPoint.pointTime = 3.0
trajectory.append(trajectoryPoint)
'''
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.002]
trajectoryPoint.positionLeft = [0.05, 0.3, 0.1]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, 0.2, 0.002]
trajectoryPoint.positionLeft = [0.05, 0.3, 0.1]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 20.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.2, 0.002]
trajectoryPoint.positionLeft = [0.05, 0.3, 0.1]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 20.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.3, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.3, 0.2]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [20.0,20.0]
trajectoryPoint.gripperRight = [20.0,20.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.3, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.002]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.05, -0.3, 0.2]
trajectoryPoint.positionLeft = [0.35, -0.2, 0.002]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 20.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.3, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.2, 0.002]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [0,0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 20.0
trajectory.append(trajectoryPoint)
# ---------------
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionRight = [0.35, -0.3, 0.2]
trajectoryPoint.positionLeft = [0.35, 0.3, 0.2]
trajectoryPoint.orientationLeft = [1,0,0,0]
trajectoryPoint.orientationRight = [1,0,0,0]
trajectoryPoint.gripperLeft = [20.0,20.0]
trajectoryPoint.gripperRight = [20.0,20.0]
trajectoryPoint.pointTime = 8.0
trajectory.append(trajectoryPoint)
'''
msg.trajectory = trajectory
pub.publish(msg)
rospy.sleep(0.1)
pub.publish(msg)
rospy.sleep(0.1)
print('sent individual')
'''
rot = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 180*np.pi/180, 'rzyx')
rotrel = tf.transformations.quaternion_from_euler(0*np.pi/180, 0, 0*np.pi/180, 'rzyx')
trajectoryPoint = Trajectory_point()
trajectoryPoint.positionAbsolute = [0.4, 0.0, 0.2]
trajectoryPoint.positionRelative = [0, 0.25, 0]
trajectoryPoint.orientationRelative = rotrel
trajectoryPoint.orientationAbsolute = rot
trajectoryPoint.gripperLeft = [0.0,0.0]
trajectoryPoint.gripperRight = [0.0,0.0]
trajectoryPoint.pointTime = 5.0
trajectory.append(trajectoryPoint)
# ---------------
msg.trajectory = trajectory
pub.publish(msg)
print('sent msg 2, combined ')
'''
rospy.spin()
