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