Ejemplo n.º 1
2
def scale_trajectory_speed(traj, scale):
    # Create a new trajectory object
    new_traj = RobotTrajectory()
    # Initialize the new trajectory to be the same as the planned trajectory
    new_traj.joint_trajectory = traj.joint_trajectory
    # Get the number of joints involved
    n_joints = len(traj.joint_trajectory.joint_names)
    # Get the number of points on the trajectory
    n_points = len(traj.joint_trajectory.points)
    # Store the trajectory points
    points = list(traj.joint_trajectory.points)
    # Cycle through all points and scale the time from start, speed and acceleration
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        point.positions = traj.joint_trajectory.points[i].positions
        for j in range(n_joints):
            point.velocities[j] = point.velocities[j] * scale
            point.accelerations[j] = point.accelerations[j] * scale * scale
        points[i] = point
    # Assign the modified points to the new trajectory
    new_traj.joint_trajectory.points = points
    # Return the new trajectory
    return new_traj
Ejemplo n.º 2
0
def main():

    pub = rospy.Publisher('sevenbot/joint_traj_controller/trajectory_command',JointTrajectory)
    rospy.init_node('traj_test')

    p1 = JointTrajectoryPoint()
    p1.positions = [0,0,0,0,0,0,0]
    p1.velocities = [0,0,0,0,0,0,0]
    p1.accelerations = [0,0,0,0,0,0,0]

    p2 = JointTrajectoryPoint()
    p2.positions = [0,0,1.0,0,-1.0,0,0]
    p2.velocities = [0,0,0,0,0,0,0]
    p2.accelerations = [0,0,0,0,0,0,0]
    p2.time_from_start = rospy.Time(4.0)

    p3 = JointTrajectoryPoint()
    p3.positions = [0,0,-1.0,0,1.0,0,0]
    p3.velocities = [0,0,0,0,0,0,0]
    p3.accelerations = [0,0,0,0,0,0,0]
    p3.time_from_start = rospy.Time(20.0)


    traj = JointTrajectory()
    traj.joint_names = ['j1','j2','j3','j4','j5','j6','j7']
    traj.points = [p1,p2,p3]


    rospy.loginfo(traj)

    r = rospy.Rate(1) # 10hz
    pub.publish(traj)
    r.sleep()
    pub.publish(traj)
Ejemplo n.º 3
0
 def add_point(self, positions, velocities, accelerations, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = copy(velocities)
     point.accelerations = copy(accelerations)
     point.time_from_start = rospy.Duration(time)
     self._goal.trajectory.points.append(point)
Ejemplo n.º 4
0
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'):
    '''
    This function will convert the plan to a joint trajectory compatible with the 
    /arm_N/arm_controller/command topic
    '''
    theplan = plan
    
    # create joint trajectory message
    jt = JointTrajectory()
    
    # fill the header
    jt.header.seq = seq
    jt.header.stamp.secs = 0 #secs
    jt.header.stamp.nsecs = 0 #nsecs
    jt.header.frame_id = frame_id
    
    # specify the joint names
    jt.joint_names = theplan.joint_trajectory.joint_names
    
    # fill the trajectory points and computer constraint times
    njtp = len(theplan.joint_trajectory.points)
    for ii in range(0, njtp):
        jtp = JointTrajectoryPoint()
        jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
        jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.accelerations =[0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.time_from_start = rospy.Duration.from_sec(secs + dt*(ii+1))
        jt.points.append(jtp)
    return jt
Ejemplo n.º 5
0
 def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if self._mode == "velocity":
         velocities = [0.0] * len(joint_names)
         cmd = dict(zip(joint_names, velocities))
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_velocities(cmd)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
     elif self._mode == "position" or self._mode == "position_w_id":
         raw_pos_mode = self._mode == "position_w_id"
         if raw_pos_mode:
             pnt = JointTrajectoryPoint()
             pnt.positions = self._get_current_position(joint_names)
             if dimensions_dict["velocities"]:
                 pnt.velocities = [0.0] * len(joint_names)
             if dimensions_dict["accelerations"]:
                 pnt.accelerations = [0.0] * len(joint_names)
         while not self._server.is_new_goal_available() and self._alive:
             self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
             # zero inverse dynamics feedforward command
             if self._mode == "position_w_id":
                 pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
                 self._pub_ff_cmd.publish(ff_pnt)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
Ejemplo n.º 6
0
def solution2hand_joint_path(solution, robot, total_time):

    trajectory = JointTrajectory()
    trajectory.joint_names = ["left_hand_synergy_joint"]

    n_waypoints = len(solution['joint_trajectory'])

    time_sec = 0.0001
    time_incr = float(total_time) / n_waypoints

    for i in range(n_waypoints):

        #extrapolation = 1.1
        joint_cmd = np.max(solution['joint_trajectory'][i])
        if i >= n_waypoints - 2:
            joint_cmd = np.minimum(joint_cmd * 1.5, 1.0)
        else:
            joint_cmd = joint_cmd
        rospy.loginfo("Joint command %d = %f" % (i, joint_cmd))
        joint_positions = [joint_cmd]

        point = JointTrajectoryPoint()
        point.positions = joint_positions
        point.velocities = [0.0] * len(joint_positions)
        point.accelerations = [0.0] * len(joint_positions)

        point.time_from_start = rospy.Duration(time_sec)

        trajectory.points.append(point)

        time_sec += time_incr

    return trajectory
Ejemplo n.º 7
0
    def scale_trajectory_speed(traj, scale):
        new_traj = RobotTrajectory()
        new_traj.joint_trajectory = traj.joint_trajectory

        n_joints = len(traj.joint_trajectory.joint_names)
        n_points = len(traj.joint_trajectory.points)
        points = list(traj.joint_trajectory.points)

        for i in range(n_points):
            point = JointTrajectoryPoint()
            point.positions = traj.joint_trajectory.points[i].positions

            point.time_from_start = traj.joint_trajectory.points[
                i].time_from_start / scale
            point.velocities = list(traj.joint_trajectory.points[i].velocities)
            point.accelerations = list(
                traj.joint_trajectory.points[i].accelerations)

            for j in range(n_joints):
                point.velocities[j] = point.velocities[j] * scale
                point.accelerations[j] = point.accelerations[j] * scale * scale
            points[i] = point

        new_traj.joint_trajectory.points = points
        return new_traj
def runTrajectory(req):

    global Traj_server;
    
    print "---------------------------------"
    print req.task
    print " "
    print req.bin_num
    print " "
    print req.using_torso
    print "---------------------------------"
    
    if req.using_torso == "y":
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num);
    else:
        file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num);

    if req.task == "Forward":
        file_name = file_root+"/forward";
    elif req.task == "Drop":
        file_name = file_root+"/drop";
    else :
        return taskResponse(False);

    f = open(file_name,"r");
    plan_file = RobotTrajectory();
    buf = f.read();
    plan_file.deserialize(buf);
    
    plan = copy.deepcopy(plan_file); 
  
        
    arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); 
    arm_left_group = moveit_commander.MoveGroupCommander("arm_left");	
    arm_left_group.set_start_state_to_current_state();    
        
    StartPnt = JointTrajectoryPoint();
    StartPnt.positions = arm_left_group.get_current_joint_values();
    StartPnt.velocities = [0]*len(StartPnt.positions);
    StartPnt. accelerations = [0]*len(StartPnt.positions);
    
    #print StartPnt;
    plan.joint_trajectory.points[0] = StartPnt;
    #print plan;
    
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    display_trajectory = moveit_msgs.msg.DisplayTrajectory();    
    robot = moveit_commander.RobotCommander();
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    display_trajectory_publisher.publish(display_trajectory);  
   
   
    print "============ Waiting while", file_name, " is visualized (again)..." 
    arm_left_group.execute(plan);
    print "Trajectory ", file_name, " finished!"   
    
    f.close();
    
    return taskResponse(True);
Ejemplo n.º 9
0
    def move_joint_traj(self, traj, dt):
        goal = FollowJointTrajectoryActionGoal()
        goal.goal.trajectory.joint_names = [
            'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
        ]
        for i in range(len(traj)):
            point = JointTrajectoryPoint()

            p = [0] * 6
            v = [0] * 6
            last_v = [0] * 6
            a = [0] * 6
            if i != len(traj) - 1 and i != 0:
                for j in range(6):
                    p[j] = traj[i][j]
                    v[j] = (traj[i + 1][j] - traj[i][j]) / dt
                    last_v[j] = (traj[i][j] - traj[i - 1][j]) / dt
                    a[j] = (v[j] - last_v[j]) / dt
            if i == len(traj) - 1:
                p = traj[-1]
            elif i == 0:
                p = traj[0]
            point.positions = p
            point.velocities = v
            point.accelerations = a
            point.time_from_start = rospy.Duration((i + 1) * dt)
            goal.goal.trajectory.points.append(point)

        self.jta.send_goal_and_wait(goal.goal)
