コード例 #1
0
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.')
コード例 #2
0
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.')
コード例 #3
0
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.')
コード例 #4
0
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))
コード例 #5
0
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.')
コード例 #6
0
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.')
コード例 #7
0
    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
コード例 #8
0
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.')
コード例 #9
0
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.")
コード例 #10
0
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.')
コード例 #11
0
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.")
コード例 #12
0
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))
コード例 #13
0
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()
コード例 #14
0
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.')
コード例 #15
0
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))
コード例 #16
0
ファイル: pickup_part.py プロジェクト: klauzr/EENX15-2181
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)
コード例 #17
0
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()
コード例 #18
0
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()