def functional():
    pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal',
                               FollowJointTrajectoryActionGoal,
                               queue_size=10)
    pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal',
                                FollowJointTrajectoryActionGoal,
                                queue_size=10)
    rospy.init_node('traj_maker', anonymous=True)
    time.sleep(4)

    rate = rospy.Rate(0.01)

    while not rospy.is_shutdown():

        traj_waypoint_1_gary = JointTrajectoryPoint()
        traj_waypoint_1_rosey = JointTrajectoryPoint()

        traj_waypoint_1_gary.positions = [0, 0, 0, 0, 0, 0]
        traj_waypoint_1_rosey.positions = [0, 0, 0, 0, 0, 0]

        traj_waypoint_1_rosey.time_from_start.secs = 10
        traj_waypoint_1_gary.time_from_start.secs = 10

        #  making message
        message_gary = FollowJointTrajectoryActionGoal()
        message_rosey = FollowJointTrajectoryActionGoal()

        #  required headers
        header_gary = std_msgs.msg.Header(stamp=rospy.Time.now())
        header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
        message_gary.goal.trajectory.header = header_gary
        message_rosey.goal.trajectory.header = header_rosey
        message_gary.header = header_gary
        message_rosey.header = header_rosey

        #  adding in joints
        joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \
                       'joint_5', 'joint_6']
        message_gary.goal.trajectory.joint_names = joint_names
        message_rosey.goal.trajectory.joint_names = joint_names

        #  appending up to 100 points
        # ex. for i in enumerate(len(waypoints)): append(waypoints[i])
        message_gary.goal.trajectory.points.append(traj_waypoint_1_gary)
        message_rosey.goal.trajectory.points.append(traj_waypoint_1_rosey)

        #  publishing to ROS node
        pub_gary.publish(message_gary)
        pub_rosey.publish(message_rosey)

        rate.sleep()

        if rospy.is_shutdown():
            break
        break
Example #2
0
    def publish_msg(self):
        # Set the message to publish as command.
        traj_vector = FollowJointTrajectoryActionGoal()
        # Current ROS time stamp
        h = Header()
        h.stamp = rospy.Time.now()
        traj_vector.header = h
        traj_vector.goal.trajectory.joint_names.append('arm_1_joint')
        traj_vector.goal.trajectory.joint_names.append('arm_2_joint')
        traj_vector.goal.trajectory.joint_names.append('arm_3_joint')
        traj_vector.goal.trajectory.joint_names.append('arm_4_joint')
        traj_vector.goal.trajectory.joint_names.append('arm_5_joint')
        traj_vector.goal.trajectory.joint_names.append('arm_6_joint')
        traj_vector.goal_id.stamp = h.stamp
        traj_vector.goal_id.id = 'rosPathPlanner' + str(h.stamp.secs)
        h2 = Header()
        h2.stamp.secs = h.stamp.secs + 1
        h2.stamp.nsecs = h.stamp.nsecs + 3e8
        traj_vector.goal.trajectory.header.stamp = h2.stamp

        points2pub = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]

        iter = 0.0
        for column in points2pub:
            point = JointTrajectoryPoint()
            iter += 1
            for q in column:
                point.positions.append(q)
                point.time_from_start.nsecs = 0
                point.time_from_start.secs = iter * 10
            traj_vector.goal.trajectory.points.append(point)

        print traj_vector
        print 'All systems go!'
        self.trajPub.publish(traj_vector)
    def callback(self, msg):
        pub_msg = FollowJointTrajectoryActionGoal()
        pub_msg.header = msg.header
        pub_msg.goal_id = msg.goal_id
        pub_msg.goal.trajectory.header = msg.goal.trajectory.header
        pub_msg.goal.trajectory.joint_names = [
            'r_thumb_roll', 'r_thumb_pitch', 'r_middle_pitch'
        ]
        thumb_r_id = msg.goal.trajectory.joint_names.index('THUMB_R')
        thumb_p_id = msg.goal.trajectory.joint_names.index('THUMB_P')
        middle_p_id = msg.goal.trajectory.joint_names.index('MIDDLE_P')

        pub_msg.goal.trajectory.points = []
        for p in msg.goal.trajectory.points:
            point = JointTrajectoryPoint()
            point.positions = [
                p.positions[thumb_r_id], p.positions[thumb_p_id],
                p.positions[middle_p_id]
            ]
            point.velocities = [
                p.velocities[thumb_r_id], p.velocities[thumb_p_id],
                p.velocities[middle_p_id]
            ]
            point.time_from_start = p.time_from_start
            pub_msg.goal.trajectory.points.append(point)

        self.joint_command_pub.publish(pub_msg)