Ejemplo n.º 10
0
 def __extend_trajectory_helper(self, plan, start_pose, travel_distance, step=0.005, time_step=0.1):
   traj = plan.joint_trajectory.points
   cur_pose_z = start_pose.position.z 
   start_time = plan.joint_trajectory.points[-1].time_from_start
   for i in range(0, int(abs(travel_distance) / step)):
     point = JointTrajectoryPoint()
     print i
     joint_goal = self.__inverse_kinematics([start_pose.position.x, start_pose.position.y, cur_pose_z + (np.sign(travel_distance) * step)], 
       [start_pose.orientation.w, start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z], NIC_SEED_STATE)
     if joint_goal == None:
       print "No IK on extend trajectory found"
       return None
     joint_list = []
     for idx, jnt in enumerate(joint_goal.tolist()):
       joint_list.append(jnt)
     point.positions = joint_list
     print joint_list
     point.velocities = [0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.0]
     point.accelerations = [0,0,0,0,0,0,0]
     time = rospy.Duration.from_sec(start_time.to_sec() + time_step)
     start_time += rospy.Duration.from_sec(time_step)
     point.time_from_start = time
     traj.append(point)
     cur_pose_z += np.sign(travel_distance) * step
   return plan
 def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if self._mode == 'velocity':
         velocities = [0.0] * len(joint_names)
         cmd = dict(zip(joint_names, velocities))
         while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()):
             self._limb.set_joint_velocities(cmd)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
     elif self._mode == 'position' or self._mode == 'position_w_id':
         raw_pos_mode = (self._mode == 'position_w_id')
         if raw_pos_mode:
             pnt = JointTrajectoryPoint()
             pnt.positions = self._get_current_position(joint_names)
             if dimensions_dict['velocities']:
                 pnt.velocities = [0.0] * len(joint_names)
             if dimensions_dict['accelerations']:
                 pnt.accelerations = [0.0] * len(joint_names)
         while (not self._server.is_new_goal_available() and self._alive and self.robot_is_enabled()):
             self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
             # zero inverse dynamics feedforward command
             if self._mode == 'position_w_id':
                 pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
                 self._pub_ff_cmd.publish(ff_pnt)
             if self._cuff_state:
                 self._limb.exit_control_mode()
                 break
             rospy.sleep(1.0 / self._control_rate)
Ejemplo n.º 12
0
def traj_speed_up(traj, spd=3.0):
    new_traj = RobotTrajectory()
    new_traj = traj

    n_joints = len(traj.joint_trajectory.joint_names)
    n_points = len(traj.joint_trajectory.points)

    points = list(traj.joint_trajectory.points)
    
    for i in range(n_points):
        point = JointTrajectoryPoint()
        point.time_from_start = traj.joint_trajectory.points[i].time_from_start / spd
        point.velocities = list(traj.joint_trajectory.points[i].velocities)
        point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
        point.positions = traj.joint_trajectory.points[i].positions

        for j in range(n_joints):
            point.velocities[j] = point.velocities[j] * spd
            point.accelerations[j] = point.accelerations[j] * spd * spd

        points[i] = point

    new_traj.joint_trajectory.points = points

    return new_traj
def handleJointTrajMsg(data):
	#### Initialize Speed Parameter
	#   axes  [l.x   l.y   l.z   a.x     a.y    a.z]
	# scalers = [0.7,  0.7,  0.7,  -3.14, -3.14, -3.14]	

	joint_traj_publisher = rospy.Publisher("robotiq_3f_controller/command", JointTrajectory)
	#### Setup JointTraj Publisher 
	JointTraj = JointTrajectory()
	JointTrajPoints = JointTrajectoryPoint()
	#### Start Mapping from PoseStamped to JointTraj
	JointTraj.header.stamp = rospy.get_rostime()
	JointTraj.joint_names = ['finger_1_joint_1','finger_1_joint_2','finger_1_joint_3','finger_2_joint_1','finger_2_joint_2','finger_2_joint_3','finger_middle_joint_1','finger_middle_joint_2','finger_middle_joint_3','palm_finger_1_joint','palm_finger_2_joint']
	# data size is 30
	JointTrajPoints.positions = data.position
	JointTrajPoints.velocities = data.velocity
	JointTrajPoints.accelerations = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
	JointTrajPoints.effort = [0]  
	JointTrajPoints.time_from_start = rospy.Duration.from_sec(0.1)

	JointTraj.points.append(JointTrajPoints)
	# JointTraj.JointTraj.linear.x=data.pose.position.y*clk*scale_factor_pos*-1
	# JointTraj.JointTraj.linear.y=data.pose.position.z*clk*scale_factor_pos
	# JointTraj.JointTraj.linear.z=data.pose.position.x*clk*scale_factor_pos

	# JointTraj.JointTraj.angular.x=data.pose.orientation.x
	# JointTraj.JointTraj.angular.y=data.pose.orientation.y
	# JointTraj.JointTraj.angular.z=data.pose.orientation.z


	#### Publish msg
	rate = rospy.Rate(100) # 100hz
	rospy.loginfo(data)
	joint_traj_publisher.publish(JointTraj)
	rate.sleep()
def scale_trajectory_speed(traj, scale):
       # Create a new trajectory object
       new_traj = RobotTrajectory()
      
       # Initialize the new trajectory to be the same as the planned trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
      
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
      
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
       
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
      
       # Cycle through all points and scale the time from start, speed and acceleration
       for i in range(n_points):
           point = JointTrajectoryPoint()
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           point.positions = traj.joint_trajectory.points[i].positions
                        
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
           
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj
Ejemplo n.º 15
0
def make_trajectory_msg(plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id="/base_link"):
    """
    This function will convert the plan to a joint trajectory compatible with the 
    /arm_N/arm_controller/command topic
    """
    theplan = plan

    # create joint trajectory message
    jt = JointTrajectory()

    # fill the header
    jt.header.seq = seq
    jt.header.stamp.secs = 0  # secs
    jt.header.stamp.nsecs = 0  # nsecs
    jt.header.frame_id = frame_id

    # specify the joint names
    jt.joint_names = theplan.joint_trajectory.joint_names

    # fill the trajectory points and computer constraint times
    njtp = len(theplan.joint_trajectory.points)
    for ii in range(0, njtp):
        jtp = JointTrajectoryPoint()
        jtp = copy.deepcopy(theplan.joint_trajectory.points[ii])
        jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0]
        jtp.time_from_start = rospy.Duration.from_sec(secs + dt * (ii + 1))
        jt.points.append(jtp)
    return jt
Ejemplo n.º 16
0
    def ProcessTrajectory(self,
                          trajectory,
                          pres_joint_pos,
                          string_trajectories=False):
        joint_names = trajectory.joint_names
        num_joints = len(joint_names)
        trajectory_points = trajectory.points
        pnt_times = [0.0] * len(trajectory_points)
        dimensions_dict = self._determine_dimensions(trajectory_points)
        # Create a new discretized joint trajectory
        num_points = len(trajectory_points)
        if num_points == 0:
            rospy.logerr("%s: Empty Trajectory" % (action_name, ))
            return False, []

        if num_points == 1:
            # Add current position as trajectory point
            first_trajectory_point = JointTrajectoryPoint()
            first_trajectory_point.positions = [
                pres_joint_pos[jnt] for jnt in joint_names
            ]
            # To preserve desired velocities and accelerations, copy them to the first
            # trajectory point if the trajectory is only 1 point.
            if dimensions_dict['velocities']:
                first_trajectory_point.velocities = deepcopy(
                    trajectory_points[0].velocities)
            if dimensions_dict['accelerations']:
                first_trajectory_point.accelerations = deepcopy(
                    trajectory_points[0].accelerations)
            first_trajectory_point.time_from_start = rospy.Duration(0)
            trajectory_points.insert(0, first_trajectory_point)
            num_points = len(trajectory_points)

        # Force Velocites/Accelerations to zero at the final timestep
        # if they exist in the trajectory
        # Remove this behavior if you are stringing together trajectories,
        # and want continuous, non-zero velocities/accelerations between
        # trajectories
        if not string_trajectories:
            if dimensions_dict['velocities']:
                trajectory_points[-1].velocities = [0.0] * len(joint_names)
            if dimensions_dict['accelerations']:
                trajectory_points[-1].accelerations = [0.0] * len(joint_names)

        # Compute Full Bezier Curve Coefficients for all 7 joints
        pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
        try:
            b_matrix = self._compute_bezier_coeff(joint_names,
                                                  trajectory_points,
                                                  dimensions_dict)
        except Exception as ex:
            rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
                          " arm with error \"{2}: {3}\"").format(
                              self._action_name, self._name,
                              type(ex).__name__, ex))
            return False, []

        return True, [
            dimensions_dict, b_matrix, trajectory_points, pnt_times, num_points
        ]
