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 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 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 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 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 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 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 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()