Example #4
0
    def data_loop(self, data):
        target_angles = eval(data.decode())

        trajectory_msg = self.generate_trajectory_msg(target_angles)

        goal_msg = FollowJointTrajectoryGoal()
        goal_msg.trajectory = trajectory_msg

        action_goal_msg = FollowJointTrajectoryActionGoal()
        action_goal_msg.header = trajectory_msg.header
        action_goal_msg.goal = goal_msg
                                    
        self.pub.publish(action_goal_msg)
    def createRobotMsg(self, msg, start_time, step_time):

        poruka = FollowJointTrajectoryActionGoal()

        h = Header()
        h.stamp = rospy.get_rostime()
        #print 'Start H'
        #print h
        #print 'end H'
        poruka.header = h

        poruka.goal.trajectory.joint_names.append('arm_1_joint')
        poruka.goal.trajectory.joint_names.append('arm_2_joint')
        poruka.goal.trajectory.joint_names.append('arm_3_joint')
        poruka.goal.trajectory.joint_names.append('arm_4_joint')
        poruka.goal.trajectory.joint_names.append('arm_5_joint')
        poruka.goal.trajectory.joint_names.append('arm_6_joint')

        poruka.goal_id.id = 'HoCook trajectory ' + str(h.stamp.nsecs)

        #h.stamp = rospy.get_rostime()
        h2 = Header()
        h2.stamp.secs = 0
        h2.stamp.nsecs = 0

        poruka.goal.trajectory.header.stamp = h2.stamp
        poruka.header.stamp = h.stamp

        temp_time = start_time

        for i in range(0, len(msg.pose_joint_1)):
            #for i in range(0, 100):
            tocka = JointTrajectoryPoint()
            '''
            tocka.positions.append(0-1.57)
            tocka.positions.append(0)
            tocka.positions.append(1.57)    # INVERTIRATI!!!!
            tocka.positions.append(i*0.5)
            tocka.positions.append(0)
            tocka.positions.append(0)
            '''
            tocka.positions.append(msg.pose_joint_1[i])
            tocka.positions.append(msg.pose_joint_2[i])
            tocka.positions.append(msg.pose_joint_3[i])  # INVERTIRATI!!!!
            tocka.positions.append(msg.pose_joint_4[i])
            tocka.positions.append(msg.pose_joint_5[i])
            tocka.positions.append(msg.pose_joint_6[i])

            tocka.velocities.append(msg.speed_joint_1[i])
            tocka.velocities.append(msg.speed_joint_2[i])
            tocka.velocities.append(msg.speed_joint_3[i])
            tocka.velocities.append(msg.speed_joint_4[i])
            tocka.velocities.append(msg.speed_joint_5[i])
            tocka.velocities.append(msg.speed_joint_6[i])
            '''
            tocka.velocities.append(0)
            tocka.velocities.append(0)
            tocka.velocities.append(0)
            tocka.velocities.append(0)
            tocka.velocities.append(0)
            tocka.velocities.append(0)
            '''
            tocka.accelerations.append(0)
            tocka.accelerations.append(0)
            tocka.accelerations.append(0)
            tocka.accelerations.append(0)
            tocka.accelerations.append(0)
            tocka.accelerations.append(0)

            temp_time = temp_time + step_time

            tocka.time_from_start.nsecs = round(
                (temp_time - math.floor(temp_time)) * pow(10, 9))
            tocka.time_from_start.secs = int(round(math.floor(temp_time)))
            #print 'time = ', str(temp_time), ', ', str(math.floor(temp_time)), ', ', str(round((temp_time - math.floor(temp_time))*pow(10, 9))), ', '
            poruka.goal.trajectory.points.append(tocka)

        return poruka