Ejemplo n.º 17
0
def cartesian_path2joint_path(waypoints, robot):
    trajectory = JointTrajectory()
    trajectory.joint_names = robot.joint_names('left_arm')

    time_sec = 0.5
    time_incr = 0.5
    for i in range(len(waypoints)):

        wpt = waypoints[i]
        joint_positions = robot.inverse_kinematics(
            [wpt.position.x, wpt.position.y, wpt.position.z], [
                wpt.orientation.x, wpt.orientation.y, wpt.orientation.z,
                wpt.orientation.w
            ])

        if joint_positions is not None:

            point = JointTrajectoryPoint()
            point.positions = joint_positions
            point.velocities = [0.0] * len(joint_positions)
            point.accelerations = [0.0] * len(joint_positions)

            point.time_from_start = rospy.Duration(time_sec)

            trajectory.points.append(point)

        else:
            rospy.logwarn("Failed to find ik solution for grasp waypoint %d" %
                          (i, ))

        time_sec += time_incr

    return trajectory
    def followTrajectory(self, traj):
        # Generate ROS trajectory message
        # Don't send head pose! plan_runner freaks out
        msg = SendJointTrajectoryRequest()
        msg.trajectory.header.stamp = rospy.Time.now()
        msg.trajectory.header.seq = 0
        msg.trajectory.joint_names = self.joint_names[2:]
        for sol in traj:
            # print "sol:", sol
            point = JointTrajectoryPoint()
            point.time_from_start = rospy.Duration(sol[0])
            point.positions = sol[3:]
            point.velocities = [0.0]*len(point.positions)
            point.accelerations = [0.0]*len(point.positions)
            point.effort = [0.0]*len(point.positions)
            msg.trajectory.points.append(point)

        # Generate ROS head message
        # All the head poses are the same -- just use the last one
        # print "Moving head to pose:", traj[-1, 1:3]
        head_msg = MoveHeadRequest(traj[-1, 1:3])
        self.move_head_service(head_msg)

        # Call SendTrajectory service
        self.traj_service(msg)
def handle_joints_move(req):
    goal = FollowJointTrajectoryGoal()
    goal.trajectory.joint_names = joint_names
    current_joints = get_current_joints()
    max_inc = max(
        map(lambda (x, y): abs(x - y), zip(req.positions, current_joints)))
    if abs(max_inc) < 0.0174:
        return QueryJointsMovementResponse(success=1)
    j_traj = JointTrajectoryPoint()
    #duration = max_inc/req.velocity
    duration = 2
    j_traj.time_from_start = rospy.Duration(duration)
    j_traj.positions = req.positions
    j_traj.velocities = [0] * 7
    j_traj.accelerations = [0] * 7
    j_traj.effort = [0.0] * 7
    goal.trajectory.points.append(j_traj)

    try:
        # Creates the SimpleActionClient, passing the type of the action
        # (FollowJointTrajectoryAction) to the constructor.
        action_client = actionlib.SimpleActionClient(
            action_client_name, FollowJointTrajectoryAction)
        # Waits until the action server has started up and started
        # listening for goals.
        action_client.wait_for_server()
        # Send goal
        action_client.send_goal(goal)
        rospy.sleep(duration)
    except rospy.ROSInterruptException as e:
        print(str(e))
    return QueryJointsMovementResponse(success=1)
    def move_arm(self, points_list):
        # # Unpause the physics
        # rospy.wait_for_service('/gazebo/unpause_physics')
        # unpause_gazebo = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        # unpause_gazebo()

        self.client.wait_for_server()

        goal = FollowJointTrajectoryGoal()

        # We need to fill the goal message with its components
        #
        # check msg structure with: rosmsg info FollowJointTrajectoryGoal
        # It is composed of 4 sub-messages:
        # "trajectory" of type trajectory_msgs/JointTrajectory
        # "path_tolerance" of type control_msgs/JointTolerance
        # "goal_tolerance" of type control_msgs/JointTolerance
        # "goal_time_tolerance" of type duration

        trajectory_msg = JointTrajectory()
        # check msg structure with: rosmsg info JointTrajectory
        # It is composed of 3 sub-messages:
        # "header" of type std_msgs/Header
        # "joint_names" of type string
        # "points" of type trajectory_msgs/JointTrajectoryPoint

        trajectory_msg.joint_names = [
            "j2n6s300_joint_1", "j2n6s300_joint_2", "j2n6s300_joint_3",
            "j2n6s300_joint_4", "j2n6s300_joint_5", "j2n6s300_joint_6"
        ]

        points_msg = JointTrajectoryPoint()
        # check msg structure with: rosmsg info JointTrajectoryPoint
        # It is composed of 5 sub-messages:
        # "positions" of type float64
        # "velocities" of type float64
        # "accelerations" of type float64
        # "efforts" of type float64
        # "time_from_start" of type duration

        points_msg.positions = [0, 0, 0, 0, 0, 0]
        points_msg.velocities = [0, 0, 0, 0, 0, 0]
        points_msg.accelerations = [0, 0, 0, 0, 0, 0]
        points_msg.effort = [0, 1, 0, 0, 0, 0]
        points_msg.time_from_start = rospy.Duration(0.01)

        # fill in points message of the trajectory message
        trajectory_msg.points = [points_msg]

        # fill in trajectory message of the goal
        goal.trajectory = trajectory_msg

        print(trajectory_msg)

        # self.client.send_goal_and_wait(goal)
        self.client.send_goal(goal)
        self.client.wait_for_result()

        rospy.sleep(2)  # wait for 2s
Ejemplo n.º 21
0
 def add_point(self, positions, time):
     point = JointTrajectoryPoint()
     point.positions = copy(positions)
     point.velocities = copy(np.zeros(len(positions)))
     point.accelerations = copy(np.zeros(len(positions)))
     point.effort = copy(np.zeros(len(positions)))
     point.time_from_start = rospy.Duration(time)
     self._goal.trajectory.points.append(point)
