Esempio n. 1
0
    def hover_the_quad(self, data): 
        """give commands to hover at 1m above the starting position"""
        a = time.time()
        if self.hover_counter == 0: 
            self.hover_start_time = data.header.stamp.to_sec()
        self.RHP_time = data.header.stamp.to_sec()-self.hover_start_time


        waypoint_specified = True
        waypoint_bc = False
        path = [[self.initial_position[0], self.initial_position[1], self.initial_position[2]], [self.initial_position[0], self.initial_position[1], self.hovering_height]]
        path = zip(*path)
        vel = np.array([[0,0,0]]); acc = np.array([[0,0,0]]); jerk = np.array([[0,0,0]])
        velocity = 1
        T, p1, p2, p3 = Minimum_snap_trajetory(velocity, path, waypoint_specified, waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()

        msg = PolynomialCoefficients()
        header = std_msgs.msg.Header()
        header.stamp =  data.header.stamp#rospy.Time.now()
        header.frame_id = 'world'
        msg.header = header
        msg.polynomial_order = 7
        for i in range(len(p1)): 
            msg.poly_x.append(p1[i]); msg.poly_y.append(p2[i]); msg.poly_z.append(p3[i])
        msg.number_of_segments = 1
        msg.planning_start_time =  self.hover_start_time
        for j in T: 
            msg.segment_end_times.append(j)
        msg.desired_direction.x = 1; msg.desired_direction.y = 0; msg.desired_direction.z = 0
        self.traj_polycoeff.publish(msg)
        f1 = open('total_time_taken.txt', 'a')
        f1.write("%s,\n" % (time.time()-a))
        self.hover_counter += 1
        """
    def hover_up(self, data):
        """give commands to hover at 1m above the starting position"""

        if self.hover_counter == 0:
            self.hover_start_time = data.header.stamp.to_sec()
#print 'i m here--', self.hover_start_time
        self.trajectory_time = data.header.stamp.to_sec(
        ) - self.hover_start_time
        #print "hover start time and trajectory time is", self.hover_start_time, self.trajectory_time

        if self.hover_counter == 0:
            waypoint_specified = True
            waypoint_bc = False
            path = [[
                self.initial_position[0], self.initial_position[1],
                self.initial_position[2]
            ],
                    [
                        self.initial_position[0], self.initial_position[1],
                        self.hovering_height
                    ]]
            path = zip(*path)
            vel = np.array([[0, 0, 0]])
            acc = np.array([[0, 0, 0]])
            jerk = np.array([[0, 0, 0]])
            planning_counter = -1
            velocity = self.velocity_during_hover_land
            T, p1, p2, p3 = Minimum_snap_trajetory(
                planning_counter, velocity, path, waypoint_specified,
                waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp
            header.frame_id = 'world'
            msg.header = header
            msg.polynomial_order = self.N
            for i in range(len(p1)):
                msg.poly_x.append(p1[i])
                msg.poly_y.append(p2[i])
                msg.poly_z.append(p3[i])
            msg.number_of_segments = 1
            msg.planning_start_time = str(self.hover_start_time)
            for j in T:
                msg.segment_end_times.append(j)
            msg.desired_direction.x = self.desired_direction[0]
            msg.desired_direction.y = self.desired_direction[1]
            msg.desired_direction.z = self.desired_direction[2]
            msg.trajectory_mode = 'hover'
            self.hover_traj_msg = msg

#print self.hover_traj_msg
        self.traj_polycoeff.publish(self.hover_traj_msg)
        self.hover_counter += 1
    def generate_trajectory_from_waypoints(self, data):
        """generates trajecory from the way points"""
        self.trajectory_time = data.header.stamp.to_sec(
        ) - self.hover_start_time

        if self.waypoint_counter == 0 and self.reached_goal == False:
            # -------change this segment in order to construct  a new trajectory---------------
            start = self.Pglobal
            end = self.end_point
            no_of_segments = 8
            path = []
            for j in range(no_of_segments + 1):
                path.append(list(start + j * (end - start) / no_of_segments))
            path = zip(*path)
            # -------change this segment in order to construct  a new trajectory---------------

            waypoint_specified = True
            waypoint_bc = False
            velocity = self.uav_velocity
            planning_counter = 0
            vel = np.array([[0, 0, 0]])
            acc = np.array([[0, 0, 0]])
            jerk = np.array([[0, 0, 0]])
            T, _p1, _p2, _p3 = Minimum_snap_trajetory(
                planning_counter, velocity, path, waypoint_specified,
                waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp  #rospy.Time.now()
            header.frame_id = 'world'
            msg.header = header
            msg.polynomial_order = self.N
            for i in range(len(_p1)):
                msg.poly_x.append(_p1[i])
                msg.poly_y.append(_p2[i])
                msg.poly_z.append(_p3[i])
            msg.number_of_segments = no_of_segments
            msg.planning_start_time = str(data.header.stamp.to_sec())
            for j in T[:no_of_segments + 1]:
                msg.segment_end_times.append(j)
            msg.desired_direction.x = self.desired_direction[0]
            msg.desired_direction.y = self.desired_direction[1]
            msg.desired_direction.z = self.desired_direction[2]
            msg.trajectory_mode = 'planning_from_waypoints'
            self.traj_msg = msg
            self.trajectory_end_in_global_time = self.trajectory_time + T[-1]

        self.traj_polycoeff.publish(self.traj_msg)
        # check if the goal is reached
        if self.trajectory_time >= self.trajectory_end_in_global_time + 5.0:  # 5 sec is the time for which the quad will hover before coming down
            self.reached_goal = True

        self.waypoint_counter += 1
    def hover_down(self, data):
        """give commands to hover at 1m above the starting position"""
        a = time.time()
        if self.land_counter == 0:
            self.land_start_time = data.header.stamp.to_sec()
            self.land_start = [
                self.Pglobal[0], self.Pglobal[1], self.Pglobal[2]
            ]

        #self.RHP_time = data.header.stamp.to_sec()-self.land_start_time

        waypoint_specified = True
        waypoint_bc = False
        path = [[self.land_start[0], self.land_start[1], self.land_start[2]],
                [self.land_start[0], self.land_start[1], 0.1]]
        path = zip(*path)
        vel = np.array([[0, 0, 0]])
        acc = np.array([[0, 0, 0]])
        jerk = np.array([[0, 0, 0]])
        planning_counter = -1
        velocity = 0.5
        T, p1, p2, p3 = Minimum_snap_trajetory(
            planning_counter, velocity, path, waypoint_specified, waypoint_bc,
            vel, acc, jerk).construct_polynomial_trajectory()

        #print 'time from start is:', self.RHP_time
        msg = PolynomialCoefficients()
        header = std_msgs.msg.Header()
        header.stamp = data.header.stamp  #rospy.Time.now()
        header.frame_id = 'world'
        msg.header = header
        msg.polynomial_order = 7
        for i in range(len(p1)):
            msg.poly_x.append(p1[i])
            msg.poly_y.append(p2[i])
            msg.poly_z.append(p3[i])
        msg.number_of_segments = 1
        msg.planning_start_time = self.land_start_time
        for j in T:
            msg.segment_end_times.append(j)
        msg.desired_direction.x = self.initial_orientation[0]
        msg.desired_direction.y = self.initial_orientation[1]
        msg.desired_direction.z = self.initial_orientation[2]
        #msg.desired_direction.x = 0; msg.desired_direction.y = -1; msg.desired_direction.z = 0
        #msg.desired_direction.x = 1; msg.desired_direction.y = 0; msg.desired_direction.z = 0
        msg.trajectory_mode = 'land'
        self.traj_polycoeff.publish(msg)
        #self.initial_dist_to_goal = np.linalg.norm(self.end_point-self.Pglobal)
        f1 = open('total_time_taken.txt', 'a')
        f1.write("%s,\n" % (time.time() - a))
        self.land_counter += 1
    def land(self, data):
        """generate trajectory for landing"""

        if self.land_counter == 0:
            self.land_start_time = data.header.stamp.to_sec()
            self.land_start = [
                self.Pglobal[0], self.Pglobal[1], self.Pglobal[2]
            ]

        if self.land_counter == 0:
            waypoint_specified = True
            waypoint_bc = False
            path = [[
                self.land_start[0], self.land_start[1], self.land_start[2]
            ], [self.land_start[0], self.land_start[1], 0.12]]
            path = zip(*path)
            vel = np.array([[0, 0, 0]])
            acc = np.array([[0, 0, 0]])
            jerk = np.array([[0, 0, 0]])
            planning_counter = -1
            velocity = self.velocity_during_hover_land
            T, p1, p2, p3 = Minimum_snap_trajetory(
                planning_counter, velocity, path, waypoint_specified,
                waypoint_bc, vel, acc, jerk).construct_polynomial_trajectory()

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp
            header.frame_id = 'world'
            msg.header = header
            msg.polynomial_order = self.N
            for i in range(len(p1)):
                msg.poly_x.append(p1[i])
                msg.poly_y.append(p2[i])
                msg.poly_z.append(p3[i])
            msg.number_of_segments = 1
            msg.planning_start_time = str(self.land_start_time)
            for j in T:
                msg.segment_end_times.append(j)

            msg.desired_direction.x = self.desired_direction[0]
            msg.desired_direction.y = self.desired_direction[1]
            msg.desired_direction.z = self.desired_direction[2]
            msg.trajectory_mode = 'land'
            self.land_traj_msg = msg

        self.traj_polycoeff.publish(self.land_traj_msg)
        self.land_counter += 1
Esempio n. 6
0
    def hover_up(self, data):
        """give commands to hover at 1m above the starting position"""

        if self.hover_counter == 0:
            self.hover_start_time = data.header.stamp.to_sec()
        self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time
        if self.hover_counter == 0:
            path = [[
                self.initial_position[0], self.initial_position[1],
                self.initial_position[2]
            ],
                    [
                        self.initial_position[0], self.initial_position[1],
                        self.hovering_height
                    ]]

            vel = np.array([0, 0, 0])
            acc = np.array([0, 0, 0])
            jerk = np.array([0, 0, 0])
            counter = 0
            velocity = self.vel_hl

            T, p1, p2, p3 = Minimum_jerk_trajetory(
                counter, velocity, path, vel, acc,
                jerk).construct_trajectory()

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp  #rospy.Time.now()
            header.frame_id = 'map'
            msg.header = header
            msg.polynomial_order = 7
            for i in range(len(p1)):
                msg.poly_x.append(p1[i])
                msg.poly_y.append(p2[i])
                msg.poly_z.append(p3[i])
            msg.number_of_segments = 1
            msg.planning_start_time = str(self.hover_start_time)
            for j in T:
                msg.segment_end_times.append(j)
            msg.desired_direction.x = self.initial_orientation[0]
            msg.desired_direction.y = self.initial_orientation[1]
            msg.desired_direction.z = self.initial_orientation[2]
            msg.execution_time_horizon = str(0)  # dummy value
            msg.trajectory_mode = 'hover'
            self.traj_polycoeff.publish(msg)

        self.hover_counter += 1
    def trajectory_in_camera_fov(self, data):
        """generates trajecory in camera workspace""" 
        r = 10
        self.end_point = np.array([r * np.sin((self.RHP_time-5)/10), -r * np.cos((self.RHP_time-5)/10),  1.5])
        #self.end_point = self.end_point + np.array([self.target_velocity*0.05, 0, 0])
        start = time.time()
        odom_msg = Odometry()
        odom_msg.pose.pose.position.x = self.end_point[0]
        odom_msg.pose.pose.position.y = self.end_point[1]
        odom_msg.pose.pose.position.z = self.end_point[2]
        odom_msg.header.stamp = rospy.Time.now()
        odom_msg.header.frame_id = 'world'

        self.target_odom.publish(odom_msg)
        t1 = time.time()-start
        
        start_point = np.zeros((0,3))
        if self.traj_gen_counter != 0: 
            start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe
        else: 
            start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity

        #start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
        points_in_cone = self.generate_regular_pyramid_grid() 
        occupied_points = ros_numpy.numpify(data)

        if len(occupied_points) != 0: 
            points_in_cone = self.generate_final_grid(points_in_cone, occupied_points)


        p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
        
 
        
        p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1]
        #p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], x) for x in p1]

        points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2]
        points_in_cone = [x for x in points_in_cone if not x[2] <= 0]

        #kdt_points_in_cone = cKDTree(points_in_cone)
        
        #if len(occupied_points) == 0: 
        #print start_point, len(points_in_cone), len(occupied_points)
        points_in_cone = np.concatenate((start_point, points_in_cone), 0)
        
        # publish generated pointcloud
        header = std_msgs.msg.Header()
        header.stamp = data.header.stamp#rospy.Time.now()
        header.frame_id = 'world'
        p = pc2.create_cloud_xyz32(header, points_in_cone)
        self.pcl_pub.publish(p)         
      
        t3 = time.time()-start
        kdt_points_in_cone = cKDTree(points_in_cone)
        closest_to_end = kdt_points_in_cone.query(self.end_point, 1)

        #closest_to_end_index = points_in_cone[closest_to_end[1]]
        #closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2]) 
        end_point_index = closest_to_end[1]

        planning_horizon = 4 # planning horizon
        execution_horizon = 1 # execution horizon
        
        dist_to_goal = np.linalg.norm(self.end_point-self.Pglobal)
        ttt = self.RHP_time-5.0
        timefactor = 1/(1+np.exp(-1*(ttt)))
        distancefactor = erf(0.4*(dist_to_goal-self.resolution))
        smoothing_factor = timefactor * distancefactor

        if self.traj_gen_counter == 0 or self.RHP_time > self.tracking_in_global_time-0.06:
            
            #one dimension simplicial complex which is basically a graph
            rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution)
            f = rips.create_simplex_tree(max_dimension = 1)
            
            # make graph
            G = nx.Graph() 
            G.add_nodes_from(range(f.num_vertices()))
            edge_list = [(simplex[0][0], simplex[0][1],  {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
            edge_list = [k for k in edge_list if k is not None]                   
            G.add_edges_from(edge_list)    
            
            try: 
                shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra')
                path = np.array([points_in_cone[j] for j in shortest_path])

            

                t4 = time.time()-start
                #dist_to_goal = np.linalg.norm(self.end_point-path[-1]) # works with the velocity profile

                if dist_to_goal < self.resolution: 
                    self.reached_goal = True
    

                
                # define replanning strategy
                
                if self.reached_goal == False: 
                    if len(path)-1 >= planning_horizon: 
                        no_of_segments = planning_horizon
                        no_of_segments_to_track = execution_horizon
                        path = path[:no_of_segments+1]
                    elif execution_horizon <= len(path)-1 < planning_horizon:
                        no_of_segments = len(path)-1
                        no_of_segments_to_track = execution_horizon
                        path = path[:no_of_segments+1]
                    elif len(path)-1 < execution_horizon:
                        no_of_segments = len(path)-1
                        no_of_segments_to_track = no_of_segments 
                        path = path[:no_of_segments+1]
                else: 
                    if self.static_goal == True: 
                        no_of_segments = len(path)-1
                        no_of_segments_to_track = no_of_segments
                        path = path[:no_of_segments+1]
                    else: 
                        if len(path)-1 >= planning_horizon: 
                            no_of_segments = planning_horizon
                            no_of_segments_to_track = execution_horizon
                            path = path[:no_of_segments+1]
                        elif execution_horizon <= len(path)-1 < planning_horizon:
                            no_of_segments = len(path)-1
                            no_of_segments_to_track = execution_horizon
                            path = path[:no_of_segments+1]
                        elif len(path)-1 < execution_horizon:
                            no_of_segments = len(path)-1
                            no_of_segments_to_track = no_of_segments
                            path = path[:no_of_segments+1]
                            
                #print '-----len of path is---', len(path)-1
                path = zip(*path)       
                #f0 = open('path.txt', 'a')
                #f0.write('%s\n' %(path))
                # now construct the minimum snap trajectory on the minimum path
                waypoint_specified = True
                waypoint_bc = False

                
                #erf1 = erf(1000*dist_to_goal)
                #erf2 = erf(-1000*(dist_to_goal-self.initial_dist_to_goal))
                
                velocity = self.uav_velocity*smoothing_factor
                #print '----------average velocity is--------', velocity 
                #f1 = open('velocity.txt', 'a')
                #f1.write('%s, %s, %s, %s, %s, %s\n' %(self.initial_dist_to_goal, dist_to_goal, ttt, timefactor, distancefactor, velocity))
                T, _p1, _p2, _p3 = Minimum_snap_trajetory(self.replanning_started, velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory()
                t5 = time.time()-start
                
               
                #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
                
        
                N = 8
                _pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
                [i.reverse() for i in _pp1]
            
                _pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
                [i.reverse() for i in _pp2]
            
                _pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
                [i.reverse() for i in _pp3]
                
        
                xx = np.poly1d(_pp1[no_of_segments_to_track-1])
                vx = np.polyder(xx, 1); accx = np.polyder(xx, 2)
                jx = np.polyder(xx, 3); sx = np.polyder(xx, 4)
        
                yy = np.poly1d(_pp2[no_of_segments_to_track-1])
                vy = np.polyder(yy, 1); accy = np.polyder(yy, 2)
                jy = np.polyder(yy, 3); sy = np.polyder(yy, 4)
                
                zz = np.poly1d(_pp3[no_of_segments_to_track-1])
                vz = np.polyder(zz, 1); accz = np.polyder(zz, 2)
                jz = np.polyder(zz, 3); sz = np.polyder(zz, 4)
                
                xdes = xx(T[no_of_segments_to_track]); ydes = yy(T[no_of_segments_to_track]); zdes = zz(T[no_of_segments_to_track])
                vxdes = vx(T[no_of_segments_to_track]); vydes = vy(T[no_of_segments_to_track]); vzdes = vz(T[no_of_segments_to_track])
                axdes = accx(T[no_of_segments_to_track]); aydes = accy(T[no_of_segments_to_track]); azdes = accz(T[no_of_segments_to_track])
                jxdes = jx(T[no_of_segments_to_track]); jydes = jy(T[no_of_segments_to_track]); jzdes = jz(T[no_of_segments_to_track])
    
                self.p_eoe = np.array([[xdes, ydes, zdes]])
                self.v_eoe = np.array([[vxdes, vydes, vzdes]])
                self.a_eoe = np.array([[axdes, aydes, azdes]])
                self.j_eoe = np.array([[jxdes, jydes, jzdes]])
        
                eoe_msg = Odometry()
                eoe_msg.pose.pose.position.x = self.p_eoe[0][0]
                eoe_msg.pose.pose.position.y = self.p_eoe[0][1]
                eoe_msg.pose.pose.position.z = self.p_eoe[0][2]
                eoe_msg.header.stamp = rospy.Time.now()
                eoe_msg.header.frame_id = 'world'
        
                self.eoe_odom.publish(eoe_msg)
                
                #f1 = open('eoe_points.txt', 'a')
                #f1.write('%s, %s\n' %(self.Pglobal, self.p_eoe))               
        
                vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
                #vector = np.array([self.end_point[0]-self.p_eoe[0][0], self.end_point[1]-self.p_eoe[0][1], self.end_point[2]-self.p_eoe[0][2]])
                #vector = self.v_eoe
                direction = vector/np.linalg.norm(vector)
                
                msg = PolynomialCoefficients()
                header = std_msgs.msg.Header()
                header.stamp = data.header.stamp#rospy.Time.now()
                header.frame_id = 'world'
                msg.header = header
                msg.polynomial_order = N-1
                for i in range(len(_p1)): 
                    msg.poly_x.append(_p1[i]); msg.poly_y.append(_p2[i]); msg.poly_z.append(_p3[i])
                msg.number_of_segments = no_of_segments_to_track
                msg.planning_start_time = data.header.stamp.to_sec()
                for j in T[:no_of_segments_to_track+1]: 
                    msg.segment_end_times.append(j)
                msg.desired_direction.x = direction[0]; msg.desired_direction.y = direction[1]; msg.desired_direction.z = direction[2]
                #msg.desired_direction.x = direction[0][0]; msg.desired_direction.y = direction[0][1]; msg.desired_direction.z = direction[0][2]
                msg.trajectory_mode = 'planning_in_camera_fov'
                self.previous_coefficients = msg
                self.traj_polycoeff.publish(msg)
                self.republish_trajectory = True

            except: 
                print 'No path between start and end'
                #vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
                #direction = vector/np.linalg.norm(vector)
                #self.previous_coefficients.desired_direction.x = direction[0]
                #self.previous_coefficients.desired_direction.y = direction[1]
                #self.previous_coefficients.desired_direction.z = direction[1]
                self.traj_polycoeff.publish(self.previous_coefficients)
                self.republish_trajectory = False
                
        else: #elif self.RHP_time <= self.tracking_in_global_time - 0.1:  
            self.traj_polycoeff.publish(self.previous_coefficients)
            self.republish_trajectory = False
        
        
        
        
        
        time_taken = time.time()-start
        #time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
        self.RHP_time = data.header.stamp.to_sec()-self.hover_start_time
        
        #print 'total time taken to execute the callbacak is:', time_taken

        f1 = open('total_time_taken.txt', 'a')
        f1.write("%s\n" % (time_taken)) 
        
        self.traj_gen_counter += 1
Esempio n. 8
0
    def trajectory_in_camera_fov(self, data):
        """generates trajecory in camera workspace""" 

        start = time.time()
        odom_msg = Odometry()
        odom_msg.pose.pose.position.x = self.end_point[0]
        odom_msg.pose.pose.position.y = self.end_point[1]
        odom_msg.pose.pose.position.z = self.end_point[2]
        odom_msg.header.stamp = rospy.Time.now()
        odom_msg.header.frame_id = 'world'

        self.target_odom.publish(odom_msg)
        t1 = time.time()-start
        
        start_point = np.zeros((0,3))
        if self.traj_gen_counter != 0: 
            start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe
        else: 
            start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity

        #start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
        points_in_cone = self.generate_regular_pyramid_grid() 
        occupied_points = ros_numpy.numpify(data)

        if len(occupied_points) != 0: 
            points_in_cone, weights = self.generate_final_grid(points_in_cone, start_point, occupied_points)
            #print weights
            weights = [200.0/i for i in weights] # 200 is just a scale by which weights are increased (after inverting them first)
            #print weights


        p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
        p2 = [-self.Rcg_vibase[0:3, 3:4] + np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1]

        points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2]
        points_in_cone = [x for x in points_in_cone if not x[2] <= 0]

        
        if len(occupied_points) == 0: 
            points_in_cone = np.concatenate((start_point, points_in_cone), 0)
        
        
        # publish generated pointcloud
        header = std_msgs.msg.Header()
        header.stamp = data.header.stamp#rospy.Time.now()
        header.frame_id = 'world'
        p = pc2.create_cloud_xyz32(header, points_in_cone)
        self.pcl_pub.publish(p)        
        t3 = time.time()-start
        kdt_points_in_cone = cKDTree(points_in_cone)
        closest_to_end = kdt_points_in_cone.query(self.end_point, 1)

        #closest_to_end_index = points_in_cone[closest_to_end[1]]
        #closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2]) 
        end_point_index = closest_to_end[1]


        #one dimension simplicial complex which is basically a graph
        rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution)
        f = rips.create_simplex_tree(max_dimension = 1)
        
        # make graph
        G = nx.Graph() 
        G.add_nodes_from(range(f.num_vertices()))
        if len(occupied_points) == 0:
            edge_list = [(simplex[0][0], simplex[0][1],  {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
        else: 

            edge_list = [(simplex[0][0], simplex[0][1],  {'weight': simplex[1] + (weights[simplex[0][0]]+weights[simplex[0][1]])/2}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
        edge_list = [k for k in edge_list if k is not None]                   
        G.add_edges_from(edge_list)    
        
        try: 
            shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra')
            length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
            #print 'length of the path is', length
            path = np.array([points_in_cone[j] for j in shortest_path])
        except: 
            print 'No path between start and end'
        
        #if self.traj_gen_counter == 0 : 
        #    for simplex in f.get_skeleton(1): 
        #        print simplex
        #length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
        
        # planning horizon is a fixed (5 for now) segments, trajectory will be planned only for these segments, 
        # execution horizon will be just 3 segments
        # at current resolution of 0.25m, that means a trajecotory of approximately 1.25m will be planned and 0.75m will be executed
        # at each time the quad plans its motion, depending on how fast it can replan, these values would be changed
        t4 = time.time()-start
        no_of_segments = 4
        no_of_segments_to_track = 1
        path = path[:no_of_segments+1] # n segments means n+1 points in the path
        #f1 = open('path.txt', 'a')
        #f1.write('%s\n' %(path)) 
        path = zip(*path)       

        # now construct the minimum snap trajectory on the minimum path
        waypoint_specified = True
        waypoint_bc = False

        T, _p1, _p2, _p3 = Minimum_snap_trajetory(self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory()
        t5 = time.time()-start
        
       
        #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
        

        N = 8
        _pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
        [i.reverse() for i in _pp1]
    
        _pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
        [i.reverse() for i in _pp2]
    
        _pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
        [i.reverse() for i in _pp3]
        

        xx = np.poly1d(_pp1[no_of_segments_to_track-1])
        vx = np.polyder(xx, 1); accx = np.polyder(xx, 2)
        jx = np.polyder(xx, 3); sx = np.polyder(xx, 4)

        yy = np.poly1d(_pp2[no_of_segments_to_track-1])
        vy = np.polyder(yy, 1); accy = np.polyder(yy, 2)
        jy = np.polyder(yy, 3); sy = np.polyder(yy, 4)
        
        zz = np.poly1d(_pp3[no_of_segments_to_track-1])
        vz = np.polyder(zz, 1); accz = np.polyder(zz, 2)
        jz = np.polyder(zz, 3); sz = np.polyder(zz, 4)
        
        xdes = xx(T[no_of_segments_to_track]); ydes = yy(T[no_of_segments_to_track]); zdes = zz(T[no_of_segments_to_track])
        vxdes = vx(T[no_of_segments_to_track]); vydes = vy(T[no_of_segments_to_track]); vzdes = vz(T[no_of_segments_to_track])
        axdes = accx(T[no_of_segments_to_track]); aydes = accy(T[no_of_segments_to_track]); azdes = accz(T[no_of_segments_to_track])
        jxdes = jx(T[no_of_segments_to_track]); jydes = jy(T[no_of_segments_to_track]); jzdes = jz(T[no_of_segments_to_track])
        """
        t = []; xx = []; yy = []; zz = []
        vx = []; vy = []; vz = []; accx = []; accy = []; accz = []
        jx = []; jy = []; jz = []

        traj_frequency = 100
        for ii in range(no_of_segments_to_track): 

            #t.append(np.linspace(T[ii], T[ii+1], int((T[ii+1]-T[ii])*traj_frequency))) 
            t.append(np.linspace(T[ii], T[ii+1], 101)) 
            xx.append(np.poly1d(p1[ii]))
            vx.append(np.polyder(xx[-1], 1)); accx.append(np.polyder(xx[-1], 2))
            jx.append(np.polyder(xx[-1], 3))#; sx.append(np.polyder(xx[-1], 4))
            
            yy.append(np.poly1d(p2[ii]))
            vy.append(np.polyder(yy[-1], 1)); accy.append(np.polyder(yy[-1], 2))
            jy.append(np.polyder(yy[-1], 3))#; sy.append(np.polyder(yy[-1], 4))
    
            zz.append(np.poly1d(p3[ii]))
            vz.append(np.polyder(zz[-1], 1)); accz.append(np.polyder(zz[-1], 2))
            jz.append(np.polyder(zz[-1], 3))#; sz.append(np.polyder(zz[-1], 4))
            
        t6 = time.time()-start
        traj = MultiDOFJointTrajectory()
        header = std_msgs.msg.Header()
        header.stamp = data.header.stamp#rospy.Time.now()
        header.frame_id = 'world'
        traj.header = header
        traj.joint_names = 'firefly/base_link' # testing for now
        
        
        trajectory_start_time = self.RHP_time
        for i in range(no_of_segments_to_track): # "changed to no_of_segments_to_track" instead of "no_of_segments" 
            for j in t[i]:
                    
                self.RHP_time = j + trajectory_start_time    
                xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
                vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
                axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
                jxdes = jx[i](j); jydes = jy[i](j); jzdes = jz[i](j)
                f4 = open('trajectory_generated.txt','a')
                f4.write("%s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n" % (self.RHP_time, xdes, ydes, zdes, vxdes, vydes, vzdes, axdes, aydes, azdes))
                    
                
                # for now angular acceleration Point msg of MultiDOFJointTrajectory() msg is  used to specify the desired direction 
                #vector = np.array([self.end_point[0]-xdes, self.end_point[1]-ydes, self.end_point[2]-zdes])
                #vector = np.array([self.p_eoe[0][0]-xdes, self.p_eoe[0][1]-ydes, self.p_eoe[0][2]-zdes])
                vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
                #vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
                direction = vector/np.linalg.norm(vector)
                
                transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
                velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
                accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
                #accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
                
                point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time))
                traj.points.append(point)
                self.uav_traj_pub.publish(traj)
                traj.points.pop(0)  
                #time.sleep(1.0/300)
        """
        self.p_eoe = np.array([[xdes, ydes, zdes]])
        self.v_eoe = np.array([[vxdes, vydes, vzdes]])
        self.a_eoe = np.array([[axdes, aydes, azdes]])
        self.j_eoe = np.array([[jxdes, jydes, jzdes]])
        #f1 = open('eoe_points.txt', 'a')
        #f1.write('%s, %s, %s, %s\n' %(self.p_eoe, self.v_eoe, self.a_eoe, self.j_eoe)) 
        

        #self.uav_traj_pub.publish(traj)
        
        self.traj_gen_counter += 1


        vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
        direction = vector/np.linalg.norm(vector)
        
        msg = PolynomialCoefficients()
        header = std_msgs.msg.Header()
        header.stamp = data.header.stamp#rospy.Time.now()
        header.frame_id = 'world'
        msg.header = header
        msg.polynomial_order = N-1
        for i in range(len(_p1)): 
            msg.poly_x.append(_p1[i]); msg.poly_y.append(_p2[i]); msg.poly_z.append(_p3[i])
        msg.number_of_segments = no_of_segments_to_track
        msg.planning_start_time = data.header.stamp.to_sec()
        for j in T[:no_of_segments_to_track+1]: 
            msg.segment_end_times.append(j)
        msg.desired_direction.x = direction[0]; msg.desired_direction.y = direction[1]; msg.desired_direction.z = direction[2]
        self.traj_polycoeff.publish(msg)
        
        
        time_taken = time.time()-start
        #time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
        self.RHP_time = data.header.stamp.to_sec()-self.hover_start_time
        
        print 'total time taken to execute the callbacak is:', time_taken

        f1 = open('total_time_taken.txt', 'a')
        f1.write("%s, %s, %s, %s, %s, %s, %s\n" % (T[no_of_segments_to_track], start, t1, t3, t4, t5, time_taken))  
Esempio n. 9
0
    def plan_in_camera_fov(self, pc_data):
        a = time.time()

        self.RHP_time = pc_data.header.stamp.to_sec() - self.hover_start_time
        distance_to_goal = np.linalg.norm(self.goal - self.Pglobal)
        if self.replanning_counter == 1:
            ps = self.Pglobal
            direction = self.initial_orientation
            self.replanning_start_time = time.time()
            currenttime = 0.1
        else:
            ps = self.Pglobal  #self.p_eoe
            vector = self.goal - self.Pglobal
            direction = vector / np.linalg.norm(vector)
            currenttime = time.time() - self.replanning_start_time

        if self.goal_moving == False and distance_to_goal > self.sensor_range:
            goal_vector = self.goal - ps
            self.lgoal = ps + goal_vector * self.sensor_range / np.linalg.norm(
                goal_vector)

            r = min(distance_to_goal, self.sensor_range)
            end_points = self.generate_regular_pyramid_grid2(r)
            p11 = np.dot(self.Rcdoc_cdl[0:3, 0:3], end_points.T).T
            p22 = self.Rvibase_cl[0:3, 3:4].T + p11
            p33 = self.Rcg_vibase[0:3, 3:4].T + np.dot(
                np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), p22.T).T
            end_points = ps[np.newaxis] + np.dot(self.Rglobal, p33.T).T

            occupied_points = ros_numpy.numpify(pc_data)

            if len(occupied_points) != 0:
                transformed_cloud = self.transform_cloud(ps, occupied_points)
                if self.replanning_counter == 100:
                    for i in transformed_cloud:
                        f3 = open(self.path + 'occ_points.txt', 'a')
                        f3.write('%s, %s\n' %
                                 (pc_data.header.stamp.to_sec(), i))
                if len(transformed_cloud) == 0:

                    final_points = end_points
                    Wcollision = [1] * len(final_points)
                    Wdist2goal = np.linalg.norm(self.lgoal - final_points,
                                                axis=1)
                    Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
                                     Wdist2goal)
                else:
                    final_points, Wdist2goal, Wcollision = self.check_collision_get_cost(
                        ps, end_points, transformed_cloud, self.lgoal)
                    Wcollision = map(lambda i: i * 1.0 / max(Wcollision),
                                     Wcollision)
                    Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
                                     Wdist2goal)
            else:
                final_points = end_points
                Wcollision = [1] * len(final_points)
                Wdist2goal = np.linalg.norm(self.lgoal - final_points, axis=1)
                Wdist2goal = map(lambda i: i * 1.0 / max(Wdist2goal),
                                 Wdist2goal)

            #print 'length of end points and occupied points' , len(Wdist2goal), len(Wcollision), len(final_points)
            Wtotal = map(lambda i, j: 3 * i + 1.0 / j, Wdist2goal, Wcollision)
            #print 'wtotal len', len(Wtotal)
            local_goal = final_points[np.argmin(Wtotal)]

            n = self.no_of_segments
            #self.N = 8
            path = zip(np.linspace(ps[0], local_goal[0], n + 1),
                       np.linspace(ps[1], local_goal[1], n + 1),
                       np.linspace(ps[2], local_goal[2], n + 1))

            vel, f1, f2 = self.get_velocity(currenttime, distance_to_goal)

            f = open(self.path + 'velocities.txt', 'a')
            f.write('%s, %s, %s, %s, %s\n' %
                    (currenttime, distance_to_goal, f1, f2, vel))

            T, _p1, _p2, _p3 = Minimum_jerk_trajetory(
                self.replanning_counter, vel, path, self.v_eoe, self.a_eoe,
                self.j_eoe).construct_trajectory()

            _pp1 = [_p1[i:i + self.N] for i in range(0, len(_p1), self.N)]
            [i.reverse() for i in _pp1]

            _pp2 = [_p2[i:i + self.N] for i in range(0, len(_p2), self.N)]
            [i.reverse() for i in _pp2]

            _pp3 = [_p3[i:i + self.N] for i in range(0, len(_p3), self.N)]
            [i.reverse() for i in _pp3]

            #t = map(lambda i: np.linspace(T[i], T[i+1], 201), range(1))
            tt = self.execution_time
            index = len(np.where(np.array(T) < tt)[0]) - 1

            xx = np.poly1d(_pp1[index])
            yy = np.poly1d(_pp2[index])
            zz = np.poly1d(_pp3[index])
            vx = np.polyder(xx, 1)
            vy = np.polyder(yy, 1)
            vz = np.polyder(zz, 1)
            accx = np.polyder(xx, 2)
            accy = np.polyder(yy, 2)
            accz = np.polyder(zz, 2)
            jx = np.polyder(xx, 3)
            jy = np.polyder(yy, 3)
            jz = np.polyder(zz, 3)
            #index = np.argmin(np.abs(np.array(t[0])-tt))

            self.p_eoe = np.array([xx(tt), yy(tt), zz(tt)])
            self.v_eoe = np.array([vx(tt), vy(tt), vz(tt)])
            self.a_eoe = np.array([accx(tt), accy(tt), accz(tt)])
            self.j_eoe = np.array([jx(tt), jy(tt), jz(tt)])

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = pc_data.header.stamp
            header.frame_id = 'map'
            msg.header = header
            msg.polynomial_order = self.N - 1
            for i in range(len(_p1)):
                msg.poly_x.append(_p1[i])
                msg.poly_y.append(_p2[i])
                msg.poly_z.append(_p3[i])
            msg.number_of_segments = n
            #msg.sensor_update_segment = N_sensor_update
            msg.planning_start_time = str(self.odom_time)
            for j in T:
                msg.segment_end_times.append(j)
            msg.desired_direction.x = direction[0]
            msg.desired_direction.y = direction[1]
            msg.desired_direction.z = direction[2]

            msg.trajectory_mode = 'planning_in_camera_fov'
            msg.execution_time_horizon = str(self.execution_time)
            self.previous_msg = msg

            self.traj_polycoeff.publish(msg)
            self.trajectory_endtime = T[-1]
            #print '1', time.time()-a
            """
            #****************** only for rviz visualization will be deleted eventually **************************************************
    
            #eoe_msg = Odometry()
            #eoe_msg.pose.pose.position.x = self.p_eoe[0]
            #eoe_msg.pose.pose.position.y = self.p_eoe[1]
            #eoe_msg.pose.pose.position.z = self.p_eoe[2]
            #eoe_msg.header.stamp = rospy.Time.now()
            #eoe_msg.header.frame_id = 'map'
            #self.eoe_odom.publish(eoe_msg)
    
    
            t = np.linspace(T[:n], T[1:n+1], 20, axis=1)
            
            xx = map(lambda i: np.poly1d(_pp1[i]), range(n))
            yy = map(lambda i: np.poly1d(_pp2[i]), range(n))
            zz = map(lambda i: np.poly1d(_pp3[i]), range(n))
            
            vx = map(lambda i: np.polyder(xx[i], 1), range(n))
            vy = map(lambda i: np.polyder(yy[i], 1), range(n))
            vz = map(lambda i: np.polyder(zz[i], 1), range(n))
            
            accx = map(lambda i: np.polyder(xx[i], 2), range(n))
            accy = map(lambda i: np.polyder(yy[i], 2), range(n))
            accz = map(lambda i: np.polyder(zz[i], 2), range(n))
    
            jx = map(lambda i: np.polyder(xx[i], 3), range(n))
            jy = map(lambda i: np.polyder(yy[i], 3), range(n))
            jz = map(lambda i: np.polyder(zz[i], 3), range(n))
            
            
                    
            #print '12', time.time()-a
            #f = open(self.path+'end_points.txt', 'a')
            #f.write('%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\n' %(time.time()-a, vel, f1, f2, local_goal, tt, T[1], currenttime, self.RHP_time, pc_data.header.stamp.to_sec(), self.hover_start_time)) 
                
            self.traj = MultiDOFJointTrajectory()
            header = std_msgs.msg.Header()
            header.stamp = rospy.Time.now()
            header.frame_id = 'map'
            self.traj.header = header
            self.traj.joint_names = 'mav_cog' # testing for now
            
            self.pathh = Path()    
            self.pathh.header.stamp = rospy.Time.now()
            self.pathh.header.frame_id = 'map'
    
            start_time =self.odom_time
            for i in range(n): # "changed to no_of_segments_to_track" instead of "no_of_segments" 
                for j in t[i]:
                    pose = PoseStamped()
                    pose.header = pc_data.header 
                    ttt = j + start_time    
                    xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
                    vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
                    axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
    
                    vector = np.array([self.goal[0]-self.Pglobal[0], self.goal[1]-self.Pglobal[1], self.goal[2]-self.Pglobal[2]])
                    #vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
                    direction = vector/np.linalg.norm(vector)
                    
                    transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
                    velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
                    accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
                    #accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
                    
                    point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(ttt))
                    self.traj.points.append(point)
                    pose.pose.position.x = xdes; pose.pose.position.y = ydes; pose.pose.position.z = zdes
                    pose.pose.orientation.x = 0; pose.pose.orientation.y = 0; pose.pose.orientation.z = 0; pose.pose.orientation.w = 1
                    self.pathh.poses.append(pose)
    
            self.uav_traj_pub.publish(self.traj)
            self.path_pub.publish(self.pathh)
            
            
            #publishing 
           
    
            f1 = open(self.path+'goals.txt', 'a')
            f1.write('%s, %s, %s, %s, %s\n' % (time.time()-a, index, T[1], self.Pglobal, ps))
            
            # publish generated pointcloud
            header = std_msgs.msg.Header()
            header.stamp = pc_data.header.stamp
            header.frame_id = 'map'#, 'firefly1/vi_sensor/camera_depth_optical_center_link'
            p = pc2.create_cloud_xyz32(header, transformed_cloud)
            self.pcl_pub.publish(p) 
            
            # publish local_goal
            header = std_msgs.msg.Header()
            header.stamp = pc_data.header.stamp
            header.frame_id = 'map'
            p = pc2.create_cloud_xyz32(header, final_points)
            self.pcl_pub2.publish(p) 
            #****************** will be deleted eventually **********************************************************************************
            """
            print '2', time.time() - a

        else:
            if RHPendcounter == 0:
                self.goal_in_fov = self.RHP_time
            self.previous_msg.execution_time_horizon = str(
                self.trajectory_endtime)
            self.traj_polycoeff.publish(self.previous_msg)
            self.RHPendcounter += 1
            if self.RHP_time > self.goal_in_fov + self.trajectory_endtime + 10:
                self.reached_goal = True

        print time.time() - a
        self.replanning_counter += 1
Esempio n. 10
0
    def callback(self, data):
        """ function to construct the trajectory
        polynmial is of the form x = c0+c1*t+c2*t**2+...cn*t**n
        """

        if self.counter == 0:
            self.Pglobal = np.array([
                data.pose.pose.position.x, data.pose.pose.position.y,
                data.pose.pose.position.z
            ])
            q = Qt(data.pose.pose.orientation.w, data.pose.pose.orientation.x,
                   data.pose.pose.orientation.y, data.pose.pose.orientation.z)
            self.Rglobal = q.rotation_matrix
            V = np.array([
                data.twist.twist.linear.x, data.twist.twist.linear.y,
                data.twist.twist.linear.z
            ])
            self.Vglobal = np.dot(self.Rglobal, V)

            t = time.time()
            r = self.r
            N = 1 + self.N  # because number of terms in a polynomial = degree+1

            QQ = []
            AA_inv = []
            #self.T = [0, 1]
            #self.no_of_segments = 1
            for i in range(self.no_of_segments):
                q = self.construct_Q(N, r, self.T[i], self.T[i + 1])
                a = self.construct_A(N, r, self.T[i], self.T[i + 1])
                a_inv = scipy.linalg.pinv(a)
                QQ = block_diag(QQ, q)
                AA_inv = block_diag(AA_inv, a_inv)
                #print 'a', a

            order = 2 * r * self.no_of_segments
            R = np.dot(AA_inv.T, np.dot(QQ, AA_inv))

            bx = np.concatenate((self.x0, self.xT), axis=0)
            by = np.concatenate((self.y0, self.yT), axis=0)
            bz = np.concatenate((self.z0, self.zT), axis=0)

            m = Model("qcp")
            order = 2 * r * self.no_of_segments

            dx = m.addVars(order,
                           lb=-GRB.INFINITY,
                           ub=GRB.INFINITY,
                           vtype=GRB.CONTINUOUS,
                           name="dx")
            dy = m.addVars(order,
                           lb=-GRB.INFINITY,
                           ub=GRB.INFINITY,
                           vtype=GRB.CONTINUOUS,
                           name="dy")
            dz = m.addVars(order,
                           lb=-GRB.INFINITY,
                           ub=GRB.INFINITY,
                           vtype=GRB.CONTINUOUS,
                           name="dz")

            # using LinExpr for the second expression is significantly faster
            obj1 = quicksum(dx[i] * LinExpr([(R[i][j], dx[j])
                                             for j in range(order)])
                            for i in range(order))
            obj2 = quicksum(dy[i] * LinExpr([(R[i][j], dy[j])
                                             for j in range(order)])
                            for i in range(order))
            obj3 = quicksum(dz[i] * LinExpr([(R[i][j], dz[j])
                                             for j in range(order)])
                            for i in range(order))

            obj = obj1 + obj2 + obj3  #+ quicksum(T[i] for i in self.no_of_segments)

            j = 0
            for i in range(order):
                if i < r:  # was r in stead of 1
                    m.addConstr(dx[i] == bx[i])
                    m.addConstr(dy[i] == by[i])
                    m.addConstr(dz[i] == bz[i])
                elif i >= order - r:
                    m.addConstr(dx[i] == bx[r + j])
                    m.addConstr(dy[i] == by[r + j])
                    m.addConstr(dz[i] == bz[r + j])
                    j += 1

            c = 1  # counter
            for i in range(r, order - 2 * r, 2 * r):
                m.addConstr(dx[i] == self.x_wp[c])
                m.addConstr(dy[i] == self.y_wp[c])
                m.addConstr(dz[i] == self.z_wp[c])

                c = c + 1
                for j in range(r):
                    m.addConstr(dx[i + j] == dx[i + j + r])
                    m.addConstr(dy[i + j] == dy[i + j + r])
                    m.addConstr(dz[i + j] == dz[i + j + r])

            m.setObjective(obj, GRB.MINIMIZE)
            #m.write('model.lp')
            m.setParam('OutputFlag', 0)
            m.setParam('PSDtol', 1e-3)
            m.setParam('NumericFocus', 3)
            m.optimize()

            runtime = m.Runtime
            optimal_objective = obj.getValue()
            #print 'optimal objective is:', optimal_objective

            x_coeff = [dx[i].X for i in range(order)]
            y_coeff = [dy[i].X for i in range(order)]
            z_coeff = [dz[i].X for i in range(order)]

            Dx = np.asarray(x_coeff)[np.newaxis].T
            Dy = np.asarray(y_coeff)[np.newaxis].T
            Dz = np.asarray(z_coeff)[np.newaxis].T
            pcx = np.dot(AA_inv, Dx)
            pcy = np.dot(AA_inv, Dy)
            pcz = np.dot(AA_inv, Dz)

            poly_coeff_x = pcx.T.ravel().tolist()
            poly_coeff_y = pcy.T.ravel().tolist()
            poly_coeff_z = pcz.T.ravel().tolist()

            if self.hover_counter == 0:
                self.hover_start_time = data.header.stamp.to_sec()

            msg = PolynomialCoefficients()
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp  #rospy.Time.now()
            header.frame_id = 'world'
            msg.header = header
            msg.polynomial_order = self.N
            for i in range(len(poly_coeff_x)):
                msg.poly_x.append(poly_coeff_x[i])
                msg.poly_y.append(poly_coeff_y[i])
                msg.poly_z.append(poly_coeff_z[i])
            msg.number_of_segments = self.no_of_segments
            msg.planning_start_time = self.hover_start_time
            for j in self.T:
                msg.segment_end_times.append(j)
            msg.desired_direction.x = 1
            msg.desired_direction.y = 0
            msg.desired_direction.z = 0
            self.traj_polycoeff.publish(msg)
            #time.sleep(self.T[self.no_of_segments]*0.9)
        else:
            print "now doing nothing"
        #self.counter += 1
        self.hover_counter += 1