Example #6
0
def functional():
    pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal',
     FollowJointTrajectoryActionGoal, queue_size=10)
    pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal',
     FollowJointTrajectoryActionGoal, queue_size=10)
    rospy.init_node('traj_maker', anonymous=True)
    time.sleep(3)

    rate = rospy.Rate(0.25)

    commanded_trajectory = [[.31, -.051, .33, -.55, .28, .60], #P1
        [.14726, -.014151, .166507, -.33571, .395997, .38657], #P2
        [-.09309, .003150, .003559, .16149, .524427, -.1867],  #P3
        [-.27752, .077886, -.1828, .38563, .682589, -.44665],  #P4
        [-.09309, .003150, .003559, .16149, .524427, -.1867],  #P3
        [.14726, -.014151, .166507, -.33571, .395997, .38657]] #P2
        #this will repeat

    while not rospy.is_shutdown():
       i=0
       for command in commanded_trajectory:

           #  first way to define a point
           traj_waypoint_1_gary = JointTrajectoryPoint()
           traj_waypoint_1_rosey = JointTrajectoryPoint
           
           #  second way to define a point
           traj_waypoint_gary = JointTrajectoryPoint(positions=command,
            time_from_start = Duration(1))
           traj_waypoint_rosey = JointTrajectoryPoint(positions=command,
            time_from_start = Duration(1))  
           
           #  making message
           message_gary = FollowJointTrajectoryActionGoal()
           message_rosey = FollowJointTrajectoryActionGoal()
           
           #  required headers
           header_gary = std_msgs.msg.Header(stamp=rospy.Time.now())
           header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
           message_gary.goal.trajectory.header = header_gary
           message_rosey.goal.trajectory.header = header_rosey
           message_gary.header = header_gary
           message_rosey.header = header_rosey
           
           #  adding in joints
           joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \
                          'joint_5', 'joint_6']
           message_gary.goal.trajectory.joint_names = joint_names
           message_rosey.goal.trajectory.joint_names = joint_names
           
           #  appending up to 100 points
           # ex. for i in enumerate(len(waypoints)): append(waypoints[i])
           if (i > 0):
               message_gary.goal.trajectory.points.append(old_waypoint)
           message_gary.goal.trajectory.points.append(traj_waypoint_gary)
           if (i > 0):
               message_rosey.goal.trajectory.points.append(old_waypoint) #current location
           message_rosey.goal.trajectory.points.append(traj_waypoint_rosey) #now "second" for timing

           #  publishing to ROS node
           #if (i % 3 == 0):  # every 3 cycles
           pub_gary.publish(message_gary)
           pub_rosey.publish(message_rosey)
           print 'published: ' + str(commanded_trajectory[i])
         
           rate.sleep()
           i += 1
           old_waypoint = traj_waypoint_rosey
           old_waypoint.time_from_start.secs = 0
           #if i ==1:
           #    rate.sleep()
           #    rate.sleep()
           #    rate.sleep()
           if i == 5:
               return
           if rospy.is_shutdown():
               break
Example #7
0
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.msg import FollowJointTrajectoryGoal
from std_msgs.msg import Header
import random

if __name__ == "__main__":
    pub = rospy.Publisher('/arm_1/arm_controller/follow_joint_trajectory/goal',
                          FollowJointTrajectoryActionGoal,
                          queue_size=10)
    rospy.init_node('fake_traj_publisher')
    rate = rospy.Rate(0.5)  # 10 Hz
    d = rospy.Duration
    while not rospy.is_shutdown():
        fake_traj = FollowJointTrajectoryActionGoal()
        fake_traj.header = Header()
        # fake_traj.header.stamp

        fake_traj.goal = FollowJointTrajectoryGoal()

        fake_traj.goal.trajectory = JointTrajectory()
        fake_traj.goal.trajectory.joint_names = [
            'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5'
        ]

        # fake_traj.goal.trajectory.points = JointTrajectoryPoint()[]
        for j in xrange(20):
            point = JointTrajectoryPoint()
            point.positions = [random.random() for i in xrange(5)]
            point.velocities = [random.random() for i in xrange(5)]
            point.time_from_start = rospy.Duration.from_sec(j)