Ejemplo n.º 22
0
def talker():
    if 1 == 0:
        pub = rospy.Publisher("/ihmc_ros/atlas/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
        jn = ["l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx", "l_arm_wry2"]
        jn_r = ["r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx", "r_arm_wry2"]
    else:
        pub = rospy.Publisher("/ihmc_ros/valkyrie/control/arm_joint_trajectory2", JointTrajectory, queue_size=10)
        jn = [
            "LeftShoulderPitch",
            "LeftShoulderRoll",
            "LeftShoulderYaw",
            "LeftElbowPitch",
            "LeftForearmYaw",
            "LeftWristRoll",
            "LeftWristPitch",
        ]
        jn_r = [
            "RightShoulderPitch",
            "RightShoulderRoll",
            "RightShoulderYaw",
            "RightElbowPitch",
            "RightForearmYaw",
            "RightWristRoll",
            "RightWristPitch",
        ]

    # this doesnt work:
    # jn = ["l_arm_shz","l_arm_shx","l_arm_ely","l_arm_elx","l_arm_wry","l_arm_wrx","l_arm_wry2", "r_arm_shz","l_arm_shx","r_arm_ely","r_arm_elx","r_arm_wry","r_arm_wrx","r_arm_wry2"]
    # value = 0

    rospy.init_node("send_arm_test", anonymous=True)
    rate = rospy.Rate(1)  # 10hz
    while not rospy.is_shutdown():

        msg = JointTrajectory()
        value = 0.1
        value_r = 0.1

        msg.joint_names = jn
        pt = JointTrajectoryPoint()
        pt.positions = [value] * len(jn)
        pt.velocities = [0] * len(jn)
        pt.accelerations = [0] * len(jn)
        pt.effort = [0] * len(jn)
        pt.time_from_start = rospy.Duration.from_sec(3)

        msg.points.append(pt)
        print msg.joint_names, rospy.get_time()
        pub.publish(msg)

        # TODO: add a sleep here between left and right

        msg.joint_names = jn_r
        msg.points[0].positions = [value_r] * len(jn)
        print msg.joint_names, rospy.get_time()
        pub.publish(msg)

        rate.sleep()
Ejemplo n.º 23
0
def construct_joint_point(position, t):
    point = JointTrajectoryPoint()
    point.positions = position
    if not isinstance(t, rospy.Duration):
        t = rospy.Duration(t)
    point.velocities = [0.0] * len(position)
    point.accelerations = [0.0] * len(position)
    point.time_from_start = t
    return point
Ejemplo n.º 24
0
    def trajectory_point(self, t, jointspace):
        """
        takes a discrete point in time, and puts the position, velocity, and
        acceleration into a ROS JointTrajectoryPoint() to be put into a 
        RobotTrajectory.  
        
        Parameters
        ----------
        t : float
        jointspace : bool
            What kind of trajectory.  Joint space points are 7x' and describe the
            angle of each arm.  Workspace points are 3x', and describe the x,y,z
            position of the end effector.  

        Returns
        -------
        :obj:`trajectory_msgs.msg.JointTrajectoryPoint`
        """
        point = JointTrajectoryPoint()
        delta_t = .01
        if jointspace:
            x_t, x_t_1, x_t_2 = None, None, None
            ik_attempts = 0
            theta_t_2 = self.get_ik(self.target_position(t-2*delta_t))
            theta_t_1 = self.get_ik(self.target_position(t-delta_t))
            theta_t   = self.get_ik(self.target_position(t))
            # print(self.target_position(t))
            #theta_t = np.array(theta_t)
            # print(theta_t)
            
            # we said you shouldn't simply take a finite difference when creating
            # the path, why do you think we're doing that here? cause you're mean

            point.positions = theta_t
            point.velocities = (theta_t - theta_t_1) / delta_t
            point.accelerations = (theta_t - 2*theta_t_1 + theta_t_2) / (2*delta_t)

        else:
            point.positions = self.target_position(t)
            point.velocities = self.target_velocity(t)
            point.accelerations = self.target_acceleration(t)
        point.time_from_start = rospy.Duration.from_sec(t)
        return point
Ejemplo n.º 25
0
    def set_position(self,goal):
	traj = JointTrajectory()
	traj_p = JointTrajectoryPoint()
	traj.joint_names = ["torso_lift_joint"]
	traj_p.positions = [goal]
	traj_p.velocities = [0.]
	traj_p.accelerations = [0.]
	traj_p.time_from_start = rospy.Duration(2.)
	traj.points.append(traj_p)
	self.pub.publish(traj)
Ejemplo n.º 26
0
    def reverse_planning(self, plan):

        rev_plan = RobotTrajectory()
        rev_joint_traj = JointTrajectory()

        rev_joint_traj.header = copy.deepcopy(plan.joint_trajectory.header)
        rev_joint_traj.joint_names = copy.deepcopy(plan.joint_trajectory.joint_names)

        new_points = []
        points = plan.joint_trajectory.points # JointTrajectoryPoint[]
        l = len(points)

        '''
        How to reverse the cartesian plan?
        Reverse the order of trajectory point's joint values.
        Change the sign of velocity and acceleration.
        Change the accumulative time parameterization to follow
        the reverse order time gap.

        But you should put the current joint value to the first
        trajectory point. If not, the planner will return error due to
        joint deviation.
        '''
        for i in range(l):

            point = JointTrajectoryPoint()

            if i == 0:
                point.positions = self.ur5.get_current_joint_values()
                point.velocities = [0.0 for val in point.positions]
                point.accelerations = [0.0 for val in point.positions]
                point.time_from_start = rospy.Duration(0)
            else:
                point.positions = copy.deepcopy(points[l-1-i].positions)
                point.velocities = [-value for value in points[l-1-i].velocities]
                point.accelerations = [-value for value in points[l-1-i].accelerations]
                point.time_from_start = new_points[i-1].time_from_start + points[l-i].time_from_start - points[l-1-i].time_from_start

            new_points.append(point)

        rev_joint_traj.points = new_points
        rev_plan.joint_trajectory = rev_joint_traj
        return rev_plan
Ejemplo n.º 27
0
def make_joint_trajectory(names, points):
    jt = JointTrajectory()
    jt.joint_names = names
    pt = JointTrajectoryPoint()
    pt.positions = points
    pt.effort = [0]*len(points)
    pt.velocities = [0]*len(points)
    pt.accelerations = [0]*len(points)
    jt.points = [pt]
    return jt
Ejemplo n.º 28
0
def point():
    global joy_r_v_avg
    global joy_r_h_avg
    global commanded_elev
    global commanded_azim
    rospy.init_node('manual_arm_control')
    rate = rospy.Rate(50)
    pub = rospy.Publisher('/arm_twist', JointTrajectoryPoint, queue_size=1)
    rospy.Subscriber('/arm_start_stop', Bool, run_cb)
    rospy.Subscriber("/joystick", Joy, joy_cb)

    MAX_AZIM = rospy.get_param("/max_azimuth", np.pi * 2)
    MIN_AZIM = rospy.get_param("/min_azimuth", -np.pi * 2)
    MAX_ELEV = rospy.get_param("/max_elevation", np.pi / 4)
    MIN_ELEV = rospy.get_param("/min_elevation", -np.pi / 4)
    AZIM_MAX_SPEED = rospy.get_param("/max_azimuth_speed", np.pi / 2)
    ELEV_MAX_SPEED = rospy.get_param("/max_elev_speed", np.pi / 16)

    while not rospy.is_shutdown():

        if run_var == True:

            commanded_elev_speed = joy_r_v_avg * ELEV_MAX_SPEED
            commanded_elev += commanded_elev_speed * dt
            commanded_azim_speed = joy_r_h_avg * AZIM_MAX_SPEED
            if commanded_azim_speed > 0:
                commanded_azim = 100
            else:
                commanded_azim = -100

            if commanded_azim > MAX_AZIM:
                commanded_azim = MAX_AZIM
                #commanded_azim_speed = 0
            elif commanded_azim < MIN_AZIM:
                commanded_azim = MIN_AZIM
                #commanded_azim_speed = 0
            if commanded_elev > MAX_ELEV:
                commanded_elev = MAX_ELEV
                #commanded_elev_speed = 0
            elif commanded_elev < MIN_ELEV:
                commanded_elev = MIN_ELEV
                #commanded_elev_speed = 0

            p = JointTrajectoryPoint()
            azim = commanded_azim
            elev = commanded_elev
            azim_speed = commanded_azim_speed
            elev_speed = commanded_elev_speed
            p.positions = [commanded_azim, commanded_elev]
            p.velocities = [commanded_azim_speed * 8, commanded_elev_speed * 2]
            p.accelerations = [0, 0]
            #p.time_from_start = rospy.Time.now()
            pub.publish(p)
            rate.sleep()
        '''elif run_var == False:'''  #TODO Set zero to current joint positions to avoid jumps
Ejemplo n.º 29
0
    def execute_cb(self, goal):
        arm_goal = FollowJointTrajectoryGoal()
        arm_goal.goal_time_tolerance = rospy.Time(1.0)
        arm_goal.trajectory.joint_names = [
            'arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4',
            'arm_joint_5'
        ]

        for pt in goal.trajectory.points:
            pt_temp = JointTrajectoryPoint()
            pt_temp.positions = pt.positions[0:5]
            pt_temp.velocities = pt.velocities[0:5]
            pt_temp.accelerations = pt.accelerations[0:5]
            pt_temp.time_from_start = pt.time_from_start
            arm_goal.trajectory.points.append(pt_temp)

        base_goal = FollowJointTrajectoryGoal()
        base_goal.goal_time_tolerance = rospy.Time(1.0)
        base_goal.trajectory.joint_names = [
            'virtual_x', 'virtual_y', 'virtual_theta'
        ]

        for pt in goal.trajectory.points:
            pt_temp = JointTrajectoryPoint()
            pt_temp.positions.append(pt.positions[6])
            pt_temp.positions.append(pt.positions[7])
            pt_temp.positions.append(pt.positions[5])
            pt_temp.velocities.append(pt.velocities[6])
            pt_temp.velocities.append(pt.velocities[7])
            pt_temp.velocities.append(pt.velocities[5])
            pt_temp.accelerations.append(pt.accelerations[6])
            pt_temp.accelerations.append(pt.accelerations[7])
            pt_temp.accelerations.append(pt.accelerations[5])
            pt_temp.time_from_start = pt.time_from_start
            base_goal.trajectory.points.append(pt_temp)

        #rospy.loginfo(base_goal.trajectory.points[0].positions) #list of positions of one point
        #rospy.loginfo(base_goal.trajectory.points[1].positions)
        #rospy.loginfo(base_goal.trajectory.points[2].positions)

        if self._as.is_preempt_requested():
            rospy.loginfo("This action has been preempted")
            self._as.set_preempted()
            success = False
            self._as.set_cancelled()
        else:
            #rospy.loginfo(base_goal.trajectory.points)
            self.base_client.send_goal(base_goal)
            self.arm_client.send_goal_and_wait(arm_goal)
            success = True

        if success:
            rospy.loginfo('Action Succeeded')
            self._as.set_succeeded()
 def move_jtp(self, pos):
     jtp_msg = JointTrajectory()
     jtp_msg.joint_names = self.joint_name_lst
     point = JointTrajectoryPoint()
     point.positions = pos
     point.velocities = self.jtp_zeros
     point.accelerations = self.jtp_zeros
     point.effort = self.jtp_zeros
     point.time_from_start = rospy.Duration(1.0 / 60.0)
     jtp_msg.points.append(point)
     self.jtp.publish(jtp_msg)
Ejemplo n.º 31
0
    def create_JTG(self, arm, pos_arr, dur_arr, stamp=None, vel_arr=None, acc_arr=None):
        # Hack
        vel_arr = [[0.]*7]*len(pos_arr)
        acc_arr = [[0.]*7]*len(pos_arr)

        ##
        # Compute joint velocities and acclereations.
        def get_vel_acc(q_arr, d_arr):
            vel_arr = [[0.]*7]
            acc_arr = [[0.]*7]
            for i in range(1, len(q_arr)):
                vel, acc = [], []
                for j in range(7):
                    vel += [(q_arr[i][j] - q_arr[i-1][j]) / d_arr[i]]
                    acc += [(vel[j] - vel_arr[i-1][j]) / d_arr[i]]
                vel_arr += [vel]
                acc_arr += [acc]
                print vel, acc
            return vel_arr, acc_arr

        if arm != 1:
            arm = 0
        jtg = JointTrajectoryGoal()
        if stamp is None:
            stamp = rospy.Time.now()
        else:
            jtg.trajectory.header.stamp = stamp

        if len(pos_arr) > 1 and (vel_arr is None or acc_arr is None):
            v_arr, a_arr = get_vel_acc(pos_arr, dur_arr)
            if vel_arr is None:
                vel_arr = v_arr
            if acc_arr is None:
                acc_arr = a_arr

        jtg.trajectory.joint_names = self.joint_names_list[arm]
        for i in range(len(pos_arr)):
            if pos_arr[i] is None or type(pos_arr[i]) is types.NoneType:
                continue
            jtp = JointTrajectoryPoint()
            jtp.positions = pos_arr[i]
            if vel_arr is None:
                vel = [0.] * 7
            else:
                vel = vel_arr[i]
            jtp.velocities = vel
            if acc_arr is None:
                acc = [0.] * 7
            else:
                acc = acc_arr[i]
            jtp.accelerations = acc
            jtp.time_from_start = rospy.Duration(dur_arr[i])
            jtg.trajectory.points.append(jtp)
        return jtg
Ejemplo n.º 32
0
    def cmd_pub(self):
        point_goal = JointTrajectoryPoint()
        point_goal.positions = [0.4, -0.5]
        point_goal.velocities = [0, 0]
        point_goal.accelerations = [0, 0]
        point_goal.time_from_start = rospy.Time(secs=2, nsecs=0)

        self.action_goal.trajectory.points.append(point_goal)
        self.action_client.wait_for_server()
        self.action_client.send_goal(self.action_goal)
        self.action_goal.trajectory.points = []
Ejemplo n.º 33
0
    def jointTrajectoryPointFromJointState(jointState, timeFromStart):
        trajPoint = JointTrajectoryPoint()
        trajPoint.positions = jointState.position

        numJoints = len(jointState.position)
        trajPoint.velocities = [0] * numJoints
        trajPoint.accelerations = [0] * numJoints
        trajPoint.effort = [0] * numJoints

        trajPoint.time_from_start = timeFromStart
        return trajPoint
Ejemplo n.º 34
0
 def createArmTrajectory(self, values):
     traj_msg = JointTrajectory()
     traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
     traj_msg.joint_names = self.GetJointNames()
     traj_msg.points = []
     end_point = JointTrajectoryPoint()
     end_point.positions = values
     end_point.velocities = [0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     end_point.time_from_start = rospy.Duration.from_sec(1.0)
     traj_msg.points.append(end_point)
     return traj_msg
Ejemplo n.º 35
0
    def stop(self, include_velocity=False, include_effort=False):
        """
        Stop the recording and returns the recorded trajectory of joints declared in the constructor
        :param include_velocity:  if True, the joint velocities will be included in the returned trajectory
        :param include_effort:  if True, the joint efforts will be included in the returned trajectory
        :return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path)
        """
        with self._recording_lock:
            self._recording = False

        # Now we reconstruct a RobotTrajectory...
        rt = RobotTrajectory()
        rt.joint_trajectory.joint_names = self._joints

        with self._joint_recorder_lock:
            for idx, js in enumerate(self._joint_recorded_points):
                jtp = JointTrajectoryPoint()
                try:
                    jtp.positions = [
                        js.position[js.name.index(joint)]
                        for joint in rt.joint_trajectory.joint_names
                    ]
                except ValueError as e:
                    # This specific JS sample does not contain the desired joints, skip it
                    continue
                else:
                    if include_velocity:
                        jtp.velocities = [
                            js.velocity[js.name.index(joint)]
                            for joint in rt.joint_trajectory.joint_names
                        ]
                    if include_effort:
                        jtp.accelerations = [
                            js.effort[js.name.index(joint)]
                            for joint in rt.joint_trajectory.joint_names
                        ]
                    jtp.time_from_start = self._joint_recorded_times[
                        idx] - self._joint_recorded_times[0]
                    rt.joint_trajectory.points.append(jtp)

        # ... as well as an End Effector trajectory
        eft = Path()
        with self._eef_recorder_lock:
            for eef in self._eef_recorded_points:
                ps = PoseStamped()
                ps.header = eef.header
                ps.header.frame_id = 'base'
                ps.pose = eef.pose
                eft.poses.append(ps)
                # twist and wrench in eef are discarded
        eft.header.frame_id = 'base'

        return rt, eft
Ejemplo n.º 36
0
 def send_left_arm_goal(self, j_positions):
     jt = JointTrajectory()
     jt.header.stamp = rospy.Time.now()
     jt.joint_names = ['l_upper_arm_roll_joint', 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_forearm_roll_joint', 
                       'l_elbow_flex_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
     jtp = JointTrajectoryPoint()
     jtp.positions = list(j_positions)
     jtp.velocities = [0.0] * len(j_positions)
     jtp.accelerations = [0.0] * len(j_positions)
     jtp.time_from_start = rospy.Duration(0.45)
     jt.points.append(jtp)
     self.left_command.publish(jt)
def trapezoid_gen(target, current_joint_angles, duration):
    goal = FollowJointTrajectoryGoal()
    goal.trajectory.joint_names = [
        'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
    ]
    goal.trajectory.header.frame_id = '/world'
    dist = target - current_joint_angles
    vmax = 1.5 * dist / duration
    acc = 3 * vmax / duration

    p1 = JointTrajectoryPoint()
    p1.positions = current_joint_angles
    p1.velocities = np.zeros((6, ))
    p1.accelerations = acc
    p1.time_from_start = rospy.Duration(0)

    p2 = JointTrajectoryPoint()
    p2.positions = np.array(p1.positions) + dist / 4
    p2.velocities = vmax
    p2.accelerations = np.zeros((6, ))
    p2.time_from_start = rospy.Duration(duration / 3)

    p3 = JointTrajectoryPoint()
    p3.positions = np.array(p1.positions) + 3 * dist / 4
    p3.velocities = vmax
    p3.accelerations = -acc
    p3.time_from_start = rospy.Duration(duration * 2 / 3)

    p4 = JointTrajectoryPoint()
    p4.positions = target
    p4.velocities = np.zeros((6, ))
    p4.accelerations = np.zeros((6, ))
    p4.time_from_start = rospy.Duration(duration)

    goal.trajectory.points.append(p1)
    goal.trajectory.points.append(p2)
    goal.trajectory.points.append(p3)
    goal.trajectory.points.append(p4)

    return goal
Ejemplo n.º 38
0
    def set_jointangles(self, arm, q, duration=0.15):
        rospy.logwarn('Currently ignoring the arm parameter.')
#        for i,p in enumerate(self.r_arm_pub_l):
#            p.publish(q[i])
        jtg = JointTrajectoryGoal()
        jtg.trajectory.joint_names = self.joint_names_list
        jtp = JointTrajectoryPoint()
        jtp.positions = q
        jtp.velocities = [0 for i in range(len(q))]
        jtp.accelerations = [0 for i in range(len(q))]
        jtp.time_from_start = rospy.Duration(duration)
        jtg.trajectory.points.append(jtp)
        self.joint_action_client.send_goal(jtg)
Ejemplo n.º 39
0
 def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
     if not self._alive:
         self._limb.exit_control_mode()
     elif self._mode == 'joint_impedance':
         pnt = JointTrajectoryPoint()
         pnt.positions = self._get_current_position(joint_names)
         if self._mode == 'joint_impedance' and self._alive:
             # zero inverse dynamics feedforward command
             pnt.velocities = [0.0] * len(joint_names)
             pnt.accelerations = [0.0] * len(joint_names)
             self._limb.cmd_joint_angles(pnt.positions,
                                         pnt.velocities,
                                         pnt.accelerations)
Ejemplo n.º 40
0
 def set_jointangles(self, arm, q, duration=0.15):
     rospy.logwarn('Currently ignoring the arm parameter.')
     #        for i,p in enumerate(self.r_arm_pub_l):
     #            p.publish(q[i])
     jtg = JointTrajectoryGoal()
     jtg.trajectory.joint_names = self.joint_names_list
     jtp = JointTrajectoryPoint()
     jtp.positions = q
     jtp.velocities = [0 for i in range(len(q))]
     jtp.accelerations = [0 for i in range(len(q))]
     jtp.time_from_start = rospy.Duration(duration)
     jtg.trajectory.points.append(jtp)
     self.joint_action_client.send_goal(jtg)
Ejemplo n.º 41
0
 def position_callback(self, msg):
     action_goal = FollowJointTrajectoryActionGoal()
     action_goal.goal_id.stamp = rospy.get_rostime()
     action_goal.goal.trajectory.joint_names = self._joint_names
     target_point = JointTrajectoryPoint()
     if self._dof == len(msg.positions):
         target_point.positions = msg.positions
     else:
         target_point.positions = []
         for position in msg.positions:
             target_point.positions.append(position)
         for i in range(self._dof - len(msg.positions)):
             target_point.positions.append(0.0)
     target_point.velocities = [self._velocity] * self._dof
     target_point.accelerations = [self._acceleration] * self._dof
     action_goal.goal.trajectory.points.append(target_point)
     self._pub.publish(action_goal)
Ejemplo n.º 42
0
def scale_trajectory_speed(traj, scale):
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
       
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj
 def createGoalWithValueAndPublish(self, value):
     jt = JointTrajectory()
     jt.joint_names = self.joint_names
     jtp = JointTrajectoryPoint()
     # Get current values and modify the one we want
     names, values = self.getNamesAndMsgListAndModifyValue(self.joint_names,
                                                           self.joint_name,
                                                           value)
     
     jtp.positions = values
     jtp.velocities = [0.0] * len(self.joint_names)
     jtp.accelerations = [0.0] * len(self.joint_names)
     # TODO: Make duration dependent on how big is the movement
     scaled_time = abs(value) * 1.5 + 0.2
     jtp.time_from_start = rospy.Duration(scaled_time) 
     jt.points.append(jtp)
     self.pub.publish(jt)
     rospy.sleep(1.0)
    def receiverLoop(self):
        while True:
            try:
                input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) )
                os.remove("/dev/shm/right_arm_goal.p")
            except IOError:
                print "file not there..."
                rospy.sleep(0.1)
                continue
            print input_msg
            goal = FollowJointTrajectoryGoal()
            rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) )
            goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0)
            rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) )
            goal.goal_tolerance = input_msg.goal.goal_tolerance 
            rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) )
            goal.path_tolerance = input_msg.goal.path_tolerance
            rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) )
            goal.trajectory.header = input_msg.goal.trajectory.header
            goal.trajectory.header.frame_id = '/base_link'
            goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names
            
            input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) )
            rospy.loginfo("input_times is:\n" + str(input_times))
            
            for input_point, time in zip(input_msg.goal.trajectory.points, input_times):
                p = JointTrajectoryPoint()
                p.accelerations = input_point.accelerations
                p.positions = input_point.positions
                p.velocities = input_point.velocities
