def hover_the_quad(self): """give commands to hover at 1m above the starting position""" self.RHP_time = time.time()-self.hovering_time #custom_traj = PolynomialTrajectory() #custom_traj.header.stamp = rospy.Time.now() #custom_traj.pdes.x = -6.5; custom_traj.pdes.y = -4.0; custom_traj.pdes.z = 2.0 #custom_traj.vdes.x = 0; custom_traj.vdes.y = 0; custom_traj.vdes.z = 0 #custom_traj.ades.x = 0; custom_traj.ades.y = 0; custom_traj.ades.z = 0 #custom_traj.ddes.x = 1.0; custom_traj.ddes.y = 0.0; custom_traj.ddes.z = 0.0 #custom_traj.controller = 0; custom_traj.time = self.RHP_time #self.custom_traj_pub.publish(custom_traj) # for now angular acceleration Point is used to specify the direction traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'frame' traj.header = header traj.joint_names = 'nothing' #print self.initial_position, self.initial_orientation, self.hovering_height transforms = Transform(translation = Point(self.initial_position[0], self.initial_position[1], self.hovering_height), rotation = Quaternion(0,0,0,1)) velocities = Twist(linear = Point(0, 0, 0), angular = Point(0,0,0)) accelerations = Twist(linear = Point(0, 0, 0), angular = Point(self.initial_orientation[0],self.initial_orientation[1],self.initial_orientation[2])) point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time)) traj.points.append(point) self.uav_traj_pub.publish(traj)
def generate_trajectory(self): traj_to_execute = MultiDOFJointTrajectory() traj_header = std_msgs.msg.Header() traj_velocities = Twist() traj_accelerations = Twist() traj_to_execute.joint_names = ["virtual_joint"] for i in range(0, 5): traj_header.stamp = self.get_clock().now().to_msg( ) #rclpy.time.Time().msg() traj_to_execute.header = traj_header #traj_quaternion = tf2.transformations.quaternion_from_euler(roll,pitch,yaw) traj_quaternion = [0.0, 0.0, 0.0, 0.0] traj_transforms = Transform() traj_transforms.translation.x = 1.0 * i traj_transforms.translation.y = 2.0 * i traj_transforms.translation.z = 3.0 * i traj_transforms.rotation = Quaternion() traj_transforms.rotation.x = traj_quaternion[0] traj_transforms.rotation.y = traj_quaternion[1] traj_transforms.rotation.z = traj_quaternion[2] traj_transforms.rotation.w = traj_quaternion[3] point_i = MultiDOFJointTrajectoryPoint() point_i.transforms.append(traj_transforms) point_i.velocities.append(traj_velocities) point_i.accelerations.append(traj_accelerations) duration_i = Duration(seconds=1.0).to_msg() point_i.time_from_start = duration_i # self.get_clock().now().to_msg()+Duration(seconds=1.0) traj_to_execute.points.append(point_i) return traj_to_execute
def hover_the_quad(self): """give commands to hover at 1m above the starting position""" self.RHP_time = time.time() - self.hovering_time # for now angular acceleration Point is used to specify the direction traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' traj.header = header traj.joint_names = 'firefly/base_link' #print self.initial_position, self.initial_orientation, self.hovering_height transforms = Transform(translation=Point(self.initial_position[0], self.initial_position[1], self.hovering_height), rotation=Quaternion(0, 0, 0, 1)) velocities = Twist(linear=Point(0, 0, 0), angular=Point(0, 0, 0)) accelerations = Twist(linear=Point(0, 0, 0), angular=Point(self.initial_orientation[0], self.initial_orientation[1], self.initial_orientation[2])) point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Duration(self.RHP_time)) traj.points.append(point) self.uav_traj_pub.publish(traj)
def command_pose(self, position, orientation): msg_traj = MultiDOFJointTrajectory() msg_traj.header.stamp = rospy.Time() msg_traj.joint_names = ["base_link"] p = Vector3(position[0], position[1], position[2]) q = Quaternion( orientation[0], orientation[1], orientation[2], orientation[3] ) transforms = [Transform(translation=p, rotation=q)] point = MultiDOFJointTrajectoryPoint( transforms, [Twist()], [Twist()], rospy.Time() ) msg_traj.points.append(point) self.pub_traj.publish(msg_traj)
def publisher(self, trajectory_parameters): """ Generates and publishes a MultiDOFJointTrajectory message from inputs to publisher topic for simulator. Args: trajectory_parameters(dict): A dictionary containing all data fields required to build a Trajectory message. """ trajectory = MultiDOFJointTrajectory() trajectory.header = Header() trajectory.header.stamp = rospy.Time() trajectory.header.frame_id = '' trajectory.joint_names = ["base_link"] # Create the message expected by the simulator. point = MultiDOFJointTrajectoryPoint([ self.create_point(trajectory_parameters["x"], trajectory_parameters["y"], trajectory_parameters["z"], trajectory_parameters["rotation_x"], trajectory_parameters["rotation_y"], trajectory_parameters["rotation_z"], trajectory_parameters["rotation_w"]) ], [ self.create_velocity(trajectory_parameters["velocity_x"], trajectory_parameters["velocity_y"], trajectory_parameters["velocity_z"], trajectory_parameters["velocity_angular_x"], trajectory_parameters["velocity_angular_y"], trajectory_parameters["velocity_angular_z"]) ], [ self.create_acceleration( trajectory_parameters["acceleration_linear_x"], trajectory_parameters["acceleration_linear_y"], trajectory_parameters["acceleration_linear_z"], trajectory_parameters["acceleration_angular_x"], trajectory_parameters["acceleration_angular_y"], trajectory_parameters["acceleration_angular_z"]) ], rospy.Time(1)) trajectory.points.append(point) pub.publish(trajectory)
def publish_trajectory(self, trajectory): trajectory_msg = MultiDOFJointTrajectory() trajectory_msg.header.frame_id = 'world' trajectory_msg.joint_names = ['base'] for idx in range(len(trajectory)): point = trajectory[idx] point['time'] = trajectory[idx]['time'] transform = Transform() transform.translation = Vector3(*(point['point'].tolist())) transform.rotation = Quaternion( *(vec_to_quat(point['velocity']).tolist())) velocity = Twist() velocity.linear = Vector3(*(point['velocity'].tolist())) acceleration = Twist() acceleration.linear = Vector3(*(point['acceleration'].tolist())) trajectory_msg.points.append( MultiDOFJointTrajectoryPoint([transform], [velocity], [acceleration], rospy.Duration(point['time']))) trajectory_msg.header.stamp = rospy.Time.now() self.traj_pub.publish(trajectory_msg)
def trajectory_in_camera_fov(self, data): """generates trajectory in camera workspace""" #self.end_point = self.end_point + np.array([-self.target_velocity*0.05,0, 0]) 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 = 'vicon' self.target_odom.publish(odom_msg) start = time.time() 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 points_in_cone = self.generate_regular_pyramid_grid() occupied_points = ros_numpy.numpify(data) #for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s, %s, %s\n' %(self.traj_gen_counter, self.RHP_time, len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) #if self.traj_gen_counter == 0: ## for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s\n' %(len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) if len(occupied_points) != 0: points_in_cone = self.generate_final_grid(points_in_cone, occupied_points) """ points_in_cone = [x[np.newaxis] for x in points_in_cone] pp1 = [np.dot(self.Rcdoc_cdl[0:3, 0:3].T, x.T) for x in points_in_cone] # now translate from visensor frame to cg frame pp2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in pp1] # now rotate the cloud in world frame points_in_cone = [self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in pp2] # remove z points if they are going inside the ground, assuming ground is z = 0 points_in_cone = [x for x in points_in_cone if not x[2] <= 0] points_in_cone = np.concatenate((start_point, points_in_cone), 0) """ #points_in_cone = np.concatenate((start_point, points_in_cone), 0) p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone] #p1 = [self.Rvibase_cl[0:3, 3:4] + 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(self.Rcg_vibase[0:3, 0:3].T, 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] points_in_cone = np.concatenate((start_point, points_in_cone), 0) # publish generated pointcloud header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' p = pc2.create_cloud_xyz32(header, points_in_cone) self.pcl_pub.publish(p) 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())) 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]) except: print 'No path between start and end' #f1 = open('path.txt', 'a') #f1.write('%s, %s\n' %(path, points_in_cone[end_point_index])) #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 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 path = zip(*path) # now construct the minimum snap trajectory on the minimum path waypoint_specified = True waypoint_bc = False planning_counter = 2 T, p1, p2, p3 = Minimum_snap_trajetory(planning_counter, self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory() #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3) N = 8 p1 = [p1[i:i + N] for i in range(0, len(p1), N)] [i.reverse() for i in p1] p2 = [p2[i:i + N] for i in range(0, len(p2), N)] [i.reverse() for i in p2] p3 = [p3[i:i + N] for i in range(0, len(p3), N)] [i.reverse() for i in p3] t = []; xx = []; yy = []; zz = [] vx = []; vy = []; vz = []; accx = []; accy = []; accz = [] jx = []; jy = []; jz = [] #print T 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], 21)) 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)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' traj.header = header traj.joint_names = 'firefly/base_link' # testing for now trajectory_start_time = data.header.stamp.to_sec()#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) # 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) 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]]) self.uav_traj_pub.publish(traj) time_taken = time.time()-start self.traj_gen_counter += 1 print 'total time taken to execute the callbacak is:', time_taken
def trajectory_callback(self, data): #initialize and prepare screen #if self.counter == 0: a = time.time() occupied_points = ros_numpy.numpify(data) points_in_cone = self.generate_regular_pyramid_grid() points_in_cone = self.generate_final_grid(points_in_cone, occupied_points) # now convert the points generated in the local frame of the visensor (xyz of the camera where z points outside the camera) # to the points in quadrotor cg xyz frame(x forward z upwards) located at the visensor print '1', time.time() - a p1 = [ self.Rvibase_cl[0:3, 3:4] + np.dot(self.Rcdoc_cdl[0:3, 0:3], x[np.newaxis].T) for x in points_in_cone ] p2 = [np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1] points_in_cone = [ self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in p2 ] points_in_cone = [x for x in points_in_cone if not x[2] <= 0] #points_in_cone = [np.dot(self.Rcg_vibase[0:3, 0:3].T, self.Rvibase_cl[0:3, 3:4]+np.dot(self.Rcdoc_cdl[0:3,0:3],x[np.newaxis].T)).ravel() for x in points_in_cone] print '2', time.time() - a kdt_points_in_cone = cKDTree(points_in_cone) closest_to_end = kdt_points_in_cone.query(self.end_point, 1) print closest_to_end #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] print 'the goal point is', points_in_cone[end_point_index] #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) 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]) #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 no_of_segments = 5 path = path[:no_of_segments + 1] # 5 segments means 6 points in the path path = zip(*path) #no_of_segments = len(path)-1 # now construct the minimum snap trajectory on the minimum path waypoint_specified = True waypoint_bc = False print path T, p1, p2, p3 = Minimum_snap_trajetory( path, waypoint_specified, waypoint_bc).construct_polynomial_trajectory() print '8', time.time() - a #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3) msg = Float32() msg.data = 0.0 self.pub1.publish(msg) N = 8 p1 = [p1[i:i + N] for i in range(0, len(p1), N)] [i.reverse() for i in p1] p2 = [p2[i:i + N] for i in range(0, len(p2), N)] [i.reverse() for i in p2] p3 = [p3[i:i + N] for i in range(0, len(p3), N)] [i.reverse() for i in p3] t = [] xx = [] yy = [] zz = [] vx = [] vy = [] vz = [] accx = [] accy = [] accz = [] print '9', time.time() - a traj_freq = 100 # frequency of the trajectory in Hz for i in range(no_of_segments): t.append( np.linspace(T[i], T[i + 1], int((T[i + 1] - T[i]) * traj_freq + 1))) xx.append(np.poly1d(p1[i])) 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[i])) 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[i])) 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)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'frame' traj.header = header traj.joint_names = 'base_link' time_diff = no_of_segments * len(t[0]) * 30 print 'inverse of time_diff is', 1.0 / time_diff #epoch = rospy.Time() #print epoch, no_of_segments #tt = rospy.Time(1, 30) #print tt msg1 = PolynomialTrajectory() for i in range(no_of_segments): for j in t[i]: 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) 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(0, 0, 0)) point = MultiDOFJointTrajectoryPoint( [transforms], [velocities], [accelerations], rospy.Time(1.0 / time_diff)) traj.points.append(point) #rospy.sleep(1.0/time_diff) # publish the trajcetory to custom ros message that is used for all other algorithm msg1.pdes.x = xdes msg1.pdes.y = ydes msg1.pdes.z = zdes msg1.vdes.x = vxdes msg1.vdes.y = vydes msg1.vdes.z = vzdes msg1.ades.x = axdes msg1.ades.y = aydes msg1.ades.z = azdes msg1.ddes.x = 1 msg1.ddes.y = 0 msg1.ddes.z = 0 msg1.controller = 0 self.custom_uav_traj_pub.publish(msg1) f0 = open('trajectory.txt', 'a') f0.write("%s, %s, %s\n" % (xdes, ydes, zdes)) time.sleep(1.0 / 120) f1 = open('trajectory1.txt', 'a') f1.write("%s\n" % (traj)) self.uav_traj_pub.publish(traj) print '11', time.time() - a
#!/usr/bin/env python import rospy from nav_msgs.msg import Odometry from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Point from std_msgs.msg import Header from trajectory_msgs.msg import MultiDOFJointTrajectory, MultiDOFJointTrajectoryPoint from geometry_msgs.msg import Twist from geometry_msgs.msg import Transform firefly_pub = rospy.Publisher('/firefly/command/trajectory',MultiDOFJointTrajectory,queue_size = 5) traj = MultiDOFJointTrajectory() traj.header.frame_id = '' traj.header.stamp = rospy.Time.now() traj.joint_names = ["base_link"] desired_pos = Transform() desired_pos.translation.x = 2 desired_pos.translation.y = 2 desired_pos.translation.z = 4 desired_vel = Twist() desired_accl = Twist() point = MultiDOFJointTrajectoryPoint([desired_pos],[desired_vel],[desired_accl],rospy.Duration(1)) traj.points.append(point) firefly_pub.publish(traj)
import math if __name__ == '__main__': if len(sys.argv) < 4: print("Usage: rosrun cascaded_pid_control set_point.py target_x target_y target_z [target_yaw]") exit(-1) rospy.init_node("SetPoint") x, y, z = float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) yaw = 0 if len(sys.argv) > 4: yaw = float(sys.argv[4]) print("Will set location of the drone to {}, {}, {}. Yaw: {} degree".format(x, y, z, yaw)) traj_pub = rospy.Publisher("/CascadedPidControl/trajectory", MultiDOFJointTrajectory, queue_size=1, latch=True) trajectory = MultiDOFJointTrajectory() trajectory.header.frame_id = 'world' trajectory.header.stamp = rospy.Time.now() trajectory.joint_names = ['base'] transform = Transform() transform.translation = Vector3(x, y, z) transform.rotation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, yaw * math.pi / 180.0, 'rxyz').tolist()) trajectory.points.append(MultiDOFJointTrajectoryPoint([transform], [Twist()], [Twist()], rospy.Duration(0))) traj_pub.publish(trajectory) while not rospy.is_shutdown(): rospy.sleep(rospy.Duration(3)) rospy.signal_shutdown("Job done.") rospy.spin()
def launch_simulation(self): # Wait for Gazebo services rospy.loginfo("Starting unreal MAV simulation setup coordination...") rospy.wait_for_service(self.ns_gazebo + "/unpause_physics") rospy.wait_for_service(self.ns_gazebo + "/set_model_state") # Prepare initialization trajectory command traj_pub = rospy.Publisher(self.ns_mav + "/command/trajectory", MultiDOFJointTrajectory, queue_size=10) traj_msg = MultiDOFJointTrajectory() traj_msg.joint_names = ["base_link"] transforms = Transform() transforms.translation.x = self.initial_position[0] transforms.translation.y = self.initial_position[1] transforms.translation.z = self.initial_position[2] point = MultiDOFJointTrajectoryPoint([transforms], [Twist()], [Twist()], rospy.Duration(0)) traj_msg.points.append(point) # Prepare initialization Get/SetModelState service set_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/set_model_state", SetModelState) get_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/get_model_state", GetModelState) mav_name = self.ns_mav[np.max( [i for i in range(len(self.ns_mav)) if self.ns_mav[i] == "/"]) + 1:] pose = Pose() pose.position.x = self.initial_position[0] pose.position.y = self.initial_position[1] pose.position.z = self.initial_position[2] model_state_set = ModelState(mav_name, pose, Twist(), "world") # Wake up gazebo rospy.loginfo("Waiting for gazebo to wake up ...") unpause_srv = rospy.ServiceProxy(self.ns_gazebo + "/unpause_physics", Empty) while not unpause_srv(): rospy.sleep(0.1) rospy.loginfo("Waiting for gazebo to wake up ... done.") # Wait for drone to spawn (imu is publishing) rospy.loginfo("Waiting for MAV to spawn ...") rospy.wait_for_message(self.ns_mav + "/imu", Imu) rospy.loginfo("Waiting for MAV to spawn ... done.") # Initialize drone stable at [0, 0, 0] rospy.loginfo("Waiting for MAV to stabilize ...") dist = 10 # Position and velocity while dist >= 0.1: traj_msg.header.stamp = rospy.Time.now() traj_pub.publish(traj_msg) set_model_srv(model_state_set) rospy.sleep(0.1) state = get_model_srv(mav_name, "world") pos = state.pose.position twist = state.twist.linear dist = np.sqrt((pos.x - self.initial_position[0])**2 + (pos.y - self.initial_position[1])**2 + (pos.z - self.initial_position[2])**2) + np.sqrt( twist.x**2 + twist.y**2 + twist.z**2) rospy.loginfo("Waiting for MAV to stabilize ... done.") # Wait for unreal client rospy.loginfo("Waiting for unreal client to setup ...") rospy.wait_for_message("ue_out_in", PointCloud2) rospy.loginfo("Waiting for unreal client to setup ... done.") # Finish self.ready_pub.publish(String("Simulation Ready")) rospy.loginfo("Unreal MAV simulation setup successfully.")
def handle_traj_gen(req): vel_constr = 2 angle_constr = 3.14159265 / 4 yaw_init_state = 0 yaw_locked = True if yaw_locked: yaw_constr_plus = yaw_init_state yaw_constr_minus = yaw_init_state else: yaw_constr_plus = 3.14159265 * 4 yaw_constr_minus = -3.14159265 * 4 input_constr = 6 x0pos = [0, 0, 1] x0vel = [0, 0, 0] x0ang = [0, 0, yaw_init_state] x0angrate = [0, 0, 0] x1pos = [2., 0., 2.5] x1vel_y = 0 x1vel_z = 0 x2pos = [2., 4., 2.5] #??? x y z mistake x2vel_y = 0 x2vel_z = 0 xFpos = [0, 0, 1.2] xFvel = [0, 0, 0] xFang = [0, 0, yaw_init_state] xFangrate = [0, 0, 0] N = 25 # x = vertcat(pos,vel,ang,angrate) x = MX.sym('x', 12) u = MX.sym('u', 4) t_transf = MX.sym('t_transf') t = MX.sym('t') xdot = diff_eq_time_trans(0, x, u, t_transf) L = (u[0]**2 + u[1]**2 + u[2]**2 + u[3]**2) * t_transf f = Function('f', [t, x, u, t_transf], [xdot]) fq = Function('fq', [t, x, u, t_transf], [L]) X0 = MX.sym('X0', 12) U = MX.sym('U', 4) TF = MX.sym('TF') M = 1 DT = 1. / N / M X = X0 Q = 0 f_handle = lambda t, x, u: f(t, x, u, TF) fq_handle = lambda t, x, u: fq(t, x, u, TF) for j in range(M): X = rk4(f_handle, DT, 0., X, U) Q = rk4(fq_handle, DT, 0., 0, U) F = Function('F', [X0, U, TF], [X, Q], ['x0', 'p', 'tf_interval'], ['xf', 'qf']) # Start with an empty NLP w = [] w0 = [] lbw = [] ubw = [] J = 0 g = [] lbg = [] ubg = [] # "Lift" initial conditions # t_interval_1 = MX.sym('t_interval_1') t_interval_2 = MX.sym('t_interval_2') t_interval_3 = MX.sym('t_interval_3') # Xk = MX.sym('X0', 12) w += [Xk] # this initial condition has the specified bound, # with only one possible solution # x0(0)=1; x1(0)=0; lbw += x0pos + x0vel + x0ang + x0angrate ubw += x0pos + x0vel + x0ang + x0angrate #solver's guess w0 += x0pos + x0vel + x0ang + x0angrate gravity, mass, kF, kM, Len = project_parameters() for k in range(N): Uk = MX.sym('U_' + str(k), 4) w += [Uk] if k != 0: lbw += [0, 0, 0, 0] ubw += [input_constr, input_constr, input_constr, input_constr] else: lbw += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] ubw += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] w0 += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_1) Xk_end = Fk['xf'] J = J + Fk['qf'] Xk = MX.sym('X_' + str(k + 1), 12) w += [Xk] if k != N - 1: lbw += [ -inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr, -angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf, -inf ] ubw += [ inf, inf, inf, vel_constr, vel_constr, vel_constr, angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf ] else: lbw += x1pos + [ -vel_constr, x1vel_y, x1vel_z, -angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf, -inf ] ubw += x1pos + [ vel_constr, x1vel_y, x1vel_z, angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf ] w0 += x0pos + [0, 0, 0, 0, 0, 3, 0, 0, 0] g += [Xk_end - Xk] lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] for k in range(N, 2 * N): Uk = MX.sym('U_' + str(k), 4) w += [Uk] lbw += [0, 0, 0, 0] ubw += [input_constr, input_constr, input_constr, input_constr] w0 += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_2) Xk_end = Fk['xf'] J = J + Fk['qf'] Xk = MX.sym('X_' + str(k + 1), 12) w += [Xk] if k != 2 * N - 1: lbw += [ -inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr, -angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf, -inf ] ubw += [ inf, inf, inf, vel_constr, vel_constr, vel_constr, angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf ] else: #??? x y z mistake lbw += x2pos + [ -vel_constr, x2vel_y, x2vel_z, -angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf, -inf ] ubw += x2pos + [ vel_constr, x2vel_y, x2vel_z, angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf ] w0 += x1pos + [0, 0, 0, 0, 0, 3, 0, 0, 0] g += [Xk_end - Xk] lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] for k in range(2 * N, 3 * N): Uk = MX.sym('U_' + str(k), 4) w += [Uk] if k != 3 * N - 1: lbw += [0, 0, 0, 0] ubw += [input_constr, input_constr, input_constr, input_constr] else: lbw += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] ubw += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] w0 += [ 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass, 0.25 * gravity * mass ] Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_3) Xk_end = Fk['xf'] J = J + Fk['qf'] Xk = MX.sym('X_' + str(k + 1), 12) w += [Xk] if k != 3 * N - 1: lbw += [ -inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr, -angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf, -inf ] ubw += [ inf, inf, inf, vel_constr, vel_constr, vel_constr, angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf ] else: lbw += xFpos + xFvel + xFang + xFangrate ubw += xFpos + xFvel + xFang + xFangrate w0 += x2pos + [0, 0, 0, 0, 0, 3, 0, 0, 0] g += [Xk_end - Xk] lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] w += [t_interval_1, t_interval_2, t_interval_3] lbw += [0.1, 0.1, 0.1] ubw += [inf, inf, inf] w0 += [2, 3, 2] # g += [t_interval_1,t_interval_2,t_interval_3] # lbg += [0,0,0] # ubg += [inf,inf,inf] prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)} solver = nlpsol('solver', 'ipopt', prob) sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) w_opt_all = sol['x'].full().flatten() t1_opt = w_opt_all[-3] t2_opt = w_opt_all[-2] t3_opt = w_opt_all[-1] print(t1_opt) print(t2_opt) print(t3_opt) w_opt = np.delete(w_opt_all, [-3, -2, -1]) pos_x_opt = w_opt[0::16] pos_y_opt = w_opt[1::16] pos_z_opt = w_opt[2::16] vel_x_opt = w_opt[3::16] vel_y_opt = w_opt[4::16] vel_z_opt = w_opt[5::16] ang_x_opt = w_opt[6::16] ang_y_opt = w_opt[7::16] ang_z_opt = w_opt[8::16] angr_x_opt = w_opt[9::16] angr_y_opt = w_opt[10::16] angr_z_opt = w_opt[11::16] u1_opt = [] u2_opt = [] u3_opt = [] u4_opt = [] for k in range(3 * N): u1_opt += [w_opt[12 + k * 16]] u2_opt += [w_opt[13 + k * 16]] u3_opt += [w_opt[14 + k * 16]] u4_opt += [w_opt[15 + k * 16]] tgrid1 = [t1_opt / N * k for k in range(N)] tgrid2 = [t1_opt + t2_opt / N * k for k in range(N)] tgrid3 = [t1_opt + t2_opt + t3_opt / N * k for k in range(N + 1)] tgrid3u = [t1_opt + t2_opt + t3_opt / N * k for k in range(N)] tgrid = tgrid1 + tgrid2 + tgrid3 tgridu = tgrid1 + tgrid2 + tgrid3u msg = MultiDOFJointTrajectory() msg.header.frame_id = '' msg.header.stamp = rospy.Time.now() msg.joint_names = ["base_link"] for i in range(len(tgrid)): transforms = Transform() transforms.translation.x = pos_x_opt[i] transforms.translation.y = pos_y_opt[i] transforms.translation.z = pos_z_opt[i] quaternion = tf.transformations.quaternion_from_euler( ang_x_opt[i], ang_y_opt[i], ang_z_opt[i]) transforms.rotation = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]) velocities = Twist() velocities.linear.x = vel_x_opt[i] velocities.linear.y = vel_y_opt[i] velocities.linear.z = vel_z_opt[i] # velocities.angular.x = accelerations = Twist() # time_from_start += rospy.Duration.from_sec(waypoints[i].waiting_time) point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Duration.from_sec(tgrid[i])) msg.points.append(point) return msg
def create_path(self, odometry, gates, last_visited_gate): # types are Odometry, [Pose], Pose start_position = odometry.pose.pose.position start_velocity = geo.rotate_vector_wrt_quaternion( odometry.pose.pose.orientation, odometry.twist.twist.linear) if last_visited_gate is not None: visited_index = None for i in range(len(gates)): if geo.distance(last_visited_gate.position, gates[i].position) < 1.0: visited_index = i if visited_index is not None: gates = gates[visited_index + 1:] + gates[:visited_index + 1] # in case of re-planning, determine the current nearest waypoint. look forward about some specific # time into the current path (currently 1s) and let that future waypoint be the starting waypoint of # re-planning waypoints = [] start = geo.vector_to_list(start_position) look_ahead_time = 1 nearest_waypoint_idx = None planning_waypoint_idx = None current_gate_location = geo.vector_to_list( self.gates[self.target_gate_idx].position) if self.current_path is not None: nearest_waypoint_idx = self.search_for_nearest_waypoint( geo.vector_to_list(start_position)) if nearest_waypoint_idx >= 0: current_path = self.current_path["path"] waypoints.append( current_path[nearest_waypoint_idx]["point_as_list"]) waypoint_time = current_path[nearest_waypoint_idx]["time"] planning_waypoint_idx = nearest_waypoint_idx current_path_length = len(current_path) while planning_waypoint_idx < current_path_length: planning_waypoint = current_path[planning_waypoint_idx] if planning_waypoint[ "time"] - waypoint_time < look_ahead_time: planning_waypoint_idx += 1 else: break if planning_waypoint_idx >= current_path_length: planning_waypoint_idx = current_path_length - 1 start_position = current_path[planning_waypoint_idx][ "point_as_list"] waypoints.append(start_position) if len(waypoints) == 0: print( "The first planning or a re-planning from scratch is needed.") orientation = np.array(current_gate_location) - np.array(start) orientation_normalized = orientation / np.linalg.norm(orientation) # append a point 5 meters behind along with the current position as the starting position waypoints.append( (np.array(start) - 5 * orientation_normalized).tolist()) waypoints.append(start) else: print("Re-planning") # we already have two position around current position, # we still need 2 positionp around the gate # # make sure we won't going back if we're still heading to the current gate # and if we're too close to the target gate, head for the next too. target_gate_location = geo.vector_to_list( self.gates[self.target_gate_idx].position) if self.is_cross_gate(self.target_gate_idx, waypoints[0], waypoints[1]): if self.target_gate_idx + 1 < len(self.gates): rospy.loginfo( "About to cross gate{}. Heading for next gate at index {}". format(self.target_gate_idx, self.target_gate_idx + 1)) gate = self.gates[self.target_gate_idx + 1] else: rospy.loginfo( "Gates are about to be all passed. Stop planning.") gate = self.gates[self.target_gate_idx] self.stop_planning = True return elif self.distance_to_gate(waypoints[0], self.target_gate_idx) <= 0.5 or \ self.distance_to_gate(waypoints[1], self.target_gate_idx) <= 0.5: if self.target_gate_idx + 1 < len(self.gates): rospy.loginfo( "Close to gate {}. Heading for gate at index {}".format( self.target_gate_idx, self.target_gate_idx + 1)) gate = self.gates[self.target_gate_idx + 1] else: rospy.loginfo( "Gates are about to be all passed. Stop planning.") gate = self.gates[self.target_gate_idx] self.stop_planning = True return else: gate = self.gates[self.target_gate_idx] # append waypoints +- 0.5 meters around the next gate offsets = [-0.5, 0.5] for offset in offsets: waypoints.append( geo.vector_to_list( geo.point_plus_vector( gate.position, geo.quaternion_to_vector(gate.orientation, offset)))) # wp = np.array(waypoints) # w0 = wp[:-1] # w1 = wp[1:] # diff = w0 - w1 # norm = np.linalg.norm(diff, axis=1) # new_path_chord_length = np.sum(norm) # scale derivative w.r.t. total chord length # if deriv is not None: # old_path_chord_length = self.current_path["chord_length"] # ratio = min(new_path_chord_length / old_path_chord_length, 1.0) # deriv = list(ratio * v for v in deriv) raw_waypoints_marker = Marker() raw_waypoints_marker.header.stamp = rospy.Time.now() raw_waypoints_marker.header.frame_id = "world" raw_waypoints_marker.color = ColorRGBA(1.0, 1.0, 0.0, 1.0) raw_waypoints_marker.scale = Vector3(0.5, 0.5, 0.5) raw_waypoints_marker.type = Marker.SPHERE_LIST raw_waypoints_marker.action = Marker.ADD raw_waypoints_marker.id = 1 for wp in waypoints: raw_waypoints_marker.points.append(Point(wp[0], wp[1], wp[2])) self.raw_waypoints_viz_pub.publish(raw_waypoints_marker) path = SmoothedPath() path.fit(waypoints) trajectory_points = [] def visit_cb(pt, deriv1, deriv2, s, t): # curvature is calcuated as norm(deriv1 x deriv2) / norm(deriv1)**3 # see: https://en.wikipedia.org/wiki/Curvature#Local_expressions_2 d1xd2 = np.cross(deriv1, deriv2) norm_d1 = np.linalg.norm(deriv1) norm_d2 = np.linalg.norm(deriv2) if norm_d1 > 1e-5: c = np.linalg.norm(d1xd2) / norm_d1**3 else: c = 0 # the first order derivative is given as ds/dt, where s is the arc length and t is the internal parameter of the spline, # not time. # because of this, the magnitude of first order derivative is not the same with viable speed, # but nevertheless, the direction of the derivative of them are the same. # also note that unit normal vector at point is just the normalized second order derivative, # it is in the opposite direction of the radius vector # the cross product of unit tangent vector and unit normal vector # is also mentioned as unit binormal vector if norm_d1 > 1e-5: unit_d1 = deriv1 / norm_d1 else: unit_d1 = np.array([0.0, 0.0, 0.0]) if norm_d2 > 1e-5: unit_d2 = deriv2 / norm_d2 else: unit_d2 = np.array([0.0, 0.0, 0.0]) unit_binormal = np.cross(unit_d1, unit_d2) ds = 0 if len(trajectory_points) > 0: last_point = trajectory_points[-1] ds = s - last_point['s'] trajectory_points.append({ 't': t, 's': s, 'point_as_list': pt, 'derivative': deriv1, 'derivative2': deriv2, 'ds': ds, 'point': Vector3(*(pt.tolist())), 'speed': self.max_speed, 'speed_direction': Vector3(*(unit_d1.tolist())), 'unit_normal': Vector3(*(unit_d2.tolist())), 'unit_binormal': Vector3(*(unit_binormal.tolist())), 'curvature': c }) path.visit_at_interval(self.ds, visit_cb, self.trajectory_length) self.current_path = {"chord_length": 0, "path": trajectory_points} # trajectory_points = [{'s': i * ds, 'speed': self.max_speed, 'curvature': geo.spline_distance_to_curvature(waypoint_spline, i*ds)} for i in range(30)] vmin = self.min_speed vmax = self.max_speed amax = self.max_acceleration def safe_speed(curvature, speed_neighbor, ds, accel=None): if accel is not None: return math.sqrt(speed_neighbor**2 + 2 * ds * accel) centripetal = curvature * speed_neighbor**2 if centripetal >= amax: return max(vmin, min(vmax, math.sqrt(abs(amax / curvature)))) remaining_acceleration = math.sqrt(amax**2 - centripetal**2) # see /Planning Motion Trajectories for Mobile Robots Using Splines/ # (refered as Sprunk[2008] later) for more details (eq 3.21) v_this = math.sqrt(speed_neighbor**2 + 2 * ds * remaining_acceleration) return max(vmin, min(vmax, v_this)) #print(geo.magnitude_vector(start_velocity)) # trajectory_points[0]['speed'] = min(vmax, max(vmin, geo.magnitude_vector(start_velocity))) trajectory_points[0]['speed'] = geo.magnitude_vector(start_velocity) #print(trajectory_points[0]['speed']) num_traj = len(trajectory_points) for i in range( num_traj - 1 ): # iterate forwards, skipping first point which is fixed to current speed trajectory_points[i + 1]['speed'] = safe_speed( trajectory_points[i + 1]['curvature'], trajectory_points[i]['speed'], trajectory_points[i + 1]['ds']) # skip the backward phase for 2 reason: # 1. we don't need a smooth stop. just complete the course # asap # 2. backward phase would change the start speed, # usually slower than the current speed. this # sudden change of speed would make the drone # "jerking" between trajectory switch. # for i in range(num_traj - 2): # j = num_traj - i - 2 # iterate backwards, skipping both end points # curvature = trajectory_points[j]['curvature'] # min_neighbor_speed = min(trajectory_points[j-1]['speed'],trajectory_points[j+1]['speed']) # trajectory_points[j]['speed'] = safe_speed(curvature, min_neighbor_speed, trajectory_points[i+1]['ds']) # Set velocity based on speed and direction for point in trajectory_points: point['velocity'] = geo.scalar_multiply(point['speed'], point['speed_direction']) # the relationship between angular velocity and linear velocity is: # \omega = r x v / || r ||^2 # where r is the radius vector, v is linear velocity. # see: https://en.wikipedia.org/wiki/Angular_velocity#Particle_in_three_dimensions # note: with the anti-commutative law of cross product, # unit binormal B = v x (-r) / (||v|| * ||r||) = r x v / (||v|| * ||r||) # and curvature k = 1 / || r || # so, omega = || v || * k * B omega = geo.scalar_multiply( geo.magnitude_vector(point['velocity']) * point['curvature'], point['unit_binormal']) point['omega'] = omega # Set time based on speed and distance trajectory_points[0]['time'] = 0.0 for i in range(num_traj - 1): ds = trajectory_points[i + 1]['ds'] prev_time = trajectory_points[i]['time'] # see Sprunk[2018] eq 3.20 ave_speed = 0.5 * (trajectory_points[i]['speed'] + trajectory_points[i + 1]['speed']) trajectory_points[i + 1]['time'] = prev_time + ds / ave_speed # low pass filter on the designated speed. lpf = LowPassFilter(trajectory_points[0]['speed'], 5) for i in range(1, num_traj): trajectory_points[i]['speed'] = lpf.update( trajectory_points[i]['speed'], trajectory_points[i]['time']) # print(geo.magnitude_vector(start_velocity)) # for pt in trajectory_points: # print(pt['speed']) # print("------") # Set acceleration based on change in velocity # we're assuming constant accelerations between waypoints, # a is just \delta V / \delta t for i in range(num_traj - 1): t_current = trajectory_points[i]['time'] t_next = trajectory_points[i + 1]['time'] dt = t_next - t_current v_current = trajectory_points[i]['velocity'] v_next = trajectory_points[i + 1]['velocity'] dv = geo.vector_from_to(v_current, v_next) w_current = trajectory_points[i]['omega'] w_next = trajectory_points[i]['omega'] dw = geo.vector_from_to(w_current, w_next) trajectory_points[i]['acceleration'] = geo.scalar_multiply( 1.0 / dt, dv) trajectory_points[i]['acceleration_w'] = geo.scalar_multiply( 1.0 / dt, dw) trajectory_points[-1]['acceleration'] = trajectory_points[-2][ 'acceleration'] trajectory_points[-1]['acceleration_w'] = trajectory_points[-2][ 'acceleration_w'] trajectory = MultiDOFJointTrajectory() trajectory.header.frame_id = '' trajectory.joint_names = ['base'] for idx in range(len(trajectory_points)): point = trajectory_points[idx] point['time'] = trajectory_points[idx]['time'] transform = Transform() transform.translation = point['point'] transform.rotation = Quaternion( *(geo.vector_to_quat(point['velocity']).tolist())) velocity = Twist() velocity.linear = point['velocity'] velocity.angular = point['omega'] acceleration = Twist() acceleration.linear = point['acceleration'] acceleration.angular = point['acceleration_w'] trajectory.points.append( MultiDOFJointTrajectoryPoint([transform], [velocity], [acceleration], rospy.Duration(point['time']))) trajectory.header.stamp = rospy.Time.now() return trajectory