Example #8
0
def functional(commanded_trajectory):
    pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal',
                               FollowJointTrajectoryActionGoal,
                               queue_size=10)
    pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal',
                                FollowJointTrajectoryActionGoal,
                                queue_size=10)
    rospy.init_node('traj_maker', anonymous=True)
    time.sleep(4)

    rate = rospy.Rate(0.01)
    i = 0
    while not rospy.is_shutdown():
        for command in commanded_trajectory:
            i += 1

            #  define a point
            traj_waypoint_1_gary = JointTrajectoryPoint()
            traj_waypoint_1_rosey = JointTrajectoryPoint()

            traj_waypoint_1_gary.positions = command
            traj_waypoint_1_rosey.positions = [-1 * elem for elem in command]

            traj_waypoint_1_rosey.time_from_start.secs = 0 + 2 * i  #first move slow
            traj_waypoint_1_gary.time_from_start.secs = 0 + 2 * i

            #debug in terminal
            print traj_waypoint_1_gary.positions
            print traj_waypoint_1_rosey.positions

            #  making message
            message_gary = FollowJointTrajectoryActionGoal()
            message_rosey = FollowJointTrajectoryActionGoal()

            #  required headers
            header_gary = std_msgs.msg.Header(stamp=rospy.Time.now())
            header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now())
            message_gary.goal.trajectory.header = header_gary
            message_rosey.goal.trajectory.header = header_rosey
            message_gary.header = header_gary
            message_rosey.header = header_rosey

            #  adding in joints
            joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \
                           'joint_5', 'joint_6']
            message_gary.goal.trajectory.joint_names = joint_names
            message_rosey.goal.trajectory.joint_names = joint_names

            #  appending up to 100 points
            # ex. for i in enumerate(len(waypoints)): append(waypoints[i])
            message_gary.goal.trajectory.points = [traj_waypoint_1_gary]
            message_rosey.goal.trajectory.points = [traj_waypoint_1_rosey]

            #  publishing to ROS node
            pub_gary.publish(message_gary)
            pub_rosey.publish(message_rosey)

            rate.sleep()

            if rospy.is_shutdown():
                break
Example #9
0
    def moveRobot(self):

        poruka = FollowJointTrajectoryActionGoal()

        h = Header()
        h.stamp = rospy.get_rostime()
        print 'Start H'
        print h
        print 'end H'
        poruka.header = h

        poruka.goal.trajectory.joint_names.append('arm_1_joint')
        poruka.goal.trajectory.joint_names.append('arm_2_joint')
        poruka.goal.trajectory.joint_names.append('arm_3_joint')
        poruka.goal.trajectory.joint_names.append('arm_4_joint')
        poruka.goal.trajectory.joint_names.append('arm_5_joint')
        poruka.goal.trajectory.joint_names.append('arm_6_joint')

        poruka.goal_id.id = 'Move trajectory ' + str(h.stamp.nsecs)

        h.stamp = rospy.get_rostime()
        poruka.goal.trajectory.header.stamp = h.stamp
        poruka.header.stamp = h.stamp

        tocka = JointTrajectoryPoint()
        '''
		tocka.positions.append(0)			
		tocka.positions.append(-1.58)		# 	INVERTIRATI NA CANOPEN
		tocka.positions.append(1.38005) 	# -1.38005 INVERTIRATI na IPI!!!!
		tocka.positions.append(0)
		tocka.positions.append(1.36365) # -1.36365  # 	INVERTIRATI NA CANOPEN
		tocka.positions.append(0)
		'''
        if (1 == 1):
            tocka.positions.append(0)
            tocka.positions.append(0)
            tocka.positions.append(-1.57)  # INVERTIRATI!!!!
            tocka.positions.append(0)
            tocka.positions.append(0)
            tocka.positions.append(0)
        else:

            tocka.positions.append(0)
            tocka.positions.append(0.0)
            tocka.positions.append(1.636)  # INVERTIRATI!!!!
            tocka.positions.append(0.0)
            tocka.positions.append(-0.0637)
            tocka.positions.append(0)

        tocka.velocities.append(0)
        tocka.velocities.append(0)
        tocka.velocities.append(0)
        tocka.velocities.append(0)
        tocka.velocities.append(0)
        tocka.velocities.append(0)

        tocka.accelerations.append(0)
        tocka.accelerations.append(0)
        tocka.accelerations.append(0)
        tocka.accelerations.append(0)
        tocka.accelerations.append(0)
        tocka.accelerations.append(0)

        tocka.time_from_start.nsecs = 0 * pow(10, 9)
        tocka.time_from_start.secs = 7

        poruka.goal.trajectory.points.append(tocka)

        self.trajPub_red.publish(poruka)
        #self.trajPub_blue.publish(poruka)

        print ""
        print poruka
        print rospy.get_rostime()
        print h.stamp