#                 rospy.loginfo("???????????????????????????????????????????????????????????????")
#                 rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start))
#                 rospy.loginfo("time is:\n" + str(time))
#                 rospy.loginfo("???????????????????????????????????????????????????????????????")
                p.time_from_start = time
                goal.trajectory.points.append(p)
            
            os.remove("/dev/shm/right_arm_goal_times.p")
            #print "Goal will be:"
            rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal))
            self.right_arm_as.send_goal(goal)
            rospy.loginfo("Waiting for result")
            self.right_arm_as.wait_for_result()
            rospy.loginfo("Goal done")
Ejemplo n.º 45
0
    def move_to(self, positions, velocities, accelerations, delta_t):
        assert(len(self.joint_names) == positions.shape[1])
        trajectory = JointTrajectory()
        trajectory.joint_names = self.joint_names

        for i in range(positions.shape[0]): # for each row i
            p = JointTrajectoryPoint()
            p.positions = list(positions[i])
            p.velocities = list(velocities[i])
            p.accelerations = list(accelerations[i])
            p.time_from_start = rospy.Duration(i * delta_t)
            trajectory.points.append(p)


        follow_goal = FollowJointTrajectoryGoal()
        follow_goal.trajectory = trajectory

        self.client.send_goal(follow_goal)
        self.client.wait_for_result()
