def __init__(self, mrobot):
## "Arbitrary" parameters
self.N_s = 15 # nb of samples for discretization
self.n_knot = 4 # nb of non zero lenght intervals of the knot series
self.t_init = 0.0
self.Tc = 0.5
self.Tp = 1.1
tstep = (self.Tp-self.t_init)/(self.N_s-1)
Tc_idx = int(round(self.Tc/tstep))
self.detection_radius = 2.0
## Mobile Robot object
self.mrob = mrobot
## Other parameters
self.d = self.mrob.l+2 # B-spline order (integer | d > l+1)
self.n_ctrlpts = self.n_knot + self.d - 1 # nb of ctrl points
## Constaints values...
## ...for equations:
# initial state
self.q_init = np.matrix([[0.0], [0.0], [np.pi/2]])
# final state
self.q_fin = np.matrix([[2.0], [5.0], [np.pi/2]])
# initial control input
self.u_init = np.matrix([[0.0], [0.0]])
# final control input
self.u_fin = np.matrix([[0.0], [0.0]])
## ...for inequations:
# Control boundary
self.u_abs_max = self.mrob.u_abs_max
# Obstacles (Q_occupied)
# TODO: Obstacles random generation
self.obst_map = np.matrix([0.25, 2.5, 0.2])
self.obst_map = np.append(self.obst_map, np.matrix([2.3, 2.5, 0.5]),
axis = 0)
self.obst_map = np.append(self.obst_map, np.matrix([1.25, 3, 0.1]),
axis = 0)
self.obst_map = np.append(self.obst_map, np.matrix([0.3, 1.0, 0.1]),
axis = 0)
self.obst_map = np.append(self.obst_map, np.matrix([-0.5, 1.5, 0.3]),
axis = 0)
# self.obst_map = np.append(self.obst_map, np.matrix([1.6, 4.3, 0.2]),
# axis = 0)
# max distance within Tp
self.D = self.Tp * self.u_abs_max[0,0]
# Ctrl pts init
C = np.array(np.zeros((self.n_ctrlpts,self.mrob.u_dim)))
C_lower = np.array([-10, -1]*self.n_ctrlpts)
C_upper = np.array([+10, +6]*self.n_ctrlpts)
self.detected_obst_idxs = []
# final trajectory
self.C_ref = []
## Generate initial b-spline knots
self.knots = self._gen_knots(self.t_init, self.Tp)
self.mtime = np.linspace(self.t_init, self.Tp, self.N_s)
## Optimization results
self.unsatisf_eq_values = []
self.unsatisf_ieq_values = []
## Plot initialization
plt.ion()
self.fig = plt.figure()
self.fig_speed = plt.subplots(2)
ax = self.fig.gca()
# creating obstacles' circles
circ = []
for r in range(self.obst_map.shape[0]):
# external dashed circles
circ = circ + \
[plt.Circle((self.obst_map[r,0], self.obst_map[r,1]),
self.obst_map[r,2]+self.mrob.rho,color='k',ls = 'dashed',fill=False)]
# internal continous circles
circ = circ + \
[plt.Circle((self.obst_map[r,0], self.obst_map[r,1]),
self.obst_map[r,2], color='k', fill=False)]
# adding circles to axis
[ax.add_artist(c) for c in circ]
# plot curve and its control points
self.rejected_path,self.plt_ctrl_pts,self.plt_curve,self.plt_dot_curve,self.seg_pts = ax.plot(
0.0, 0.0, 'm:',
0.0, 0.0, '*',
0.0, 0.0, 'b-',
0.0, 0.0, 'g.',
0.0, 0.0, 'rd')
# formating figure
plt.xlabel('x(m)')
plt.ylabel('y(m)')
plt.title('Generated trajectory')
ax.axis('equal')
axarray = self.fig_speed[0].axes
self.plt_linspeed, = axarray[0].plot(0.0, 0.0)
self.plt_angspeed, = axarray[1].plot(0.0, 0.0)
axarray[0].set_ylabel('v(m/s)')
axarray[0].set_title('Linear speed')
axarray[1].set_xlabel('time(s)')
axarray[1].set_ylabel('w(rad/s)')
axarray[1].set_title('Angular speed')
axarray[0].grid()
axarray[1].grid()
final_z = self.mrob.phi0(self.q_fin)
self.last_q = self.q_init
self.last_u = self.u_init
last_z = self.mrob.phi0(self.last_q)
self.all_dz = []
self.all_rejected_z = []
self.itcount = 0
usepyopt = False
while LA.norm(last_z - final_z) > self.D:
# while the remaining dist (straight line) is greater than the max dist during Tp
self.detected_obst_idxs = self._detected_obst_idx(last_z)
# print('No of detected obst: {}'.format(len(self.detected_obst_idxs)))
# print('Detected obst: {}'.format(self.obst_map[self.detected_obst_idxs,:]))
# initiate ctrl points (straight line towards final z)
direc = final_z - last_z
direc = direc/LA.norm(direc)
C[:,0] =np.array(np.linspace(last_z[0,0],\
last_z[0,0]+self.D*direc[0,0], self.n_ctrlpts)).T
C[:,1] =np.array(np.linspace(last_z[1,0],\
last_z[1,0]+self.D*direc[1,0], self.n_ctrlpts)).T
tic = time.time()
if usepyopt:
# Define the optimization problem
self.opt_prob = pyOpt.Optimization(
'Faster path with obstacles', # name of the problem
self._obj_func) # object function (criterium, eq. and ineq.)
self.opt_prob.addObj('J')
self.opt_prob.addVarGroup( # minimization arguments
'C',
self.mrob.u_dim*self.n_ctrlpts, # dimension
'c', # continous
lower=list(C_lower),
value=list(np.squeeze(C.reshape(1,self.n_ctrlpts*self.mrob.u_dim))),
upper=list(C_upper))
self.opt_prob.addConGroup( # equations constraints
'ec',
self.mrob.q_dim + self.mrob.u_dim, # dimension
'e') # equations
self.opt_prob.addConGroup( # inequations constraints
'ic',
self.N_s*self.mrob.u_dim +
self.N_s*len(self.detected_obst_idxs), # dimenstion
'i') # inequations
# solve constrained optmization
# solver = pyOpt.SLSQP(pll_type='POA')
# solver.setOption('ACC', 1e-6)
# solver.setOption('MAXIT', 30)
solver = pyOpt.ALGENCAN(pll_type='POA')
solver.setOption('epsfeas', 1e-1)
solver.setOption('epsopt', 9e-1)
[J, C_aux, information] = solver(self.opt_prob)
C_aux = np.array(C_aux)
# if information.value != 0 and iformation.value != 9:
usepyopt = False
else:
# solve constrained optmization
outp = fmin_slsqp(self._criteria,
np.squeeze(C.reshape(1,self.n_ctrlpts*self.mrob.u_dim)),
eqcons=(),
f_eqcons=self._feqcons,
ieqcons=(),
f_ieqcons=self._fieqcons,
iprint=1,
iter=50,
acc=1e-6,
full_output=True,
callback=self._plot_update)
C_aux = outp[0]
imode = outp[3]
print('\n'.format(imode))
if imode != 0 and imode != 9:
usepyopt = True
continue
print('Elapsed time for {} iteraction: {}'.format(self.itcount, time.time()-tic))
print('No of equations unsatisfied: {}'.format(len(self.unsatisf_eq_values)))
print('Mean and variance of equations unsatisfied: ({},{})'.format(np.mean(self.unsatisf_eq_values), np.std(self.unsatisf_eq_values)))
print('No of inequations unsatisfied: {}'.format(len(self.unsatisf_ieq_values)))
print('Mean and variance of inequations unsatisfied: ({},{})'.format(np.mean(self.unsatisf_ieq_values), np.std(self.unsatisf_ieq_values)))
# test if opt went well
# if yes
C = C_aux
# if no
# continue
C = C.reshape(self.n_ctrlpts, self.mrob.u_dim)
# store ctrl points and [z dz ddz](t)
self.C_ref += [C]
dz = self._comb_bsp(self.mtime[0:Tc_idx], C, 0).T
for dev in range(1,self.mrob.l+1):
dz = np.append(dz,self._comb_bsp(
self.mtime[0:Tc_idx], C, dev).T,axis=0)
self.all_dz += [dz]
rejected_z = self._comb_bsp(self.mtime[Tc_idx:], C, 0).T
self.all_rejected_z += [np.append(np.append(dz[0:2,:], rejected_z, axis=1), np.fliplr(rejected_z), axis=1)]
# update needed values
self.knots = self.knots + self.Tc
self.mtime = [tk+self.Tc for tk in self.mtime]
last_z = self.all_dz[-1][0:self.mrob.u_dim,-1]
# print('Last z: {}', last_z)
# print(self.all_dz[-1][:,-1].reshape(
# self.mrob.l+1, self.mrob.u_dim).T)
self.last_q = self.mrob.phi1(self.all_dz[-1][:,-1].reshape(
self.mrob.l+1, self.mrob.u_dim).T)
self.last_u = self.mrob.phi2(self.all_dz[-1][:,-1].reshape(
self.mrob.l+1, self.mrob.u_dim).T)
#raw_input('paradinha')
self.itcount += 1
#endwhile
self.detected_obst_idxs = self._detected_obst_idx(last_z)
print(self.detected_obst_idxs)
# initiate ctrl points (straight line towards final z)
C[:,0] =np.array(np.linspace(last_z[0,0],\
final_z[0,0], self.n_ctrlpts)).T
C[:,1] =np.array(np.linspace(last_z[1,0],\
final_z[1,0], self.n_ctrlpts)).T
x_aux = np.append(np.asarray([self.Tp]), np.squeeze(C.reshape(1,self.n_ctrlpts*self.mrob.u_dim)), axis=1)
print(x_aux)
self._lstep_plot_update(x_aux)
tic = time.time()
while True:
if False:
# Define the optimization problem
self.opt_prob = pyOpt.Optimization(
'Faster path with obstacles', # name of the problem
self._lstep_obj_func) # object function (criterium, eq. and ineq.)
self.opt_prob.addObj('J')
self.opt_prob.addVarGroup( # minimization arguments
'x',
self.mrob.u_dim*self.n_ctrlpts+1, # dimension
'c', # continous
lower=[0.0]+list(C_lower),
value=[self.Tp]+list(np.squeeze(C.reshape(1,self.n_ctrlpts*self.mrob.u_dim))),
upper=[1e10]+list(C_upper))
self.opt_prob.addConGroup( # equations constraints
'ec',
2*self.mrob.q_dim + 2*self.mrob.u_dim, # dimension
'e') # equations
self.opt_prob.addConGroup( # inequations constraints
'ic',
self.N_s*self.mrob.u_dim +
self.N_s*len(self.detected_obst_idxs), # dimenstion
'i') # inequations
# solve constrained optmization
# solver = pyOpt.SLSQP(pll_type='POA')
# solver.setOption('ACC', 1e-6)
# solver.setOption('MAXIT', 30)
solver = pyOpt.ALGENCAN(pll_type='POA')
solver.setOption('epsfeas', 5e-1)
solver.setOption('epsopt', 9e-1)
[J, x_aux, information] = solver(self.opt_prob)
x_aux = np.array(x_aux)
break
else:
# solve constrained optmization
outp = fmin_slsqp(self._lstep_criteria,
x_aux,
eqcons=(),
f_eqcons=self._lstep_feqcons,
ieqcons=(),
f_ieqcons=self._lstep_fieqcons,
iprint=1,
iter=30,
acc=1e-6,
full_output=True,
callback=self._lstep_plot_update)
x_aux = outp[0]
imode = outp[3]
print('\n'.format(imode))
if imode != 0 and imode != 9:
usepyopt = True
continue
break
print('Elapsed time for {} (last) iteraction: {}'.format(self.itcount, time.time()-tic))
print('No of equations unsatisfied: {}'.format(len(self.unsatisf_eq_values)))
print('Mean and variance of equations unsatisfied: ({},{})'.format(np.mean(self.unsatisf_eq_values), np.std(self.unsatisf_eq_values)))
print('No of inequations unsatisfied: {}'.format(len(self.unsatisf_ieq_values)))
print('Mean and variance of inequations unsatisfied: ({},{})'.format(np.mean(self.unsatisf_ieq_values), np.std(self.unsatisf_ieq_values)))
# test if opt went well
# if yes
dt_final = x_aux[0]
print(x_aux)
self.t_final = self.mtime[0] + dt_final
C = x_aux[1:].reshape(self.n_ctrlpts, self.mrob.u_dim)
# if no
# continue
print('Final time: {}'.format(self.t_final))
# store ctrl points and [z dz ddz](t)
self.C_ref += [C]
self.mtime = np.linspace(self.mtime[0], self.t_final, self.N_s)
dz = self._comb_bsp(self.mtime, C, 0).T
for dev in range(1,self.mrob.l+1):
dz = np.append(dz,self._comb_bsp(
self.mtime, C, dev).T,axis=0)
self.all_dz += [dz]
def Opt():
problem = {}
# Problem Solution Info #
order = 5 # Order should be kept relatively low <=6, if more accuracy is required, increase the number of partitions
N = order-1
problem['N'] = N
problem['nConstraint'] = 10
problem['nDivisions'] = 1 # number of segments, each fitted with its own polynomial of the specified order
# Problem Info #
isp = 290
problem['x0'] = -3200
problem['xf'] = 0
problem['y0'] = 2000
problem['yf'] = 0
problem['u0'] = 625
problem['uf'] = 0
problem['v0'] = -270
problem['vf'] = 0
problem['udotf'] = 0
problem['m0'] = 8500
problem['ve'] = isp*9.81
thrust = 600000
problem['Tmax'] = thrust
problem['Tmin'] = thrust*0.1 # 10% throttle
V0 = (problem['u0']**2 + problem['v0']**2)**0.5
fpa0 = np.arcsin(problem['v0']/V0)
problem['mu0'] = np.pi+fpa0
problem['mudotmax'] = 40*np.pi/180 # 40 deg/s
problem['mudotmin'] = -problem['mudotmax']
# Initial Guess
tf = 12
x = np.linspace(problem['x0'],problem['xf'],order+1)
y = np.linspace(problem['y0'],problem['yf'],order+1)
# tau = -cos(pi*np.arange(0,N+2)/(N+1))
# t = (tau+1)*0.5*tf
# x = interp1d(np.linspace(0,tf,order+1),x)(t)
# y = interp1d(np.linspace(0,tf,order+1),y)(t)
c0 = np.hstack((x[1:-1],y[1:-1],tf))
# Form D
problem['D'] = ChebyshevDiff(order)
opt = pyOpt.Optimization('Flat Pseudospectral PDG',lambda c: Cost(c,problem))
# Add the design variables
for i,xi in enumerate(x[1:-1]):
opt.addVar('x{}'.format(i+1), 'c', lower = problem['x0'], upper = problem['xf'], value = xi)
for i,xi in enumerate(y[1:-1]):
opt.addVar('y{}'.format(i+1), 'c', lower = problem['yf'], upper = problem['y0'], value = xi)
opt.addVar('tf','c', lower = 5, upper = 50, value=tf)
# Add the objective and constraints
opt.addObj('J')
for i in range(1,7):
opt.addCon('g{}'.format(i),'e')
for i in range(1,4*problem['nConstraint'] + 0*order):
opt.addCon('h{}'.format(i),'i')
# optimizer = pyOpt.COBYLA()
# optimizer = pyOpt.ALPSO()
optimizer = pyOpt.ALGENCAN()
# optimizer = pyOpt.SLSQP()
# optimizer = pyOpt.SDPEN()
# optimizer = pyOpt.PSQP()
# optimizer = pyOpt.SOLVOPT()
sens_type = 'CS' # Differencing Type, options ['FD', CS']
# optimizer.setOption('MAXIT',100) #SLSQP option
# optimizer.setOption('MIT',200) # PSQP
# fopt,copt,info = optimizer(opt,sens_type=sens_type)
fopt,copt,info = optimizer(opt)
print info
print opt.solution(0)
t,x,y,u,v,udot,vdot,m,T,mu,mudot = Parse(copt,problem)
plt.figure()
plt.plot(x,y)
plt.title('Positions')
plt.figure()
plt.plot(u,v)
plt.title('Velocities')
plt.figure()
plt.plot(udot,vdot)
plt.title('Accelerations')
plt.figure()
plt.plot(t,m)
plt.title('Mass')
plt.figure()
plt.plot(t,mu*180/np.pi)
plt.title('Thrust Angle')
plt.figure()
plt.plot(t,T)
plt.title('Thrust')
plt.figure()
plt.plot(t,x)
plt.figure()
plt.plot(t,y)
plt.show()
def optimizePyopt(self, solver, sens, options):
# pyOpt requires empty options to be specified as {}, not None
if options is None: options = {}
eqConstr = ('SLSQP' not in solver and 'CONMIN' not in solver
and 'COBYLA' not in solver and 'FILTERSD' not in solver
and 'SDPEN' not in solver)
# Organize constraints
indLinEq = []
indLinIneq = []
indNonlinEq = []
indNonlinIneq = []
for k, bnd in enumerate(self.linBounds):
if bnd[0] == bnd[1] and eqConstr: indLinEq.append(k)
else: indLinIneq.append(k)
indLinEq = np.array(indLinEq)
indLinIneq = np.array(indLinIneq)
for k, bnd in enumerate(self.nonlinBounds):
if bnd[0] == bnd[1] and eqConstr: indNonlinEq.append(k)
else: indNonlinIneq.append(k)
indNonlinEq = np.array(indNonlinEq)
indNonlinIneq = np.array(indNonlinIneq)
# pyOpt objective
def objectivePyopt(xIn, *args, **kwargs):
x = xIn[:self.nvar]
f = self.objective(x)
g = np.zeros(0, dtype=float)
if len(indLinEq) > 0:
g = np.r_[g, np.dot(self.linMat[indLinEq, :], x)]
if len(indNonlinEq) > 0:
g = np.r_[g,
self.evalNonlinConstraints(x, 'constr', indNonlinEq)]
if len(indLinIneq) > 0:
g = np.r_[g, np.dot(self.linMat[indLinIneq, :], x)]
if len(indNonlinIneq) > 0:
g = np.r_[
g,
self.evalNonlinConstraints(x, 'constr', indNonlinIneq)]
fail = 0
if f >= self.inf or np.any(g >= self.inf): fail = 1
return f, g, fail
# pyOpt gradient
def gradientPyopt(xIn, f, g, *args, **kwargs):
x = xIn[:self.nvar]
df = self.gradient(x)
dg = np.zeros((0, self.nvar), dtype=float)
if len(indLinEq) > 0:
dg = np.r_[dg, self.linMat[indLinEq, :]]
if len(indNonlinEq) > 0:
dg = np.r_[dg,
self.evalNonlinConstraints(x, 'jac', indNonlinEq)]
if len(indLinIneq) > 0:
dg = np.r_[dg, self.linMat[indLinIneq, :]]
if len(indNonlinIneq) > 0:
dg = np.r_[dg,
self.evalNonlinConstraints(x, 'jac', indNonlinIneq)]
fail = 0
if f >= self.inf or np.any(g >= self.inf): fail = 1
return df.reshape((1, -1)), dg, fail
# Instantiate optimization problem
optProb = pyOpt.Optimization('pyopt', objectivePyopt)
# Add objective
optProb.addObj('objective')
# Add variables
optProb.addVarGroup('var',
self.nvar,
type='c',
value=self.varInit,
lower=self.varBounds[:, 0],
upper=self.varBounds[:, 1])
# Add constraints
if len(indLinEq) > 0:
optProb.addConGroup('lin-equality',
len(indLinEq),
type='e',
equal=self.linBounds[indLinEq, 0])
if len(indNonlinEq) > 0:
optProb.addConGroup('nonlin-equality',
len(indNonlinEq),
type='e',
equal=self.nonlinBounds[indNonlinEq, 0])
if len(indLinIneq) > 0:
optProb.addConGroup('lin-inequality',
len(indLinIneq),
type='i',
lower=self.linBounds[indLinIneq, 0],
upper=self.linBounds[indLinIneq, 1])
if len(indNonlinIneq) > 0:
optProb.addConGroup('nonlin-inequality',
len(indNonlinIneq),
type='i',
lower=self.nonlinBounds[indNonlinIneq, 0],
upper=self.nonlinBounds[indNonlinIneq, 1])
# Setup solver
if 'SNOPT' in solver:
optimizer = pyOpt.SNOPT(options=options)
if 'SLSQP' in solver:
optimizer = pyOpt.SLSQP(options=options)
if 'CONMIN' in solver:
optimizer = pyOpt.CONMIN(options=options)
if 'ALGENCAN' in solver:
optimizer = pyOpt.ALGENCAN(options=options)
if 'ALPSO' in solver:
optimizer = pyOpt.ALPSO(options=options)
if 'ALHSO' in solver:
optimizer = pyOpt.ALHSO(options=options)
if 'COBYLA' in solver:
optimizer = pyOpt.COBYLA(options=options)
if 'FILTERSD' in solver:
optimizer = pyOpt.FILTERSD(options=options)
if 'KOPT' in solver:
optimizer = pyOpt.KOPT(options=options)
if 'MIDACO' in solver:
optimizer = pyOpt.MIDACO(options=options)
if 'KSQP' in solver:
optimizer = pyOpt.KSQP(options=options)
if 'SDPEN' in solver:
optimizer = pyOpt.SDPEN(options=options)
if 'SOLVOPT' in solver:
optimizer = pyOpt.SOLVOPT(options=options)
# Run optimization
if sens == 'finite-difference':
optimizer(optProb, sens_type='FD')
else:
optimizer(optProb, sens_type=gradientPyopt)
# Extract solution
j = len(optProb._solutions) - 1
xStar = np.zeros(self.nvar)
for k in range(self.nvar):
xStar[k] = optProb._solutions[j].getVar(k).value
return xStar, objectivePyopt(xStar)[0]