Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
629 lines (564 sloc) 23 KB
from numpy import *
from scipy.sparse import csc_matrix
import osqp
import math
class Waypoint:
"""
Store a waypoint.
Attributes:
x,y,z: position in world frame
yaw: Euler angle of waypoint
time: time at which the waypoint is to be reached
"""
def __init__(self, x, y, z, yaw, time):
self.x = x
self.y = y
self.z = z
self.yaw = yaw
self.time = time
def calc_time(start, end):
"""
Calculate a fitting time for the trajectory between two waypoints.
"""
distance3D = sqrt((start[0]-end[0])**2 + (start[1]-end[1])**2 + (start[2]-end[2])**2)
time = distance3D # ..where an engineer cries and a programmer sees an easy solution
return time
def joint(waypoints):
"""
Calculate a trajectory by a joint operation.
"""
# total number of segments
numSegments = len(waypoints) - 1
# every segment has its own polynomial of 4th degree for X,Y and Z and a polynomial of 2nd degree for Yaw
numCoefficients = numSegments * (3*5+3)
# list of calculated trajectory coefficients
trajectory = []
# start + end X,Y,Z,Yaw position for every segment: 8
# rendezvous X,Y,Z,Yaw velocity: 4
# absolute start + end X,Y,Z (+ start Yaw) velocity: 7
numConstraints = numSegments * 8 + (numSegments - 1) * 4 + 7
P_numpy = zeros((numCoefficients, numCoefficients))
for i in range(numSegments):
P_numpy[0 + i * 18, 0 + i * 18] = 1 # minimize snap for X
# P_numpy[2 + i * 18, 2 + i * 18] = 100 # minimize acceleration for X
P_numpy[5 + i * 18, 5 + i * 18] = 1 # minimize snap for Y
# P_numpy[7 + i * 18, 7 + i * 18] = 100 # minimize acceleration for Y
P_numpy[10 + i * 18, 10 + i * 18] = 1 # minimize snap for Z
# P_numpy[12 + i * 18, 12 + i * 18] = 100 # minimize acceleration for Z
P_numpy[15 + i * 18, 15 + i * 18] = 1 # minimize acceleration for Yaw
P = csc_matrix(P_numpy) # convert to CSC for performance
# =============================
# Gradient vector (linear terms), we have none
# =============================
q = zeros((numCoefficients, 1))
q = hstack(q) # convert to hstack for performance
# =============================
# Inequality matrix (left side), we have none
# =============================
G = zeros((numConstraints, numCoefficients))
# =============================
# Inequality vector (right side), we have none
# =============================
h = zeros((numConstraints, 1))
h = hstack(h) # convert to hstack for performance
# =============================
# Equality matrix (left side)
# =============================
A = zeros((numConstraints, numCoefficients))
# =============================
# Equality vector (right side)
# =============================
b = zeros((numConstraints, 1))
# =============================
# Set up of Equality Constraints
# =============================
cc = -1 # Current Constraint
for i in range(numSegments):
# "start of segment" position constraints
cc += 1 # X Position
A[cc, 0 + i * 18] = waypoints[i].time ** 4
A[cc, 1 + i * 18] = waypoints[i].time ** 3
A[cc, 2 + i * 18] = waypoints[i].time ** 2
A[cc, 3 + i * 18] = waypoints[i].time
A[cc, 4 + i * 18] = 1
b[cc, 0] = waypoints[i].x
cc += 1 # Y Position
A[cc, 5 + i * 18] = waypoints[i].time ** 4
A[cc, 6 + i * 18] = waypoints[i].time ** 3
A[cc, 7 + i * 18] = waypoints[i].time ** 2
A[cc, 8 + i * 18] = waypoints[i].time
A[cc, 9 + i * 18] = 1
b[cc, 0] = waypoints[i].y
cc += 1 # Z Position
A[cc, 10 + i * 18] = waypoints[i].time ** 4
A[cc, 11 + i * 18] = waypoints[i].time ** 3
A[cc, 12 + i * 18] = waypoints[i].time ** 2
A[cc, 13 + i * 18] = waypoints[i].time
A[cc, 14 + i * 18] = 1
b[cc, 0] = waypoints[i].z
cc += 1 # Yaw Angle
A[cc, 15 + i * 18] = waypoints[i].time ** 2
A[cc, 16 + i * 18] = waypoints[i].time
A[cc, 17 + i * 18] = 1
b[cc, 0] = waypoints[i].yaw
# "end of segment" position constraints
cc += 1 # X Position
A[cc, 0 + i * 18] = waypoints[i + 1].time ** 4
A[cc, 1 + i * 18] = waypoints[i + 1].time ** 3
A[cc, 2 + i * 18] = waypoints[i + 1].time ** 2
A[cc, 3 + i * 18] = waypoints[i + 1].time
A[cc, 4 + i * 18] = 1
b[cc, 0] = waypoints[i + 1].x
cc += 1 # Y Position
A[cc, 5 + i * 18] = waypoints[i + 1].time ** 4
A[cc, 6 + i * 18] = waypoints[i + 1].time ** 3
A[cc, 7 + i * 18] = waypoints[i + 1].time ** 2
A[cc, 8 + i * 18] = waypoints[i + 1].time
A[cc, 9 + i * 18] = 1
b[cc, 0] = waypoints[i + 1].y
cc += 1 # Z Position
A[cc, 10 + i * 18] = waypoints[i + 1].time ** 4
A[cc, 11 + i * 18] = waypoints[i + 1].time ** 3
A[cc, 12 + i * 18] = waypoints[i + 1].time ** 2
A[cc, 13 + i * 18] = waypoints[i + 1].time
A[cc, 14 + i * 18] = 1
b[cc, 0] = waypoints[i + 1].z
cc += 1 # Yaw Angle
A[cc, 15 + i * 18] = waypoints[i + 1].time ** 2
A[cc, 16 + i * 18] = waypoints[i + 1].time
A[cc, 17 + i * 18] = 1
b[cc, 0] = waypoints[i + 1].yaw
# segment rendezvous constraints
if i == 0:
continue
cc += 1 # X Velocity Rendezvous
A[cc, 0 + i * 18] = 4 * waypoints[i].time ** 3
A[cc, 1 + i * 18] = 3 * waypoints[i].time ** 2
A[cc, 2 + i * 18] = 2 * waypoints[i].time
A[cc, 3 + i * 18] = 1
A[cc, 0 + i * 18 - 18] = -1 * A[cc, 0 + i * 18]
A[cc, 1 + i * 18 - 18] = -1 * A[cc, 1 + i * 18]
A[cc, 2 + i * 18 - 18] = -1 * A[cc, 2 + i * 18]
A[cc, 3 + i * 18 - 18] = -1 * A[cc, 3 + i * 18]
cc += 1 # Y Velocity Rendezvous
A[cc, 5 + i * 18] = 4 * waypoints[i].time ** 3
A[cc, 6 + i * 18] = 3 * waypoints[i].time ** 2
A[cc, 7 + i * 18] = 2 * waypoints[i].time
A[cc, 8 + i * 18] = 1
A[cc, 5 + i * 18 - 18] = -1 * A[cc, 5 + i * 18]
A[cc, 6 + i * 18 - 18] = -1 * A[cc, 6 + i * 18]
A[cc, 7 + i * 18 - 18] = -1 * A[cc, 7 + i * 18]
A[cc, 8 + i * 18 - 18] = -1 * A[cc, 8 + i * 18]
cc += 1 # Z Velocity Rendezvous
A[cc, 10 + i * 18] = 4 * waypoints[i].time ** 3
A[cc, 11 + i * 18] = 3 * waypoints[i].time ** 2
A[cc, 12 + i * 18] = 2 * waypoints[i].time
A[cc, 13 + i * 18] = 1
A[cc, 10 + i * 18 - 18] = -1 * A[cc, 10 + i * 18]
A[cc, 11 + i * 18 - 18] = -1 * A[cc, 11 + i * 18]
A[cc, 12 + i * 18 - 18] = -1 * A[cc, 12 + i * 18]
A[cc, 13 + i * 18 - 18] = -1 * A[cc, 13 + i * 18]
cc += 1 # Yaw Velocity Rendezvous
A[cc, 15 + i * 18] = 2 * waypoints[i].time
A[cc, 16 + i * 18] = 1
A[cc, 15 + i * 18 - 18] = -1 * A[cc, 15 + i * 18]
A[cc, 16 + i * 18 - 18] = -1 * A[cc, 16 + i * 18]
# cc += 1 # X Acceleration Rendezvous
# A[cc, 0 + i * 18] = 12 * waypoints[0].time ** 2
# A[cc, 1 + i * 18] = 6 * waypoints[0].time
# A[cc, 2 + i * 18] = 2
# A[cc, 0 + i * 18 - 18] = -1 * A[cc, 0 + i * 18]
# A[cc, 1 + i * 18 - 18] = -1 * A[cc, 1 + i * 18]
# A[cc, 2 + i * 18 - 18] = -1 * A[cc, 2 + i * 18]
# cc += 1 # Y Acceleration Rendezvous
# A[cc, 5 + i * 18] = 12 * waypoints[0].time ** 2
# A[cc, 6 + i * 18] = 6 * waypoints[0].time
# A[cc, 7 + i * 18] = 2
# A[cc, 5 + i * 18 - 18] = -1 * A[cc, 5 + i * 18]
# A[cc, 6 + i * 18 - 18] = -1 * A[cc, 6 + i * 18]
# A[cc, 7 + i * 18 - 18] = -1 * A[cc, 7 + i * 18]
# cc += 1 # Z Acceleration Rendezvous
# A[cc, 10 + i * 18] = 12 * waypoints[0].time ** 2
# A[cc, 11 + i * 18] = 6 * waypoints[0].time
# A[cc, 12 + i * 18] = 2
# A[cc, 10 + i * 18 - 18] = -1 * A[cc, 10 + i * 18]
# A[cc, 11 + i * 18 - 18] = -1 * A[cc, 11 + i * 18]
# A[cc, 12 + i * 18 - 18] = -1 * A[cc, 12 + i * 18]
# cc += 1 # Yaw Acceleration Rendezvous
# A[cc, 15 + i * 18] = 2
# A[cc, 15 + i * 18 - 18] = -1 * A[cc, 15 + i * 18]
# cc += 1 # X Jerk Rendezvous
# A[cc, 0] = 24 * waypoints[0].time
# A[cc, 1] = 6
# A[cc, 0 + i * 18 - 18] = -1 * A[cc, 0 + i * 18]
# A[cc, 1 + i * 18 - 18] = -1 * A[cc, 1 + i * 18]
# cc += 1 # Y Jerk Rendezvous
# A[cc, 5] = 24 * waypoints[0].time
# A[cc, 6] = 6
# A[cc, 5 + i * 18 - 18] = -1 * A[cc, 5 + i * 18]
# A[cc, 6 + i * 18 - 18] = -1 * A[cc, 6 + i * 18]
# cc += 1 # Z Jerk Rendezvous
# A[cc, 10] = 24 * waypoints[0].time
# A[cc, 11] = 6
# A[cc, 10 + i * 18 - 18] = -1 * A[cc, 10 + i * 18]
# A[cc, 11 + i * 18 - 18] = -1 * A[cc, 11 + i * 18]
#
# cc += 1 # X Snap Rendezvous
# A[cc, 0] = 24
# A[cc, 0 + i * 18 - 18] = -1 * A[cc, 0 + i * 18]
# cc += 1 # Y Snap Rendezvous
# A[cc, 5] = 24
# A[cc, 5 + i * 18 - 18] = -1 * A[cc, 5 + i * 18]
# cc += 1 # Z Snap Rendezvous
# A[cc, 10] = 24
# A[cc, 10 + i * 18 - 18] = -1 * A[cc, 10 + i * 18]
cc += 1 # absolute start X velocity
A[cc, 0] = 4 * waypoints[0].time ** 3
A[cc, 1] = 3 * waypoints[0].time ** 2
A[cc, 2] = 2 * waypoints[0].time
A[cc, 3] = 1
cc += 1 # absolute start Y velocity
A[cc, 5] = 4 * waypoints[0].time ** 3
A[cc, 6] = 3 * waypoints[0].time ** 2
A[cc, 7] = 2 * waypoints[0].time
A[cc, 8] = 1
cc += 1 # absolute start Z velocity
A[cc, 10] = 4 * waypoints[0].time ** 3
A[cc, 11] = 3 * waypoints[0].time ** 2
A[cc, 12] = 2 * waypoints[0].time
A[cc, 13] = 1
cc += 1 # absolute start Yaw velocity
A[cc, 15] = 2 * waypoints[0].time
A[cc, 16] = 1
cc += 1 # absolute end X velocity
A[cc, numCoefficients - 18 + 0] = 4 * waypoints[-1].time ** 3
A[cc, numCoefficients - 18 + 1] = 3 * waypoints[-1].time ** 2
A[cc, numCoefficients - 18 + 2] = 2 * waypoints[-1].time
A[cc, numCoefficients - 18 + 3] = 1
cc += 1 # absolute end Y velocity
A[cc, numCoefficients - 18 + 5] = 4 * waypoints[-1].time ** 3
A[cc, numCoefficients - 18 + 6] = 3 * waypoints[-1].time ** 2
A[cc, numCoefficients - 18 + 7] = 2 * waypoints[-1].time
A[cc, numCoefficients - 18 + 8] = 1
cc += 1 # absolute end Z velocity
A[cc, numCoefficients - 18 + 10] = 4 * waypoints[-1].time ** 3
A[cc, numCoefficients - 18 + 11] = 3 * waypoints[-1].time ** 2
A[cc, numCoefficients - 18 + 12] = 2 * waypoints[-1].time
A[cc, numCoefficients - 18 + 13] = 1
#cc += 1 # absolute end Yaw velocity
#A[cc, numCoefficients - 18 + 15] = 2 * waypoints[-1].time
#A[cc, numCoefficients - 18 + 16] = 1
#cc += 1 # absolute start X acceleration
# A[c, 0] = 12 * waypoints[0].time ** 2
# A[c, 1] = 6 * waypoints[0].time
# A[c, 2] = 2
#cc += 1 # absolute start Y acceleration
# A[c, 5] = 12 * waypoints[0].time ** 2
# A[c, 6] = 6 * waypoints[0].time
# A[c, 7] = 2
#cc += 1 # absolute start Z acceleration
# A[cc, 10] = 12 * waypoints[0].time ** 2
# A[cc, 11] = 6 * waypoints[0].time
# A[cc, 12] = 2
#cc += 1 # absolute start Yaw acceleration
# A[cc, 15] = 2
#cc += 1 # absolute end X acceleration
# A[cc, numCoefficients - 18 + 0] = 12 * waypoints[-1].time ** 2
# A[cc, numCoefficients - 18 + 1] = 6 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 2] = 2
#cc += 1 # absolute end Y acceleration
# A[cc, numCoefficients - 18 + 5] = 12 * waypoints[-1].time ** 2
# A[cc, numCoefficients - 18 + 6] = 6 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 7] = 2
#cc += 1 # absolute end Z acceleration
# A[cc, numCoefficients - 18 + 10] = 12 * waypoints[-1].time ** 2
# A[cc, numCoefficients - 18 + 11] = 6 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 12] = 2
#cc += 1 # absolute end Yaw acceleration
# A[cc, numCoefficients - 18 + 15] = 2
#cc += 1 # absolute start X jerk
# A[cc, 0] = 24 * waypoints[0].time
# A[cc, 1] = 6
#cc += 1 # absolute start Y jerk
# A[cc, 5] = 24 * waypoints[0].time
# A[cc, 6] = 6
#cc += 1 # absolute start Z jerk
# A[cc, 10] = 24 * waypoints[0].time
# A[cc, 11] = 6
#cc += 1 # absolute end X jerk
# A[cc, numCoefficients - 18 + 0] = 24 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 1] = 6
#cc += 1 # absolute end Y jerk
# A[cc, numCoefficients - 18 + 5] = 24 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 6] = 6
#cc += 1 # absolute end Z jerk
# A[cc, numCoefficients - 18 + 10] = 24 * waypoints[-1].time
# A[cc, numCoefficients - 18 + 11] = 6
#cc += 1 # absolute start X snap
# A[cc, 0] = 24
#cc += 1 # absolute start Y snap
# A[cc, 5] = 24
#cc += 1 # absolute start Z snap
# A[cc, 10] = 24
#cc += 1 # absolute end X snap
# A[cc, numCoefficients - 18 + 0] = 24
#cc += 1 # absolute end Y snap
# A[cc, numCoefficients - 18 + 5] = 24
#cc += 1 # absolute end Z snap
# A[cc, numCoefficients - 18 + 10] = 24
# =============================
# Solver Setup
# =============================
# OSQP needs:
# P = quadratic terms
# q = linear terms
# A = constraint matrix of ALL constraints (inequality & equality)
# l = lower constraints
# u = upper constraints
P = csc_matrix(P)
q = hstack(q)
h = hstack(h)
b = hstack(b)
A = vstack([G, A])
A = csc_matrix(A)
l = -inf * ones(len(h))
l = hstack([l, b])
u = hstack([h, b])
# setup solver and solve
m = osqp.OSQP()
m.setup(P=P, q=q, A=A, l=l, u=u) # extra solver variables can be set here
res = m.solve()
# save to trajectory variable
for i in range(0, size(res.x), 18):
segment = res.x[i:i + 18]
trajectory.append(segment)
print("QP solution Number following: ", res.x)
return trajectory
def separate(waypoints):
"""
Calculate a trajectory by separate operations.
"""
# every segment has its own polynomial of 4th degree for X,Y and Z and a polynomial of 2nd degree for Yaw
numCoefficients = 3*5+3
# total number of segments
numSegments = len(waypoints) - 1
# list of calculated trajectory coefficients
trajectory = []
for i in range(numSegments):
# X,Y,Z,Yaw position at start and end: 8
# X,Y,Z,Yaw velocity at start: 4
# X,Y,Z acceleration at start: 3
numConstraints = 15
# X,Y,Z velocity at absolute end: 3
# they are initialized as zero, so no changes needed
if i == numSegments-1:
numConstraints += 3
# =============================
# Identity matrix for main part of QP (normally the Hesse matrix (quadratic terms), but this is a least squared problem)
# =============================
P = zeros((numCoefficients, numCoefficients))
P[0, 0] = 1 # minimize snap for X
P[5, 5] = 1 # minimize snap for Y
P[10, 10] = 1 # minimize snap for Z
P[15, 15] = 1 # minimize acceleration for Yaw
# =============================
# Gradient vector (linear terms), we have none
# =============================
q = zeros((numCoefficients, 1))
# =============================
# Inequality matrix (left side), we have none
# =============================
G = zeros((numConstraints, numCoefficients))
# =============================
# Inequality vector (right side), we have none
# =============================
h = zeros((numConstraints, 1))
# =============================
# Equality matrix (left side)
# =============================
A = zeros((numConstraints, numCoefficients))
# X Position Start
A[0, 0] = waypoints[i].time ** 4
A[0, 1] = waypoints[i].time ** 3
A[0, 2] = waypoints[i].time ** 2
A[0, 3] = waypoints[i].time
A[0, 4] = 1
# Y Position Start
A[1, 5] = waypoints[i].time ** 4
A[1, 6] = waypoints[i].time ** 3
A[1, 7] = waypoints[i].time ** 2
A[1, 8] = waypoints[i].time
A[1, 9] = 1
# Z Position Start
A[2, 10] = waypoints[i].time ** 4
A[2, 11] = waypoints[i].time ** 3
A[2, 12] = waypoints[i].time ** 2
A[2, 13] = waypoints[i].time
A[2, 14] = 1
# Yaw Angle Start
A[3, 15] = waypoints[i].time ** 2
A[3, 16] = waypoints[i].time
A[3, 17] = 1
# X Position End
A[4, 0] = waypoints[i + 1].time ** 4
A[4, 1] = waypoints[i + 1].time ** 3
A[4, 2] = waypoints[i + 1].time ** 2
A[4, 3] = waypoints[i + 1].time
A[4, 4] = 1
# Y Position End
A[5, 5] = waypoints[i + 1].time ** 4
A[5, 6] = waypoints[i + 1].time ** 3
A[5, 7] = waypoints[i + 1].time ** 2
A[5, 8] = waypoints[i + 1].time
A[5, 9] = 1
# Z Position End
A[6, 10] = waypoints[i + 1].time ** 4
A[6, 11] = waypoints[i + 1].time ** 3
A[6, 12] = waypoints[i + 1].time ** 2
A[6, 13] = waypoints[i + 1].time
A[6, 14] = 1
# Yaw Angle End
A[7, 15] = waypoints[i + 1].time ** 2
A[7, 16] = waypoints[i + 1].time
A[7, 17] = 1
# X Velocity Start
A[8, 0] = 4 * waypoints[i].time ** 3
A[8, 1] = 3 * waypoints[i].time ** 2
A[8, 2] = 2 * waypoints[i].time
A[8, 3] = 1
# Y Velocity Start
A[9, 5] = 4 * waypoints[i].time ** 3
A[9, 6] = 3 * waypoints[i].time ** 2
A[9, 7] = 2 * waypoints[i].time
A[9, 8] = 1
# Z Velocity Start
A[10, 10] = 4 * waypoints[i].time ** 3
A[10, 11] = 3 * waypoints[i].time ** 2
A[10, 12] = 2 * waypoints[i].time
A[10, 13] = 1
# Yaw Velocity Start
A[11, 15] = 2 * waypoints[i].time
A[11, 16] = 1
# X Acceleration Start
A[12, 0] = 12 * waypoints[i].time ** 2
A[12, 1] = 6 * waypoints[i].time
A[12, 2] = 2
# Y Acceleration Start
A[13, 5] = 12 * waypoints[i].time ** 2
A[13, 6] = 6 * waypoints[i].time
A[13, 7] = 2
# Z Acceleration Start
A[14, 10] = 12 * waypoints[i].time ** 2
A[14, 11] = 6 * waypoints[i].time
A[14, 12] = 2
# Yaw Acceleration Start
#A[15, 15] = 2
# X Jerk Start
#A[16, 0] = 24 * waypoints[i].time
#A[16, 1] = 6
# Y Jerk Start
#A[17, 5] = 24 * waypoints[i].time
#A[17, 6] = 6
# Z Jerk Start
#A[18, 10] = 24 * waypoints[i].time
#A[18, 11] = 6
# X Snap Start
#A[19, 0] = 24
# Y Snap Start
#A[20, 5] = 24
# Z Snap Start
#A[21, 10] = 24
# for full stop at absolute End
if i == numSegments - 1:
# X Velocity End
A[15, 0] = 4 * waypoints[i + 1].time ** 3
A[15, 1] = 3 * waypoints[i + 1].time ** 2
A[15, 2] = 2 * waypoints[i + 1].time
A[15, 3] = 1
# Y Velocity End
A[16, 5] = 4 * waypoints[i + 1].time ** 3
A[16, 6] = 3 * waypoints[i + 1].time ** 2
A[16, 7] = 2 * waypoints[i + 1].time
A[16, 8] = 1
# Z Velocity End
A[17, 10] = 4 * waypoints[i + 1].time ** 3
A[17, 11] = 3 * waypoints[i + 1].time ** 2
A[17, 12] = 2 * waypoints[i + 1].time
A[17, 13] = 1
# =============================
# Equality vector (right side)
# =============================
b = zeros((numConstraints, 1))
b[0, 0] = waypoints[i].x
b[1, 0] = waypoints[i].y
b[2, 0] = waypoints[i].z
b[3, 0] = waypoints[i].yaw
b[4, 0] = waypoints[i+1].x
b[5, 0] = waypoints[i+1].y
b[6, 0] = waypoints[i+1].z
b[7, 0] = waypoints[i+1].yaw
# Derivatives = 0 for absolute Start, else Rendezvous of Segments
if i != 0:
b[8, 0] = 4 * trajectory[-1][0] * waypoints[i].time ** 3 + 3 * trajectory[-1][1] * waypoints[i].time ** 2 + 2 * trajectory[-1][2] * waypoints[i].time + trajectory[-1][3]
b[9, 0] = 4 * trajectory[-1][5] * waypoints[i].time ** 3 + 3 * trajectory[-1][6] * waypoints[i].time ** 2 + 2* trajectory[-1][7] * waypoints[i].time + trajectory[-1][8]
b[10, 0] = 4 * trajectory[-1][10] * waypoints[i].time ** 3 + 3 * trajectory[-1][11] * waypoints[i].time ** 2 + 2 * trajectory[-1][12] * waypoints[i].time + trajectory[-1][13]
b[11, 0] = 2 * trajectory[-1][15] * waypoints[i].time + trajectory[-1][16]
b[12, 0] = 12 * trajectory[-1][0] * waypoints[i].time ** 2 + 6 * trajectory[-1][1] * waypoints[i].time + 2 * trajectory[-1][2]
b[13, 0] = 12 * trajectory[-1][5] * waypoints[i].time ** 2 + 6 * trajectory[-1][6] * waypoints[i].time + 2 * trajectory[-1][7]
b[14, 0] = 12 * trajectory[-1][10] * waypoints[i].time ** 2 + 6 * trajectory[-1][11] * waypoints[i].time + 2 * trajectory[-1][12]
# =============================
# Solver Setup
# =============================
# OSQP needs:
# P = quadratic terms
# q = linear terms
# A = constraint matrix of ALL constraints (inequality & equality)
# l = lower constraints
# u = upper constraints
P = csc_matrix(P)
q = hstack(q)
h = hstack(h)
b = hstack(b)
A = vstack([G, A])
A = csc_matrix(A)
l = -inf * ones(len(h))
l = hstack([l, b])
u = hstack([h, b])
# setup solver and solve
m = osqp.OSQP()
m.setup(P=P, q=q, A=A, l=l, u=u) # extra solver variables can be set here
res = m.solve()
# save to trajectory variable
trajectory.append(res.x)
print("QP solution Number ", i, "following: ", res.x)
return trajectory
def planner(waypoint_arr, isJoint=True):
"""
Starting point of any generation.
Waypoints are given as an array of arrays and transformed into an array of Waypoints.
"""
# test waypoints
# waypoint_arr = []
# waypoint_arr.append([0, 0, 0, 2, 0])
# waypoint_arr.append([1, 5, 0, 4, 3])
# waypoint_arr.append([2, 5, 5, 3, 1])
# waypoint_arr.append([3, 0, 5, 1, 5])
# waypoint_arr.append([4, -5, 0, 2, 4])
waypoints = []
# the given "waypoints" are just the x,y,z,yaw values -> convert them to actual Waypoint objects
for i in range(len(waypoint_arr)):
# calculate time of waypoint
if i == 0:
time = 0
else:
time += calc_time(waypoint_arr[i-1], waypoint_arr[i])
# create and append waypoint
waypoint = Waypoint(waypoint_arr[i][0], waypoint_arr[i][1], waypoint_arr[i][2], waypoint_arr[i][3], time)
waypoints.append(waypoint)
# either calculate jointly or separately
if isJoint:
trajectory = joint(waypoints)
else:
trajectory = separate(waypoints)
# after closing trajectory visualization
return waypoints, trajectory
You can’t perform that action at this time.