Ejemplo n.º 46
0
    def stop(self, include_velocity=False, include_effort=False):
        """
        Stop the recording and returns the recorded trajectory of joints declared in the constructor
        :param include_velocity:  if True, the joint velocities will be included in the returned trajectory
        :param include_effort:  if True, the joint efforts will be included in the returned trajectory
        :return: (joint_traj, eef_traj) resp. the recorded joint trajectory (RobotTrajectory) and end effector trajectory in frame 'base' (Path)
        """
        with self._recording_lock:
            self._recording = False

        # Now we reconstruct a RobotTrajectory...
        rt = RobotTrajectory()
        rt.joint_trajectory.joint_names = self._joints

        with self._joint_recorder_lock:
            for idx, js in enumerate(self._joint_recorded_points):
                jtp = JointTrajectoryPoint()
                try:
                    jtp.positions = [js.position[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
                except ValueError as e:
                    # This specific JS sample does not contain the desired joints, skip it
                    continue
                else:
                    if include_velocity:
                        jtp.velocities = [js.velocity[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
                    if include_effort:
                        jtp.accelerations = [js.effort[js.name.index(joint)] for joint in rt.joint_trajectory.joint_names]
                    jtp.time_from_start = self._joint_recorded_times[idx] - self._joint_recorded_times[0]
                    rt.joint_trajectory.points.append(jtp)

        # ... as well as an End Effector trajectory
        eft = Path()
        with self._eef_recorder_lock:
            for eef in self._eef_recorded_points:
                ps = PoseStamped()
                ps.header = eef.header
                ps.header.frame_id = 'base'
                ps.pose = eef.pose
                eft.poses.append(ps)
                # twist and wrench in eef are discarded
        eft.header.frame_id = 'base'

        return rt, eft
Ejemplo n.º 47
0
def track_trajectory( pos, vel, acc, time, 
                      arm = 'r', client = None,
                      stamp = None):

    if (acc == None):
        acc = np.zeros( pos.shape )

    if ( len( pos ) != len( vel ) or len( pos ) != len( time ) ):
        rospy.logerr( '[track_trajectory] dimensions do not agree' )

    rospy.loginfo( 'arm \'%s\' tracking trajectory with %d points'
                   %( arm, len( pos ) ) )    

    if (client == None):
        client = init_jt_client(arm)
    else:
        arm = client.action_client.ns[0:1]; # ignore arm argument

    joint_names = get_joint_names( ''.join( ( arm, '_arm_controller' ) ) )

    # create trajectory message
    jt = JointTrajectory()
    jt.joint_names = joint_names
    jt.points = []

    t = 0
    for i in range( len( pos ) ):
        jp = JointTrajectoryPoint()
        jp.time_from_start = rospy.Time.from_sec( time[i] )
        jp.positions = pos[i,:]
        jp.velocities = vel[i,:]
        jp.accelerations = acc[i,:]
        jt.points.append( jp )
        t += 1

    if ( stamp == None ):
        stamp = rospy.Time.now()

    # push trajectory goal to ActionServer
    jt_goal = JointTrajectoryGoal()
    jt_goal.trajectory = jt
    jt_goal.trajectory.header.stamp = stamp;
    client.send_goal(jt_goal)
def spam_controller(controller_name, joint_names):
    cmd_topic = "/" + controller_name + "/command"
    rospy.loginfo("Going to spam controller: " + controller_name + 
                  "\n in command topic: " + cmd_topic + 
                  "\n with joints: " + str(joint_names) )
    ctl_pub = rospy.Publisher(cmd_topic, JointTrajectory)
    rospy.sleep(0.1)
    while not rospy.is_shutdown():
        jt = JointTrajectory()
        jt.joint_names = joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = get_valid_random_values(joint_names, joints_dict)
        jtp.velocities = [0.0] * len(joint_names)
        jtp.accelerations = [0.0] * len(joint_names)
        jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
        jt.points.append(jtp)
        ctl_pub.publish(jt)
        rospy.logdebug("Sent to: " + cmd_topic + "\n" + str(jt))
        rospy.sleep(jtp.time_from_start)
Ejemplo n.º 49
0
 def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
     pnt = JointTrajectoryPoint()
     pnt.time_from_start = rospy.Duration(cmd_time)
     num_joints = b_matrix.shape[0]
     pnt.positions = [0.0] * num_joints
     if dimensions_dict["velocities"]:
         pnt.velocities = [0.0] * num_joints
     if dimensions_dict["accelerations"]:
         pnt.accelerations = [0.0] * num_joints
     for jnt in range(num_joints):
         b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
         # Positions at specified time
         pnt.positions[jnt] = b_point[0]
         # Velocities at specified time
         if dimensions_dict["velocities"]:
             pnt.velocities[jnt] = b_point[1]
         # Accelerations at specified time
         if dimensions_dict["accelerations"]:
             pnt.accelerations[jnt] = b_point[-1]
     return pnt
    def _get_point(self, joint_names, time):
        pnt = JointTrajectoryPoint()
        pnt.time_from_start = rospy.Duration(time)
        pnt.positions = [0.0] * len(joint_names)
        pnt.velocities = [0.0] * len(joint_names)
        pnt.accelerations = [0.0] * len(joint_names)
        time2 = math.pow(time, 2)
        time3 = math.pow(time, 3)
        time4 = math.pow(time, 4)
        time5 = math.pow(time, 5)

        for jnt in xrange(len(joint_names)):
            # Positions at specified time
            pnt.positions[jnt] = (self._coeff[jnt][0]
                                  + time * self._coeff[jnt][1]
                                  + time2 * self._coeff[jnt][2]
                                  + time3 * self._coeff[jnt][3]
                                  + time4 * self._coeff[jnt][4]
                                  + time5 * self._coeff[jnt][5]
                                  )

            # Velocities at specified time
            pnt.velocities[jnt] = (self._coeff[jnt][1]
                                   + 2.0 * time * self._coeff[jnt][2]
                                   + (3.0 * time2
                                      * self._coeff[jnt][3])
                                   + (4.0 * time3
                                      * self._coeff[jnt][4])
                                   + (5.0 * time4
                                      * self._coeff[jnt][5])
                                   )

            # Accelerations at specified time
            pnt.accelerations[jnt] = (2.0 * self._coeff[jnt][2]
                                      + 6.0 * time * self._coeff[jnt][3]
                                      + (12.0 * time2
                                         * self._coeff[jnt][4])
                                      + (20.0 * time3
                                         * self._coeff[jnt][5])
                                      )
        return pnt
 def receiverLoop(self):
     while True:
         print "self.conn.recv()"
         input_msg = self.conn.recv()
         print input_msg
         goal = FollowJointTrajectoryGoal()
         goal.goal_time_tolerance = input_msg.goal_time_tolerance
         goal.goal_tolerance = input_msg.goal_tolerance 
         goal.path_tolerance = input_msg.path_tolerance
         goal.trajectory.header = input_msg.trajectory.header
         goal.trajectory.joint_names = input_msg.joint_names
         for input_point in input_msg.trajectory.points:
             p = JointTrajectoryPoint()
             p.accelerations = input_point.accelerations
             p.positions = input_point.positions
             p.velocities = input_point.velocities
             p.time_from_start = input_point.time_from_start
             goal.trajectory.points.append(p)
         print "Goal will be:"
         print goal
         #self.right_arm_as.send_goal(goal)
         rospy.loginfo("Waiting for result")
         self.right_arm_as.wait_for_result()
         rospy.loginfo("Goal done")
Ejemplo n.º 52
0
                   pos = pos.split(", ")
                   for i in range(0, len(pos)):
                        pos[i] = float(pos[i])
                   y.positions = pos
                   #get vel
                   vel = el[counter + 2].split(": ")[1][1:-1]
                   vel = vel.split(", ")
                   for i in range(0, len(vel)):
                        vel[i] = float(vel[i])
                   y.velocities = vel
                   #get acc
                   acc = el[counter + 3].split(": ")[1][1:-1]
                   acc = acc.split(", ")
                   for i in range(0, len(acc)):
                        acc[i] = float(acc[i])
                   y.accelerations = acc
                   #get time
                   st = el[counter + 6].split(": ")[1]
                   et = el[counter + 7].split(": ")[1]
                   y.time_from_start.secs = float(st)
                   y.time_from_start.nsecs = float(et)

                   x.joint_trajectory.points.append(y)    

                   counter = counter + 8
    
         acHan.execute_plan(x)

        
t.close()
Ejemplo n.º 53
0
    def _on_trajectory_action(self, goal):
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        # Load parameters for trajectory
        self._get_trajectory_parameters(joint_names, goal)
        # Create a new discretized joint trajectory
        num_points = len(trajectory_points)
        if num_points == 0:
            rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
            self._server.set_aborted()
            return
        rospy.loginfo("%s: Executing requested joint trajectory" % (self._action_name,))
        rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
        control_rate = rospy.Rate(self._control_rate)

        dimensions_dict = self._determine_dimensions(trajectory_points)

        if num_points == 1:
            # Add current position as trajectory point
            first_trajectory_point = JointTrajectoryPoint()
            first_trajectory_point.positions = self._get_current_position(joint_names)
            # To preserve desired velocities and accelerations, copy them to the first
            # trajectory point if the trajectory is only 1 point.
            if dimensions_dict["velocities"]:
                first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
            if dimensions_dict["accelerations"]:
                first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
            first_trajectory_point.time_from_start = rospy.Duration(0)
            trajectory_points.insert(0, first_trajectory_point)
            num_points = len(trajectory_points)

        # Force Velocites/Accelerations to zero at the final timestep
        # if they exist in the trajectory
        # Remove this behavior if you are stringing together trajectories,
        # and want continuous, non-zero velocities/accelerations between
        # trajectories
        if dimensions_dict["velocities"]:
            trajectory_points[-1].velocities = [0.0] * len(joint_names)
        if dimensions_dict["accelerations"]:
            trajectory_points[-1].accelerations = [0.0] * len(joint_names)

        # Compute Full Bezier Curve Coefficients for all 7 joints
        pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
        try:
            b_matrix = self._compute_bezier_coeff(joint_names, trajectory_points, dimensions_dict)
        except Exception as ex:
            rospy.logerr(
                ("{0}: Failed to compute a Bezier trajectory for {1}" ' arm with error "{2}: {3}"').format(
                    self._action_name, self._name, type(ex).__name__, ex
                )
            )
            self._server.set_aborted()
            return
        # Wait for the specified execution time, if not provided use now
        start_time = goal.trajectory.header.stamp.to_sec()
        if start_time == 0.0:
            start_time = rospy.get_time()
        baxter_dataflow.wait_for(lambda: rospy.get_time() >= start_time, timeout=float("inf"))
        # Loop until end of trajectory time.  Provide a single time step
        # of the control rate past the end to ensure we get to the end.
        # Keep track of current indices for spline segment generation
        now_from_start = rospy.get_time() - start_time
        end_time = trajectory_points[-1].time_from_start.to_sec()
        while now_from_start < end_time and not rospy.is_shutdown():
            # Acquire Mutex
            now = rospy.get_time()
            now_from_start = now - start_time
            idx = bisect.bisect(pnt_times, now_from_start)
            # Calculate percentage of time passed in this interval
            if idx >= num_points:
                cmd_time = now_from_start - pnt_times[-1]
                t = 1.0
            elif idx >= 0:
                cmd_time = now_from_start - pnt_times[idx - 1]
                t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
            else:
                cmd_time = 0
                t = 0

            point = self._get_bezier_point(b_matrix, idx, t, cmd_time, dimensions_dict)

            # Command Joint Position, Velocity, Acceleration
            command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
            self._update_feedback(deepcopy(point), joint_names, now_from_start)
            # Release the Mutex
            if not command_executed:
                return
            control_rate.sleep()
        # Keep trying to meet goal until goal_time constraint expired
        last = trajectory_points[-1]
        last_time = trajectory_points[-1].time_from_start.to_sec()
        end_angles = dict(zip(joint_names, last.positions))

        def check_goal_state():
            for error in self._get_current_error(joint_names, last.positions):
                if self._goal_error[error[0]] > 0 and self._goal_error[error[0]] < math.fabs(error[1]):
                    return error[0]
            if (
                self._stopped_velocity > 0.0
                and max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)])
                > self._stopped_velocity
            ):
                return False
            else:
                return True

        while now_from_start < (last_time + self._goal_time) and not rospy.is_shutdown():
            if not self._command_joints(joint_names, last, start_time, dimensions_dict):
                return
            now_from_start = rospy.get_time() - start_time
            self._update_feedback(deepcopy(last), joint_names, now_from_start)
            control_rate.sleep()

        now_from_start = rospy.get_time() - start_time
        self._update_feedback(deepcopy(last), joint_names, now_from_start)

        # Verify goal constraint
        result = check_goal_state()
        if result is True:
            rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" % (self._action_name, self._name))
            self._result.error_code = self._result.SUCCESSFUL
            self._server.set_succeeded(self._result)
        elif result is False:
            rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" % (self._action_name, self._name))
            self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
            self._server.set_aborted(self._result)
        else:
            rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" % (self._action_name, result, self._name))
            self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
            self._server.set_aborted(self._result)
        self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
 def spin(self):
     
     r = rospy.Rate(1.0/self.dt)
     
     while not rospy.is_shutdown():
         
         if self.req_pos is None:
             
             r.sleep()
             continue
         
         self.lock.acquire()
         req_vel = copy.deepcopy(self.joint_req_vel)
         last_vel_command  = copy.deepcopy(self.last_vel_command)
         act_position = copy.deepcopy(self.act_position)
         #req_pos = copy.deepcopy(self.req_pos)
         
         
         now = rospy.Time.now()
         
         if last_vel_command is None:
             
             self.lock.release()
             r.sleep()
             continue
             
         
         if (now - last_vel_command) > rospy.Duration(0.5):
             
             print "final positions"
             print act_position.positions
             self.last_vel_command = None
             
             self.lock.release()
             r.sleep()
             continue
         
         
         jt = JointTrajectory()
         jt.joint_names = self.joint_names
     
         jtp = JointTrajectoryPoint()
         
    
     
         jtp.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         jtp.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
         jtp.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     
         #tl = list(self.req_pos.positions)
     
         for idx in range(0,len(req_vel)):
             
             self.req_pos[idx] = self.req_pos[idx] + (req_vel[idx]*self.dt)
     
             jtp.positions[idx] = self.req_pos[idx]
             jtp.velocities[idx] = req_vel[idx]
             #jtp.accelerations[idx] = 150.0
         
         
             jt.header.stamp = now
             jt.points.append(jtp)
             
         self.lock.release()
     
         self.pub.publish(jt)
     
     
         r.sleep()
Ejemplo n.º 55
0
cur_names, cur_pos = zip(*cur_state) # unzip
end_pos = []

for p in cur_pos:
  end_pos.append(p + 0.2)

traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(2.0)
traj_msg.joint_names = cur_names
traj_msg.points = []

orig_point = JointTrajectoryPoint()
orig_point.positions = cur_pos
orig_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
orig_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
orig_point.time_from_start = rospy.Duration.from_sec(0.0)

traj_msg.points.append(orig_point)
end_point = JointTrajectoryPoint()
end_point.positions = end_pos
end_point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
end_point.accelerations = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
end_point.time_from_start = rospy.Duration.from_sec(2.0)

traj_msg.points.append(end_point)
print traj_msg

traj_f = client.execute(traj_msg)
print traj_f.result()
                'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint']


