def settings(self, QuadDesiredStates: QuadStates):
"""
Do the settings and defined the goal states.
Rerun this function everytime the initial condition or goal states change.
"""
# load environment
self.env = JinEnv.Quadrotor()
self.env.initDyn(Jx=self.QuadPara.inertial_x, Jy=self.QuadPara.inertial_y, Jz=self.QuadPara.inertial_z, \
mass=self.QuadPara.mass, l=self.QuadPara.l, c=self.QuadPara.c)
# set the desired goal states
self.env.initCost_Polynomial(QuadDesiredStates, w_thrust=0.05)
# create UAV optimal control object with time-warping function
self.oc = CPDP.COCSys()
beta = SX.sym('beta')
dyn = beta * self.env.f
self.oc.setAuxvarVariable(vertcat(beta, self.env.cost_auxvar))
self.oc.setStateVariable(self.env.X)
self.oc.setControlVariable(self.env.U)
self.oc.setDyn(dyn)
path_cost = beta * self.env.path_cost
self.oc.setPathCost(path_cost)
self.oc.setFinalCost(self.env.final_cost)
self.oc.setIntegrator(self.n_grid)
# define the loss function and interface function
self.interface_pos_fn = Function('interface', [self.oc.state], [self.oc.state[0:3]])
self.interface_ori_fn = Function('interface', [self.oc.state], [self.oc.state[6:10]])
self.diff_interface_pos_fn = Function('diff_interface', [self.oc.state], [jacobian(self.oc.state[0:3], self.oc.state)])
self.diff_interface_ori_fn = Function('diff_interface', [self.oc.state], [jacobian(self.oc.state[6:10], self.oc.state)])
# initialize some variables for optimization methods, self.oc.n_auxvar is the length of optimization parameter
if (self.optimization_method_str == "Vanilla"):
pass
elif (self.optimization_method_str == "Nesterov"):
# initialization for Nesterov
self.velocity_Nesterov = np.array([0] * self.oc.n_auxvar)
elif (self.optimization_method_str == "Adam"):
# initialization for Adam
self.momentum_vector_adam = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_adam = np.array([0] * self.oc.n_auxvar)
self.momentum_vector_hat_adam = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_hat_adam = np.array([0] * self.oc.n_auxvar)
elif (self.optimization_method_str == "Nadam"):
# initialization for Nadam
self.momentum_vector_nadam = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_nadam = np.array([0] * self.oc.n_auxvar)
self.momentum_vector_hat_nadam = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_hat_nadam = np.array([0] * self.oc.n_auxvar)
elif (self.optimization_method_str == "AMSGrad"):
# initialization for AMSGrad
self.momentum_vector_amsgrad = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_amsgrad = np.array([0] * self.oc.n_auxvar)
self.velocity_vector_hat_amsgrad = np.array([0] * self.oc.n_auxvar)
else:
raise Exception("Wrong optimization method type!")
def settings(self, QuadDesiredStates: QuadStates):
"""
Do the settings and defined the goal states.
Rerun this function everytime the initial condition or goal states change.
"""
# load environment
self.env = JinEnv.Quadrotor()
self.env.initDyn(Jx=self.QuadPara.inertial_x, Jy=self.QuadPara.inertial_y, Jz=self.QuadPara.inertial_z, \
mass=self.QuadPara.mass, l=self.QuadPara.l, c=self.QuadPara.c)
# define the cost function: weights and features
features = vcat([(self.env.X[0])**2, self.env.X[0], \
(self.env.X[1])**2, self.env.X[1], \
(self.env.X[2])**2, self.env.X[2], dot(self.env.U,self.env.U)])
weights = SX.sym('weights', features.shape)
# define the final cost function
self.env.initFinalCost(QuadDesiredStates)
# load the oc solver object
self.oc = LFC.OCSys()
self.oc.setStateVariable(self.env.X)
self.oc.setControlVariable(self.env.U)
dt = 0.1
dyn = self.env.X + dt * self.env.f
self.oc.setDyn(dyn)
self.oc.setPathCost(features=features, weights=weights)
self.oc.setFinalCost(10 * self.env.final_cost)
# initialize sthe MVE solver
self.mve = LFC.MVE()
self.mve.initSearchRegion(x_lb=[0, -8, 0, -8, 0, -8, 0],
x_ub=[1, 8, 1, 8, 1, 8, 0.5])
self.weights_trace = []
self.corrections_trace = []
self.correction_time_trace = []
#!/usr/bin/env python3
import os
import sys
import time
sys.path.append(os.getcwd()+'/CPDP')
sys.path.append(os.getcwd()+'/JinEnv')
sys.path.append(os.getcwd()+'/lib')
import CPDP
import JinEnv
from casadi import *
from scipy.integrate import solve_ivp
import scipy.io as sio
# ---------------------------------------load environment---------------------------------------
env = JinEnv.SinglePendulum()
env.initDyn(l=1, m=1, damping_ratio=0.1)
env.initCost(wu=.01)
# --------------------------- create optimal control object ----------------------------------------
oc = CPDP.COCSys_TimeVarying()
oc.setStateVariable(env.X)
oc.setControlVariable(env.U)
t = SX.sym('t')
oc.setTimeVariable(t)
beta1 = SX.sym('beta1')
beta2 = SX.sym('beta2')
beta3 = SX.sym('beta3')
beta4 = SX.sym('beta4')
#!/usr/bin/env python3
import os
import sys
import time
sys.path.append(os.getcwd() + '/CPDP')
sys.path.append(os.getcwd() + '/JinEnv')
sys.path.append(os.getcwd() + '/lib')
import CPDP
import JinEnv
from casadi import *
# ---------------------------------------load environment-------------------------------
env = JinEnv.RobotArm()
env.initDyn(l1=1, m1=1, l2=1, m2=1, dg=0)
env.initCost_Polynomial(wu=.5)
# --------------------------- create optimal control object --------------------------
oc = CPDP.COCSys()
beta = SX.sym('beta')
dyn = beta * env.f
oc.setAuxvarVariable(vertcat(beta, env.cost_auxvar))
oc.setStateVariable(env.X)
oc.setControlVariable(env.U)
oc.setDyn(dyn)
path_cost = beta * env.path_cost
oc.setPathCost(path_cost)
oc.setFinalCost(env.final_cost)
oc.setIntegrator(n_grid=30)
# --------------------------- set initial condition and horizon ------------------------------
ini_state = [-pi / 2, 0, 0, 0]
#!/usr/bin/env python3
import os
import sys
import time
sys.path.append(os.getcwd()+'/CPDP')
sys.path.append(os.getcwd()+'/JinEnv')
sys.path.append(os.getcwd()+'/lib')
import CPDP
import JinEnv
from casadi import *
from scipy.integrate import solve_ivp
# ---------------------------------------load environment---------------------------------------
env = JinEnv.Rocket()
env.initDyn(Jx=1, Jy=1, Jz=1, mass=1, l=1)
env.initCost2(wthrust=0.1)
# --------------------------- create OC object----------------------------------------
oc = CPDP.COCSys()
beta = SX.sym('beta')
dyn = beta * env.f
oc.setAuxvarVariable(vertcat(beta, env.cost_auxvar))
oc.setStateVariable(env.X)
oc.setControlVariable(env.U)
oc.setDyn(dyn)
path_cost = beta * env.path_cost
oc.setPathCost(path_cost)
oc.setFinalCost(env.final_cost)
oc.setIntegrator(n_grid=15)