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