CONTROLLER_CMD_TOPIC = "/left_arm_controller/command"

def get_n_randoms(n, min, max):
    randoms = []
    max_value = max - min
    for i in range(n):
        randoms.append((random.random() * max_value) + min) # this scales the value between min and max
    return randoms

if __name__ == '__main__':
    rospy.init_node('random_left_arm_goals')
    ctl_pub = rospy.Publisher(CONTROLLER_CMD_TOPIC, JointTrajectory)
    rospy.sleep(0.1)
    
    while not rospy.is_shutdown():
        jt = JointTrajectory()
        jt.joint_names = JOINT_NAMES
        jtp = JointTrajectoryPoint()
        jtp.positions = get_n_randoms(len(JOINT_NAMES), -1.0, 4.0)
        jtp.velocities = [0.0] * len(JOINT_NAMES)
        jtp.accelerations = [0.0] * len(JOINT_NAMES)
        jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
        jt.points.append(jtp)
        ctl_pub.publish(jt)
        rospy.loginfo("Sent: " + str(jt))
        rospy.sleep(jtp.time_from_start)

Ejemplo n.º 57
0
def getPlans(f, acHan):

    file_plans = []

    plans = f.read().split("PLAN : ")
    plans = plans[1:]

    for p in plans:
        name_plan = p.split(' >\n')
        #print(name_plan[0])

        el = name_plan[1].split('\n')    

        if (len(el) < 5):
            file_plans.append(el[0])
        else:
            x = RobotTrajectory()
   
            el = p.split('\n')
            #get frame id
            fram_id = el[7].split(": ")[1]
            x.joint_trajectory.header.frame_id = fram_id
            #get joint names
            joints = el[8].split(": ")[1][1:-1]
            joints = joints.split(", ")
            for j in range(0, len(joints)):
                joints[j] = joints[j].split('\'')[1]
            x.joint_trajectory.joint_names = joints

            #get points
            done = False
            counter = 10
            while(not done):
                if ("multi" in el[counter]):
                    done = True
                else:
                    y = JointTrajectoryPoint()

                    #get pos
                    pos = el[counter + 1].split(": ")[1][1:-1]
                    pos = pos.split(", ")
                    for i in range(0, len(pos)):
                        pos[i] = float(pos[i])
                    y.positions = pos
                    #get vel
                    vel = el[counter + 2].split(": ")[1][1:-1]
                    vel = vel.split(", ")
                    for i in range(0, len(vel)):
                        vel[i] = float(vel[i])
                    y.velocities = vel
                    #get acc
                    acc = el[counter + 3].split(": ")[1][1:-1]
                    acc = acc.split(", ")
                    for i in range(0, len(acc)):
                        acc[i] = float(acc[i])
                    y.accelerations = acc
                    #get time
                    st = el[counter + 6].split(": ")[1]
                    et = el[counter + 7].split(": ")[1]
                    y.time_from_start.secs = float(st)
                    y.time_from_start.nsecs = float(et)

                    x.joint_trajectory.points.append(y)    

                    counter = counter + 8
            
            file_plans.append(x)    
    
    return file_plans
def makePoint(position):
    mypoint = JointTrajectoryPoint()    
    mypoint.positions = position
    mypoint.velocities = [ 0, 0, 0, 0, 0, 0, 0 ]
    mypoint.accelerations = [ 0, 0, 0, 0, 0, 0, 0 ]
    return mypoint