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 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 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 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 getMsg(self):
self.newTrajectory = 0
msg = Trajectory_msg()
msg.header.stamp = rospy.Time.now()
msg.mode = self.mode
self.trajectory = []
if self.mode == 'combined':
absolutePosition, absoluteOrientation, relativePosition, relativeOrientation = \
utils.calcAbsoluteAndRelative(self.gripperRight, self.gripperLeft, self.transformer)
absoluteTemp = self.gripperRight.copyClass()
relativeTemp = self.gripperLeft.copyClass()
absoluteTemp.update(absolutePosition, absoluteOrientation)
relativeTemp.update(relativePosition, relativeOrientation)
absoluteTemp.flipped = -1
minDist, lengthRight, minIndex = utils.closesPointDLO(
self.DLO, self.gripperRight.getPosition())
minDist, lengthLeft, minIndex = utils.closesPointDLO(
self.DLO, self.gripperLeft.getPosition())
if lengthRight > lengthLeft:
absoluteTemp.flipped = 1
else:
gripperRightTemp = self.gripperRight.copyClass()
gripperLeftTemp = self.gripperLeft.copyClass()
for i in range(self.startSubTaskIdx, len(self.subTasks)):
inputArgs = self.subTasks[i].inputArgs
input_ = []
for j in range(len(inputArgs)):
input_.append(eval(inputArgs[j]))
trajectoryPoint = self.subTasks[i].getTrajectoryPoint(input_)
self.trajectory.extend(trajectoryPoint)
msg.trajectory = self.trajectory
return msg
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 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 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 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 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 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()