def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
def plot_arrows(name, ax, x, y, phi, theta):
x_vec = ca.cos(theta)*ca.cos(phi)
y_vec = ca.cos(theta)*ca.sin(phi)
ax.quiver(x, y, x_vec, y_vec,
units='xy', angles='xy', scale=1, width=0.1,
headwidth=4, headlength=6, headaxislength=4,
color='r', lw='0.1')
return [Patch(color='red', label=name)]
def setTrajectory(ocp, t0):
tk = t0
for k in range(N):
ocp.y[k,0] = A*C.sin(omega*tk)
ocp.y[k,1] = A*C.cos(omega*tk)*omega
tk += ts
ocp.yN[0] = A*C.sin(omega*tk)
ocp.yN[1] = A*C.cos(omega*tk)*omega
refLog.append(numpy.copy(numpy.vstack((ocp.y[:,:2], ocp.yN.T))))
def _plot_arrows_3D(name, ax, x, y, phi, psi):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
psi = ca.veccat(psi)
x_vec = ca.cos(psi) * ca.cos(phi)
y_vec = ca.cos(psi) * ca.sin(phi)
z_vec = ca.sin(psi)
ax.quiver(x + x_vec, y + y_vec, z + z_vec,
x_vec, y_vec, z_vec,
color='r', alpha=0.8)
return [Patch(color='red', label=name)]
def plot_arrows3D(name, ax, x, y, phi, theta):
x = ca.veccat(x)
y = ca.veccat(y)
z = ca.DMatrix.zeros(x.size())
phi = ca.veccat(phi)
theta = ca.veccat(theta)
x_vec = ca.cos(theta)*ca.cos(phi)
y_vec = ca.cos(theta)*ca.sin(phi)
z_vec = ca.sin(theta)
length = 2.0
ax.quiver(x+length*x_vec, y+length*y_vec, z+length*z_vec,
x_vec, y_vec, z_vec,
length=length, arrow_length_ratio=0.5)
return [Patch(color='red', label=name)]
def quaternion_rpy(roll, pitch, yaw):
"""Returns a quaternion ([x,y,z,w], w scasadi.ar) from roll pitch yaw ZYX
convention."""
cr = cs.cos(roll/2.0)
sr = cs.sin(roll/2.0)
cp = cs.cos(pitch/2.0)
sp = cs.sin(pitch/2.0)
cy = cs.cos(yaw/2.0)
sy = cs.sin(yaw/2.0)
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
w = cr * cp * cy + sr * sp * sy
# Remember to normalize:
nq = cs.sqrt(x*x + y*y + z*z + w*w)
return cs.vertcat(w/nq, x/nq, y/nq, z/nq)
def axis_angle_to_rotation(axis, angle):
# Source:
# https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
res = cs.MX.eye(3)
res += cs.sin(angle) * cs.skew(axis)
res += (1 - cs.cos(angle)) * cs.mtimes(cs.skew(axis), cs.skew(axis))
return res
def robust_control_stages(ocp, delta, t0):
"""
Returns
-------
stage: :obj: `~rockit.stage.Stage
x : :obj: `~casadi.MX
"""
stage = ocp.stage(t0=t0, T=1)
x = stage.state(2)
u = stage.state()
der_state = vertcat(x[1],
-0.1 * (1 - x[0]**2 + delta) * x[1] - x[0] + u + delta)
stage.set_der(x, der_state)
stage.set_der(u, 0)
stage.subject_to(u <= 40)
stage.subject_to(u >= -40)
stage.subject_to(x[0] >= -0.25)
L = stage.variable()
stage.add_objective(L)
stage.subject_to(L >= sumsqr(x[0] - 3))
bound = lambda t: 2 + 0.1 * cos(10 * t)
stage.subject_to(x[0] <= bound(stage.t))
stage.method(MultipleShooting(N=20, M=1, intg='rk'))
return stage, x, stage.at_t0(u)
def proc_model(self):
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
g = c.MX(self.nx,self.nu)
g[0,0] = c.cos(self.x[2]); g[0,1] = 0;
g[1,0] = c.sin(self.x[2]); g[1,1] = 0;
g[2,0] = c.tan(self.x[3])/self.length; g[2,1] = 0
g[3,0] = 0; g[3,1] = 1;
# f = c.Function('f',[self.x,self.u],[self.x[0] + self.u[0]*c.cos(self.x[2])*self.dt,
# self.x[1] + self.u[0]*c.sin(self.x[2])*self.dt,
# self.x[2] + self.u[0]*c.tan(self.x[3])*self.dt/self.length,
# self.x[3] + self.u[1]*self.dt])
f = c.Function('f',[self.x,self.u],[self.x + c.mtimes(g,self.u)*self.dt])
# A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.x),
# c.jacobian(f(self.x,self.u)[1],self.x),
# c.jacobian(f(self.x,self.u)[2],self.x),
# c.jacobian(f(self.x,self.u)[3],self.x)]) #linearization
# B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u)[0],self.u),
# c.jacobian(f(self.x,self.u)[1],self.u),
# c.jacobian(f(self.x,self.u)[2],self.u),
# c.jacobian(f(self.x,self.u)[3],self.u)])
A = c.Function('A',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.x)])
B = c.Function('B',[self.x,self.u],[c.jacobian(f(self.x,self.u),self.u)])
return f,A, B
def rotation_matrix_from_axis_angle(axis, angle):
"""
Conversion of unit axis and angle to 4x4 rotation matrix according to:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
:param axis: 3x1 Matrix
:type axis: Matrix
:type angle: Union[float, Symbol]
:return: 4x4 Matrix
:rtype: Matrix
"""
ct = ca.cos(angle)
st = ca.sin(angle)
vt = 1 - ct
m_vt_0 = vt * axis[0]
m_vt_1 = vt * axis[1]
m_vt_2 = vt * axis[2]
m_st_0 = axis[0] * st
m_st_1 = axis[1] * st
m_st_2 = axis[2] * st
m_vt_0_1 = m_vt_0 * axis[1]
m_vt_0_2 = m_vt_0 * axis[2]
m_vt_1_2 = m_vt_1 * axis[2]
return Matrix(
[[ct + m_vt_0 * axis[0], -m_st_2 + m_vt_0_1, m_st_1 + m_vt_0_2, 0],
[m_st_2 + m_vt_0_1, ct + m_vt_1 * axis[1], -m_st_0 + m_vt_1_2, 0],
[-m_st_1 + m_vt_0_2, m_st_0 + m_vt_1_2, ct + m_vt_2 * axis[2], 0],
[0, 0, 0, 1]])
def inverted_pendulum_nonlinear_ode(x, u):
M = 2.4
m = 0.23
l = 0.36
g = 9.81
dx1_dt = x[1]
dx2_dt = (u * ca.cos(x[0]) -
(M + m) * g * ca.sin(x[0]) + m * l * ca.cos(x[0]) *
ca.sin(x[0]) * x[1]**2) / (m * l * ca.cos(x[0])**2 - (M + m) * l)
dx3_dt = x[3]
dx4_dt = (u + m * l * ca.sin(x[0]) * x[1]**2 - m * g * ca.cos(x[0]) *
ca.sin(x[0])) / (M + m - m * ca.cos(x[0])**2)
rhs = [dx1_dt, dx2_dt, dx3_dt, dx4_dt]
return ca.vertcat(*rhs)
def _create_continuous_dynamics(self):
# Unpack arguments
[x_b, y_b, z_b, vx_b, vy_b, vz_b,
x_c, y_c, vx_c, vy_c, phi, psi] = self.x[...]
[F_c, w_phi, w_psi, theta] = self.u[...]
# Define the governing ordinary differential equation (ODE)
rhs = cat.struct_SX(self.x)
rhs['x_b'] = vx_b
rhs['y_b'] = vy_b
rhs['z_b'] = vz_b
rhs['vx_b'] = 0
rhs['vy_b'] = 0
rhs['vz_b'] = -self.g
rhs['x_c'] = vx_c
rhs['y_c'] = vy_c
rhs['vx_c'] = F_c * ca.cos(phi + theta) - self.mu * vx_c
rhs['vy_c'] = F_c * ca.sin(phi + theta) - self.mu * vy_c
rhs['phi'] = w_phi
rhs['psi'] = w_psi
op = {'input_scheme': ['x', 'u'],
'output_scheme': ['x_dot']}
return ca.SXFunction('Continuous dynamics',
[self.x, self.u], [rhs], op)
def __init__(self, N=30, step=0.01):
# We construct the model as a set of differential-algebraic equations (DAE)
self.dae = casadi.DaeBuilder()
# Parameters
self.n = 4
self.m = 2 # controls
self.N = N
self.step = step
self.T = N * step # Time horizon (seconds)
self.u_upper = 2.5
self.fixed_points = dict() # None yet
# Constants
self.lr = 2.10 # distance from CG to back wheel in meters
self.lf = 2.67
# Source, page 40: https://www.diva-portal.org/smash/get/diva2:860675/FULLTEXT01.pdf
# States
z = self.dae.add_x('z', self.n)
x = z[0]
y = z[1]
v = z[2]
psi = z[3]
self.STATE_NAMES = [
'x',
'y',
'v',
'psi',
]
# Controls
u = self.dae.add_u('u', self.m) # acceleration
a = u[0]
delta_f = u[1] # front steering angle
# This is a weird "state".
beta = casadi.arctan(self.lr / (self.lr + self.lf) *
casadi.tan(delta_f))
self.CONTROL_NAMES = ['a', 'delta_f']
# Define ODEs
xdot = v * casadi.cos(psi + beta)
ydot = v * casadi.sin(psi + beta)
vdot = a
psidot = v / self.lr * casadi.sin(beta)
zdot = casadi.vertcat(xdot, ydot, vdot, psidot)
self.dae.add_ode('zdot', zdot)
# Customize Matplotlib:
mpl.rcParams['font.size'] = 18
mpl.rcParams['lines.linewidth'] = 3
mpl.rcParams['axes.grid'] = True
self.state_estimate = None
self.control_estimate = np.zeros((self.m, self.N))
def generate_griewank(n=2, a=1., b=4000., func_opts={}, data_type=cs.SX):
x = data_type.sym("x", n)
product_part = 1.
for i in range(n):
product_part = product_part * cs.cos(x[i] / cs.sqrt(i + 1))
f = a + (1. / b) * cs.sumsqr(x) - product_part
func = cs.Function("griewank", [x], [f], ["x"], ["f"], func_opts)
return func, [[-600., 600.]] * n, [[0.] * n]
def rotation_z(angle):
"""Returns a rotation matrix for a left-handed rotation around the z-axis by the given angle."""
c, s = cs.cos(angle), cs.sin(angle)
R = cs.SX.zeros(3, 3)
R[0,0],R[0,1],R[0,2] = c, s, 0
R[1,0],R[1,1],R[1,2] = -s, c, 0
R[2,0],R[2,1],R[2,2] = 0, 0, 1
return R
def Cd_wave_Korn(Cl, t_over_c, mach, sweep=0, kappa_A=0.95):
"""
Wave drag_force coefficient prediction using the (very) low-fidelity Korn Equation method; derived in "Configuration Aerodynamics" by W.H. Mason, Sect. 7.5.2, pg. 7-18
:param Cl: (2D) lift_force coefficient
:param t_over_c: thickness-to-chord ratio
:param sweep: sweep angle, in degrees
:param kappa_A: Airfoil technology factor (0.95 for supercritical section, 0.87 for NACA 6-series)
:return: Wave drag coefficient
"""
mach = cas.fmax(mach, 0)
sweep_rad = np.pi / 180 * sweep
Mdd = kappa_A / cas.cos(sweep_rad) - t_over_c / cas.cos(sweep_rad) ** 2 - Cl / (10 * cas.cos(sweep_rad) ** 3)
Mcrit = Mdd - (0.1 / 80) ** (1 / 3)
# Cd_wave = 20 * cas.ramp(mach - Mcrit) ** 4
Cd_wave = 20 * cas.if_else(mach > Mcrit, mach - Mcrit, 0) ** 4
return Cd_wave
def continuous_dynamics(state, control):
# Unpack arguments
[x_b,y_b,vx_b,vy_b,x_c,y_c,phi] = state[...]
[v,w,psi] = control[...]
# Define right-hand side
rhs = cat.struct_SX(state)
rhs['x_b'] = vx_b
rhs['y_b'] = vy_b
rhs['vx_b'] = 0
rhs['vy_b'] = 0
rhs['x_c'] = v * (ca.cos(psi) * ca.cos(phi) - \
ca.sin(psi) * ca.sin(phi))
rhs['y_c'] = v * (ca.cos(psi) * ca.sin(phi) + \
ca.sin(psi) * ca.cos(phi))
rhs['phi'] = w
return ca.SXFunction('Continuous dynamics',[state,control],[rhs])
def step_sym(self, t, h, x, u, w):
thetadot = u[1]
v = u[0]
theta_out = x[2] + thetadot * h + w[2]
theta_average = 0.5 * (theta_out + x[2])
x_out = x[0] + h * v * cs.cos(theta_average) + w[0]
y_out = x[1] + h * v * cs.sin(theta_average) + w[1]
return cs.vertcat(x_out, y_out, theta_out)
def _plot_arrows(name, ax, x, y, phi):
x_vec = ca.cos(phi)
y_vec = ca.sin(phi)
ax.quiver(x, y, x_vec, y_vec,
units='xy', angles='xy', scale=1, width=0.08,
headwidth=4, headlength=6, headaxislength=5,
color='r', alpha=0.8, lw=0.1)
return [Patch(color='red', label=name)]
def continuous_dynamics(state, control):
# Unpack arguments
[x_b, y_b, z_b, vx_b, vy_b, vz_b, x_c, y_c, phi] = state[...]
[v, w, psi] = control[...]
# Define right-hand side
rhs = cat.struct_SX(state)
rhs["x_b"] = vx_b
rhs["y_b"] = vy_b
rhs["z_b"] = vz_b
rhs["vx_b"] = 0
rhs["vy_b"] = 0
rhs["vz_b"] = -g0
rhs["x_c"] = v * (ca.cos(psi) * ca.cos(phi) - ca.sin(psi) * ca.sin(phi))
rhs["y_c"] = v * (ca.cos(psi) * ca.sin(phi) + ca.sin(psi) * ca.cos(phi))
rhs["phi"] = w
return ca.SXFunction("Continuous dynamics", [state, control], [rhs])
def from_euler(cls, e):
assert e.shape == (3, 1) or e.shape == (3, )
q = ca.SX(4, 1)
cosPhi_2 = ca.cos(e[0] / 2)
cosTheta_2 = ca.cos(e[1] / 2)
cosPsi_2 = ca.cos(e[2] / 2)
sinPhi_2 = ca.sin(e[0] / 2)
sinTheta_2 = ca.sin(e[1] / 2)
sinPsi_2 = ca.sin(e[2] / 2)
q[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + \
sinPhi_2 * sinTheta_2 * sinPsi_2
q[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - \
cosPhi_2 * sinTheta_2 * sinPsi_2
q[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + \
sinPhi_2 * cosTheta_2 * sinPsi_2
q[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - \
sinPhi_2 * sinTheta_2 * cosPsi_2
return q
def kinematics(self, state, U, epsilon=0):
f,_,_ = self.proc_model()
w0 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[0,0]))
w1 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[1,1]))
w2 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[2,2]))
w3 = epsilon*np.random.normal(0,np.sqrt(self.Sigma_w[3,3]))
w = c.DM([[w0],[w1],[w2],[w3]])
state_n = f(state,c.blockcat([[U[0] + w[0]],[U[1] + w[1]],[U[2] + w[2]],[U[3] + w[3]]]))
state_n[6] = c.atan2(c.sin(state_n[6]),c.cos(state_n[6]))
state_n[7] = c.atan2(c.sin(state_n[7]),c.cos(state_n[7]))
state_n[8] = c.atan2(c.sin(state_n[8]),c.cos(state_n[8]))
return state_n
def get_link_relative_transform_casadi(self, qi):
""" Link transform according to the Denavit-Hartenberg convention.
Casadi compatible function.
"""
if self.joint_type == JointType.revolute:
a, alpha, d, theta = self.dh.a, self.dh.alpha, self.dh.d, qi
elif self.joint_type == JointType.prismatic:
a, alpha, d, theta = self.dh.a, self.dh.alpha, qi, self.dh.theta
c_t, s_t = ca.cos(theta), ca.sin(theta)
c_a, s_a = ca.cos(alpha), ca.sin(alpha)
row1 = ca.hcat([c_t, -s_t * c_a, s_t * s_a, a * c_t])
row2 = ca.hcat([s_t, c_t * c_a, -c_t * s_a, a * s_t])
row3 = ca.hcat([0, s_a, c_a, d])
row4 = ca.hcat([0, 0, 0, 1])
return ca.vcat([row1, row2, row3, row4])
def generate_bartelsconn(n=2, func_opts={}, data_type=cs.SX):
if n != 2:
raise ValueError("bartelsconn is only defined for n=2")
x = data_type.sym("x", n)
f = cs.norm_1(cs.sumsqr(x) + x[0] * x[1])
f += cs.norm_1(cs.sin(x[0]))
f += cs.norm_1(cs.cos(x[1]))
func = cs.Function("bartelsconn", [x], [f], ["x"], ["f"], func_opts)
return func, [[-500., 500.]] * n, [[0.] * n]
def prediction_state(x0, u, T, N):
# define predition horizon function
states_ = np.zeros((N + 1, 3))
states_[0, :] = x0
for i in range(N):
states_[i + 1, 0] = states_[i, 0] + u[i, 0] * ca.cos(states_[i, 2]) * T
states_[i + 1, 1] = states_[i, 1] + u[i, 0] * ca.sin(states_[i, 2]) * T
states_[i + 1, 2] = states_[i, 2] + u[i, 1] * T
return states_
def __init__(self):
self.opti = ca.Opti()
self.N = 200
self.d = 2.0
self.dmax = 20.0
self.umax = 20.0
self.pi = -np.pi
self.l = 0.5
self.m1 = 0.5
self.m2 = 0.1
self.g = -9.81
self.T = 2.0
self.h = self.T / self.N
goal1 = self.opti.parameter()
goal2 = self.opti.parameter()
goal3 = self.opti.parameter()
goal4 = self.opti.parameter()
self.opti.set_value(goal1, self.d)
self.opti.set_value(goal2, self.pi)
self.opti.set_value(goal3, 0)
self.opti.set_value(goal4, 0)
self.goal = [goal1, goal2, goal3, goal4]
self.q1 = self.opti.variable(self.N)
self.q2 = self.opti.variable(self.N)
self.q1_dot = self.opti.variable(self.N)
self.q2_dot = self.opti.variable(self.N)
self.u = self.opti.variable(self.N)
self.q1_ddot = (
((self.l * self.m2 * ca.sin(self.q2) * (self.q2**2)) + (self.u) +
(self.m2 * self.g * ca.cos(self.q2) * ca.sin(self.q2))) /
(self.m1 + self.m2 * (ca.sin(self.q2)**2)))
self.q2_ddot = (
((self.l * self.m2 * ca.cos(self.q2) * ca.sin(self.q2) *
(self.q2_dot**2)) + (self.u * ca.cos(self.q2)) +
((self.m1 + self.m2) * self.g * ca.sin(self.q2))) /
(self.m1 * self.l + self.l * self.m2 * (ca.sin(self.q2)**2)))
self.state = ca.horzcat(self.q1, self.q2, self.q1_dot, self.q2_dot)
self.model = ca.horzcat(self.q1_dot, self.q2_dot, self.q1_ddot,
self.q2_ddot)
def buildDynamicalSystem(self):
g = 9.8
x = ca.MX.sym('x')
dx = ca.MX.sym('dx')
theta = ca.MX.sym('theta')
dtheta = ca.MX.sym('dtheta')
u = ca.MX.sym('u')
states = ca.vertcat(x, dx, theta, dtheta);
controls = u;
param1 = ca.MX.sym('param_1')
param2 = ca.MX.sym('param_2')
param3 = ca.MX.sym('param_3')
params = ca.vertcat(param1, param2, param3)
print("params_shape", params.shape)
# build matrix for mass matrix
phi_1_1 = ca.MX(2, 3)
phi_1_1[0, 0] = ca.MX.ones(1)
phi_1_1[1, 1] = ca.cos(theta)
phi_1_2 = ca.MX(2, 3)
phi_1_2[0, 1] = ca.cos(theta)
phi_1_2[1, 2] = 4/3*ca.MX.ones(1)
mass_matrix = ca.horzcat(ca.mtimes(phi_1_1, params), ca.mtimes(phi_1_2, params))
phi_2 = ca.MX(2, 3)
phi_2[0, 1] = -1 * ca.sin(theta) * dtheta * dtheta
phi_2[1, 1] = -g * ca.sin(theta)
forces = ca.mtimes(phi_2, params)
actions = ca.vertcat(controls, ca.MX.zeros(1))
b = actions - forces
states_dd = ca.solve(mass_matrix, b)
states_d = ca.vertcat(dx, states_dd[0], dtheta, states_dd[1])
return states, states_d, controls, params
def _add_constraints(self):
# State Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.V_MIN, self.v_dv, self.V_MAX))
# Initial State Constraint
self.opti.subject_to(self.x_dv[0] == self.z_curr[0])
self.opti.subject_to(self.y_dv[0] == self.z_curr[1])
self.opti.subject_to(self.psi_dv[0] == self.z_curr[2])
self.opti.subject_to(self.v_dv[0] == self.z_curr[3])
# State Dynamics Constraints
for i in range(self.N):
beta = casadi.atan(self.L_R / (self.L_F + self.L_R) *
casadi.tan(self.df_dv[i]))
self.opti.subject_to(
self.x_dv[i + 1] == self.x_dv[i] + self.DT *
(self.v_dv[i] * casadi.cos(self.psi_dv[i] + beta)))
self.opti.subject_to(
self.y_dv[i + 1] == self.y_dv[i] + self.DT *
(self.v_dv[i] * casadi.sin(self.psi_dv[i] + beta)))
self.opti.subject_to(self.psi_dv[i +
1] == self.psi_dv[i] + self.DT *
(self.v_dv[i] / self.L_R * casadi.sin(beta)))
self.opti.subject_to(self.v_dv[i + 1] == self.v_dv[i] + self.DT *
(self.acc_dv[i]))
# Input Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_MIN, self.acc_dv, self.A_MAX))
self.opti.subject_to(
self.opti.bounded(self.DF_MIN, self.df_dv, self.DF_MAX))
# Input Rate Bound Constraints
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[0],
self.acc_dv[0] - self.u_prev[0],
self.A_DOT_MAX + self.sl_acc_dv[0]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[0],
self.df_dv[0] - self.u_prev[1],
self.DF_DOT_MAX + self.sl_df_dv[0]))
for i in range(self.N - 1):
self.opti.subject_to(
self.opti.bounded(self.A_DOT_MIN - self.sl_acc_dv[i + 1],
self.acc_dv[i + 1] - self.acc_dv[i],
self.A_DOT_MAX + self.sl_acc_dv[i + 1]))
self.opti.subject_to(
self.opti.bounded(self.DF_DOT_MIN - self.sl_df_dv[i + 1],
self.df_dv[i + 1] - self.df_dv[i],
self.DF_DOT_MAX + self.sl_df_dv[i + 1]))
# Other Constraints
self.opti.subject_to(0 <= self.sl_df_dv)
self.opti.subject_to(0 <= self.sl_acc_dv)
def constrainInvariants(ocp):
R_n2b = ocp.lookup('R_n2b',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))
# constrain line angle
for k in range(0,nk):
for j in range(0,ocp.deg+1):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k,degIdx=j),'>=',C.cos(65*pi/180), tag=('line angle',k))
def dynamics(self, x, u):
# state x = [x,y, vx, vy]
x_next = x[0] + self.dt * cos(x[3]) * x[2]
y_next = x[1] + self.dt * sin(x[3]) * x[2]
v_next = x[2] + self.dt * u[0]
theta_next = x[3] + self.dt * u[1]
state_next = [x_next, y_next, v_next, theta_next]
return state_next
def dynamics(self, x, u):
# Get states
xPos = x[0]
yPos = x[1]
xVel = x[2]
yVel = x[3]
ang = x[4]
angVel = x[5]
# Get controls
thrust_percentage = u[0]
thrust_angle = u[1]
# Get disturbing wind force (only in horizontal direction)
force_wind = u[2]
# Compute some intermediate values
thrust_magnitude = thrust_percentage * self.maxThrust
sa = cas.sin(ang)
ca = cas.cos(ang)
st = cas.sin(thrust_angle)
ct = cas.cos(thrust_angle)
# == LINEAR FORCES ==
force_gravity = -self.m * self.g
force_thrust = thrust_magnitude * cas.vertcat(ct * sa - st * ca,
ct * ca + st * sa)
# Compute the linear accelerations
xAcc = (force_thrust[0] + force_wind) / self.m
yAcc = (force_thrust[1] + force_gravity) / self.m
# == TORQUES ==
torque_wind = self.l * ca * force_wind
torque_thrust = self.L * st * thrust_magnitude
# Compute the angular acceleration
angAcc = (torque_thrust + torque_wind) / self.I
# Stack the derivatives
xdot = cas.vertcat(xVel, yVel, xAcc, yAcc, angVel, angAcc)
return xdot
def doit():
# state vector is (x, y, theta)
x = cs.SX.sym('x', 3)
# control vector is (v, omega)
u = cs.SX.sym('u', 2)
# explicit variables for readability
v = u[0]
omega = u[1]
theta = x[2]
# unicycle kinematic model
xdot = cs.vertcat(v*cs.cos(theta),
v*cs.sin(theta),
omega)
# intermediate cost, final cost, final constraint
l = cs.sumsqr(u)
xf_des = np.array([0, 1, 0])
lf = 1000.0 * cs.sumsqr(x - xf_des)
hf = x - xf_des
# create solver
solver = ilqr.IterativeLQR(x=x,
u=u,
xdot=xdot,
dt=0.1,
N=100,
intermediate_cost=l,
final_cost=lf,
final_constraint=None)
# solve
solver.randomizeInitialGuess()
solver.solve(50)
plt.figure()
plt.title('Total cost')
plt.semilogy(solver._dcost, '--s')
plt.grid()
plt.figure()
plt.title('State trajectory')
plt.plot(solver._state_trj)
plt.legend(['x', 'y', 'theta'])
plt.grid()
plt.figure()
plt.title('Control trajectory')
plt.plot(solver._ctrl_trj)
plt.legend(['v', 'omega'])
plt.grid()
plt.show()
def continuous_dynamics(x, u, p):
#extract states and inputs
posx = x[zvars.index('posx')-ninputs]
posy = x[zvars.index('posy')-ninputs]
phi = x[zvars.index('phi')-ninputs]
vx = x[zvars.index('vx')-ninputs]
vy = x[zvars.index('vy')-ninputs]
omega = x[zvars.index('omega')-ninputs]
d = x[zvars.index('d')-ninputs]
delta = x[zvars.index('delta')-ninputs]
theta = x[zvars.index('theta')-ninputs]
ddot = u[zvars.index('ddot')]
deltadot = u[zvars.index('deltadot')]
thetadot = u[zvars.index('thetadot')]
#build CasADi expressions for dynamic model
#front lateral tireforce
alphaf = -casadi.atan2((omega*lf + vy), vx) + delta
Ffy = Df*casadi.sin(Cf*casadi.atan(Bf*alphaf))
#rear lateral tireforce
alphar = casadi.atan2((omega*lr - vy),vx)
Fry = Dr*casadi.sin(Cr*casadi.atan(Br*alphar))
#rear longitudinal forces
Frx = (Cm1-Cm2*vx) * d - Croll -Cd*vx*vx
#let z = [u, x] = [ddot, deltadot, thetadot, posx, posy, phi, vx, vy, omega, d, delta, theta]
statedot = np.array([
vx * casadi.cos(phi) - vy * casadi.sin(phi), #posxdot
vx * casadi.sin(phi) + vy * casadi.cos(phi), #posydot
omega, #phidot
1/m * (Frx - Ffy*casadi.sin(delta) + m*vy*omega), #vxdot
1/m * (Fry + Ffy*casadi.cos(delta) - m*vx*omega), #vydot
1/Iz * (Ffy*lf*casadi.cos(delta) - Fry*lr), #omegadot
ddot,
deltadot,
thetadot
])
return statedot
def exp(cls, v):
assert v.shape == (3, 1) or q.shape == (3, )
q = ca.SX(4, 1)
theta = ca.norm_2(v)
q[0] = ca.cos(theta / 2)
c = ca.sin(theta / 2)
n = ca.norm_2(v)
q[1] = c * v[0] / n
q[2] = c * v[1] / n
q[3] = c * v[2] / n
return ca.if_else(n > eps, q, ca.SX([1, 0, 0, 0]))
def mobile_robot_ode(t, x, u, p, z):
"""
Mobile robot
"""
# Parameter configuratio
dx_dt = u[0] * ca.cos(x[2])
dy_dt = u[0] * ca.sin(x[2])
dtheta_dt = u[1]
rhs = [dx_dt, dy_dt, dtheta_dt]
return ca.vertcat(*rhs)
def generate_salomon(n=2,
a=1.,
b=2 * cs.np.pi,
c=0.1,
func_opts={},
data_type=cs.SX):
x = data_type.sym("x", n)
sumsq = cs.sumsqr(x)
f = a - cs.cos(b * cs.sqrt(sumsq)) + c * cs.sqrt(sumsq)
func = cs.Function("salomon", [x], [f], ["x"], ["f"], func_opts)
return func, [[-10., 10.]] * n, [[0.] * n]
def init(self, horizon_times=None):
ObstaclexD.init(self, horizon_times=horizon_times)
if self.signals['angular_velocity'][:, -1] == 0.:
self.cos = cos(self.signals['orientation'][:, -1][0])
self.sin = sin(self.signals['orientation'][:, -1][0])
self.gon_weight = 1.
return
# theta, omega
theta = self.define_parameter('theta', 1)
omega = self.signals['angular_velocity'][:, -1][0]
# theta, omega at time zero of time horizon
theta0 = theta - self.t*omega
# cos, sin, weight spline over time horizon
Ts = 2.*np.pi/abs(omega)
if self.options['horizon_time'] is None:
raise ValueError(
'You need to provide a horizon time when using rotating obstacles!')
else:
T = self.options['horizon_time']
n_quarters = int(np.ceil(4*T/Ts))
knots_theta = np.r_[np.zeros(3), np.hstack(
[0.25*k*np.ones(2) for k in range(1, n_quarters+1)]), 0.25*n_quarters]*(Ts/T)
Tf, knots = get_interval_T(BSplineBasis(knots_theta, 2), 0, 1.)
basis = BSplineBasis(knots, 2)
# coefficients based on nurbs representation of circle
cos_cfs = np.r_[1., np.sqrt(
2.)/2., 0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2., 0., np.sqrt(2.)/2., 1.]
sin_cfs = np.r_[0., np.sqrt(
2.)/2., 1., np.sqrt(2.)/2., 0., -np.sqrt(2.)/2., -1., -np.sqrt(2.)/2., 0.]
weight_cfs = np.r_[1., np.sqrt(
2.)/2., 1., np.sqrt(2.)/2., 1., np.sqrt(2.)/2., 1., np.sqrt(2.)/2., 1.]
cos_cfs = Tf.dot(np.array([cos_cfs[k % 8] for k in range(len(basis))]))
sin_cfs = Tf.dot(np.array([sin_cfs[k % 8] for k in range(len(basis))]))
weight_cfs = Tf.dot(
np.array([weight_cfs[k % 8] for k in range(len(basis))]))
cos_wt = BSpline(basis, cos_cfs)
sin_wt = BSpline(basis, sin_cfs)*np.sign(omega)
self.cos = cos_wt*cos(theta0) - sin_wt*sin(theta0)
self.sin = cos_wt*sin(theta0) + sin_wt*cos(theta0)
self.gon_weight = BSpline(basis, weight_cfs)
def generate_ackleyn4(n=2, a=0.2, b=3., c=2., func_opts={}, data_type=cs.SX):
x = data_type.sym("x", n)
f = 0.
for i in range(n - 1):
f += cs.exp(-a) * cs.sqrt(x[i]**2 + x[i + 1]**2)
f += b * cs.cos(c * x[i]) + cs.sin(c * x[i + 1])
func = cs.Function("ackleyn4", [x], [f], ["x"], ["f"], func_opts)
if n == 2:
glob_min = [[-1.51, -0.755]]
else:
glob_min = []
return func, [[-35., 35.]] * n, glob_min
def axis_rotation(axis, qi):
"""Returns the dual quaternion for a rotation along an axis.
AXIS MUST BE NORMALIZED!
"""
res = cs.SX.zeros(8)
cqi = cs.cos(qi / 2.0)
sqi = cs.sin(qi / 2.0)
res[0] = axis[0] * sqi
res[1] = axis[1] * sqi
res[2] = axis[2] * sqi
res[3] = cqi
return res
def euler2mat(self, rpy):
c_rpy = cs.cos(rpy)
s_rpy = cs.sin(rpy)
c0 = cs.vertcat(c_rpy[2] * c_rpy[1], s_rpy[2] * c_rpy[1], -s_rpy[1])
c1 = cs.vertcat(c_rpy[2] * s_rpy[1] * s_rpy[0] - s_rpy[2] * c_rpy[0],
s_rpy[2] * s_rpy[1] * s_rpy[0] + c_rpy[2] * c_rpy[0],
c_rpy[1] * s_rpy[0])
c2 = cs.vertcat(c_rpy[2] * s_rpy[1] * c_rpy[0] + s_rpy[2] * s_rpy[0],
s_rpy[2] * s_rpy[1] * c_rpy[0] - c_rpy[2] * s_rpy[0],
c_rpy[1] * c_rpy[0])
M = cs.horzcat(c0, c1, c2)
return M
def _create_objective_function(model, V, warm_start):
[final_cost] = model.cl([V['X', model.n]])
running_cost = 0
for k in range(model.n):
[stage_cost] = model.c([V['X', k], V['U', k]])
# Encourage looking at the ball
d = ca.veccat([ca.cos(V['X', k, 'psi'])*ca.cos(V['X', k, 'phi']),
ca.cos(V['X', k, 'psi'])*ca.sin(V['X', k, 'phi']),
ca.sin(V['X', k, 'psi'])])
r = ca.veccat([V['X', k, 'x_b'] - V['X', k, 'x_c'],
V['X', k, 'y_b'] - V['X', k, 'y_c'],
V['X', k, 'z_b']])
r_cos_omega = ca.mul(d.T, r)
if warm_start:
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
stage_cost += 1e-1 * (1 - cos_omega)
else:
stage_cost -= 1e-1 * r_cos_omega * model.dt
running_cost += stage_cost
return final_cost + running_cost
def apply_unary_opcode(code, p): assert code in unary_opcodes, "Opcode not recognized!" if code==5: return -p elif code==10: return casadi.sqrt(p) elif code==11: return casadi.sin(p) elif code==12: return casadi.cos(p) elif code==13: return casadi.tan(p) assert False
def _create_observation_covariance_function(self, N_min, N_max):
d = ca.veccat([ca.cos(self.x['psi']) * ca.cos(self.x['phi']),
ca.cos(self.x['psi']) * ca.sin(self.x['phi']),
ca.sin(self.x['psi'])])
r = ca.veccat([self.x['x_b'] - self.x['x_c'],
self.x['y_b'] - self.x['y_c'],
self.x['z_b']])
r_cos_omega = ca.mul(d.T, r)
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
# Look at the ball
N = self.z.squared(ca.SX.zeros(self.nz, self.nz))
# variance = N_max * (1 - cos_omega) + N_min
# variance = ca.mul(r.T, r) * (N_max * (1 - cos_omega) + N_min)
variance = ca.norm_2(r) * (N_max * (1 - cos_omega) + N_min)
N['x_b', 'x_b'] = variance
N['y_b', 'y_b'] = variance
N['z_b', 'z_b'] = variance
op = {'input_scheme': ['x'],
'output_scheme': ['N']}
return ca.SXFunction('Observation covariance',
[self.x], [N], op)
def R_from_roll_pitch_yaw(roll, pitch, yaw): ca1 = cos(yaw) sa1 = sin(yaw) cb1 = cos(pitch) sb1 = sin(pitch) cc1 = cos(roll) sc1 = sin(roll) ca1sb1 = ca1*sb1 sa1sb1 = sa1*sb1 if casadi.SXMatrix in map(type,[roll,pitch,yaw]): R = SXMatrix(3,3) else: R = numpy.zeros((3,3)) R[0,0] = ca1*cb1 R[0,1] = ca1sb1*sc1-sa1*cc1 R[0,2] = ca1sb1*cc1+sa1*sc1 R[1,0] = sa1*cb1 R[1,1] = sa1sb1*sc1+ca1*cc1 R[1,2] = sa1sb1*cc1-ca1*sc1 R[2,0] = -sb1 R[2,1] = cb1*sc1 R[2,2] = cb1*cc1 return R
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])])
r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']])
r_cos_theta = ca.mul(d.T,r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz,nz))
R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction('Observation covariance',[state],[R])
def Rz(theta): if type(theta)==SXMatrix: R = SXMatrix(4,4) R[0,0] = 1 R[1,1] = 1 R[2,2] = 1 R[3,3] = 1 c = cos(theta) s = sin(theta) R[0,0] = c R[1,0] = s R[0,1] = -s R[1,1] = c return R
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])])
r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]])
r_cos_theta = ca.mul(d.T, r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz, nz))
R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction("Observation covariance", [state], [R])
def create_simple_plan(x0, F, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final position
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
def setPhase(self,phase):
maxPoly = max([max(self.fits[name].polyOrder) for name in self.fits])
maxCos = max([max(self.fits[name].cosOrder) for name in self.fits])
maxSin = max([max(self.fits[name].sinOrder) for name in self.fits])
# all bases
polyBases = [phase**po for po in range(maxPoly+1)]
cosBases = [C.cos(co*phase) for co in range(maxCos+1)]
sinBases = [C.sin(co*phase) for si in range(maxSin+1)]
self.fitsWithPhase = {}
for name in self.fits:
fit = self.fits[name]
polys = [coeff*polyBases[order] for coeff,order in zip(fit.polyCoeffs, fit.polyOrder)]
coses = [coeff*cosBases[order] for coeff,order in zip(fit.cosCoeffs, fit.cosOrder)]
sins = [coeff*sinBases[order] for coeff,order in zip(fit.sinCoeffs, fit.sinOrder)]
self.fitsWithPhase[name] = sum(polys+coses+sins)
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "dmotor_torque"
, 'dddr'
] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
mip = 1. mic = 5. # Declare variables (use scalar graph) t = ca.SX.sym( 't' ) # time u = ca.SX.sym( 'u' ) # control x = ca.SX.sym( 'x' , 4 ) # state # ODE rhs function # x[0] = dtheta # x[1] = theta # x[2] = dx # x[3] = x sint = ca.sin( x[1] ) cost = ca.cos( x[1] ) fric = mic * ca.sign( x[2] ) num = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l ) denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) ) ddtheta = num / denom ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp) x_err = x-x_target cost_mat = np.diag( [1,1,1,1] ) ode = ca.vertcat([ ddtheta, \ x[0], \ ddx, \ x[2] ] )
def setupOcp(dae,conf,nk=50,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
ocp.setupCollocation(ocp.lookup('endTime'))
# constrain invariants
def constrainInvariantErrs():
dcm = ocp.lookup('dcm',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, dcm)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0)
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0)
constrainInvariantErrs()
# constraint line angle
for k in range(0,nk):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))
# constrain airspeed
def constrainAirspeedAlphaBeta():
for k in range(0,nk):
ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 5)
ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k), (-5,10))
ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k), (-10,10))
constrainAirspeedAlphaBeta()
# constrain tether force
for k in range(nk):
ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0)
ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0)
# make it periodic
for name in [ "y","z",
"dy","dz",
"w1","w2","w3",
"ddelta",
"aileron","elevator",
"r","dr"]:
ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1))
# periodic attitude
# rawekite.kiteutils.periodicEulers(ocp)
# rawekite.kiteutils.periodicOrthonormalizedDcm(ocp)
rawekite.kiteutils.periodicDcm(ocp)
# bounds
ocp.bound('aileron',(-0.04,0.04))
ocp.bound('elevator',(-0.1,0.1))
ocp.bound('daileron',(-2,2))
ocp.bound('delevator',(-2,2))
ocp.bound('x',(0.1,1000))
ocp.bound('y',(-100,100))
ocp.bound('z',(-0.5,7))
ocp.bound('r',(4,4))
ocp.bound('dr',(-10,10))
ocp.bound('ddr',(-2.5,2.5))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-70,70))
for w in ['w1','w2','w3']:
ocp.bound(w,(-4*pi,4*pi))
ocp.bound('delta',(-0.01,1.01*2*pi))
ocp.bound('ddelta',(-pi/8,8*pi))
ocp.bound('motor_torque',(-1000,1000))
ocp.bound('endTime',(0.5,7.0))
ocp.bound('w0',(10,10))
# boundary conditions
ocp.bound('delta',(0,0),timestep=0)
ocp.bound('delta',(2*pi,2*pi),timestep=-1)
# objective function
obj = 0
for k in range(nk):
# control regularization
ddr = ocp.lookup('ddr',timestep=k)
tc = ocp.lookup('motor_torque',timestep=k)
daileron = ocp.lookup('daileron',timestep=k)
delevator = ocp.lookup('delevator',timestep=k)
daileronSigma = 0.1
delevatorSigma = 0.1
ddrSigma = 5.0
torqueSigma = 1.0
# tc = tc - 390
ailObj = daileron*daileron / (daileronSigma*daileronSigma)
eleObj = delevator*delevator / (delevatorSigma*delevatorSigma)
winchObj = ddr*ddr / (ddrSigma*ddrSigma)
torqueObj = tc*tc / (torqueSigma*torqueSigma)
obj += ailObj + eleObj + winchObj + torqueObj
ocp.setObjective( obj/nk )
ocp.setQuadratureDdt('quadrature_energy', 'winch_power')
# spawn telemetry thread
callback = rawe.telemetry.startTelemetry(ocp, conf, callbacks=[(rawekite.kiteTelemetry.showAllPoints,'multi-carousel')])
# solver
solverOptions = [ ("expand",True)
# ,("qp_solver",C.NLPQPSolver)
# ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
, ("linear_solver","ma57")
, ("max_iter",1000)
, ("tol",1e-9)
# , ("Timeout", 1e6)
# , ("UserHM", True)
# , ("ScaleConIter",True)
# , ("ScaledFD",True)
# , ("ScaledKKT",True)
# , ("ScaledObj",True)
# , ("ScaledQP",True)
]
# initial guess
ocp.guessX(x0)
for k in range(0,nk+1):
val = 2.0*pi*k/nk
ocp.guess('delta',val,timestep=k,quiet=True)
ocp.guess('motor_torque',0)
ocp.guess('endTime',1.5)
ocp.guess('daileron',0)
ocp.guess('delevator',0)
ocp.guess('ddr',0)
ocp.guess('w0',10)
ocp.setupSolver( solverOpts=solverOptions,
callback=callback )
return ocp
def setupOcp(dae,conf,nk=50,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
print "setting up collocation..."
ocp.setupCollocation(ocp.lookup('endTime'))
ocp.setQuadratureDdt('quadrature energy', 'winch power')
# constrain invariants
def constrainInvariantErrs():
dcm = ocp.lookup('dcm',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, dcm)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot',None))
constrainInvariantErrs()
# constrain line angle
for k in range(0,nk):
ocp.constrain(ocp.lookup('cos(line angle)',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))
# constrain airspeed
def constrainAirspeedAlphaBeta():
for k in range(0,nk):
ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 5, tag=('airspeed',k))
ocp.constrainBnds(ocp.lookup('alpha(deg)',timestep=k), (-5,20), tag=('alpha(deg)',k))
ocp.constrainBnds(ocp.lookup('beta(deg)', timestep=k), (-10,10), tag=('beta(deg)',k))
constrainAirspeedAlphaBeta()
# constrain tether force
for k in range(nk):
ocp.constrain( ocp.lookup('tether tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension',(k,1)))
ocp.constrain( ocp.lookup('tether tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension',(k,ocp.deg)))
# bounds
ocp.bound('aileron',(-0.04,0.04))
ocp.bound('elevator',(-0.1,0.1))
ocp.bound('daileron',(-2.0,2.0))
ocp.bound('delevator',(-2.0,2.0))
ocp.bound('motor_torque',(-10000,10000))
ocp.bound('x',(-200,200))
ocp.bound('y',(-200,200))
ocp.bound('z',(-0.5,200))
ocp.bound('r',(1,31))
ocp.bound('dr',(-30,30))
ocp.bound('ddr',(-500,500))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-70,70))
for w in ['w1','w2','w3']:
ocp.bound(w,(-4*pi,4*pi))
ocp.bound('delta',(-12*pi,12*pi))
ocp.bound('ddelta',(-12*pi,12*pi))
ocp.bound('endTime',(1.5,15))
ocp.bound('w0',(10,10))
ocp.bound('phase0',(-12*pi,12*pi))
ocp.bound('phaseF',(-12*pi,12*pi))
# boundary conditions
ocp.bound('delta',(0,0),timestep=-1,quiet=True)
ocp.bound('ddelta',(0,0),timestep=-1,quiet=True)
def getFourierFit(filename,phase):
# load the fourier fit
f=open(filename,'r')
trajFits = pickle.load(f)
f.close()
trajFits.setPhase(phase)
return trajFits
startup = getFourierFit("data/carousel_opt_fourier.dat",ocp.lookup('phase0'))
crosswind = getFourierFit("data/crosswind_opt_fourier.dat",ocp.lookup('phaseF'))
def getFourierDcm(vars):
return C.vertcat([C.horzcat([vars['e11'], vars['e12'], vars['e13']]),
C.horzcat([vars['e21'], vars['e22'], vars['e23']]),
C.horzcat([vars['e31'], vars['e32'], vars['e33']])])
# for name in ['y','z','dy','dz','r','dr','w1','w2','w3','delta','ddelta']:
for name in ['y','z','dy','dz','r','dr','delta','ddelta']:
ocp.constrain(ocp.lookup(name,timestep=0), '==', startup[name])
# for name in ['y','z','dy','dz','r','dr','w1','w2','w3']:
for name in ['y','z','dy','dz','r','dr']:
ocp.constrain(ocp.lookup(name,timestep=-1), '==', crosswind[name])
# match DCMs
rawe.kiteutils.matchDcms(ocp, rawe.kiteutils.getDcm(ocp,0), getFourierDcm(startup))
rawe.kiteutils.matchDcms(ocp, rawe.kiteutils.getDcm(ocp,-1), getFourierDcm(crosswind))
# initial guess
phase0Guess = -4.0*pi
phaseFGuess = -0.4*2*pi
namesF = ['x','y','z','dx','dy','dz','r','dr','w1','w2','w3','e11','e12','e13','e21','e22','e23','e31','e32','e33']
names0 = namesF+['delta','ddelta']
initfun = C.MXFunction([ocp.lookup('phase0')],[startup[name] for name in names0])
initfun.init()
for j in range(nk+1):
alpha = float(j)/nk
phase = phase0Guess*(1-alpha) + phaseFGuess*alpha
initfun.setInput([phase])
initfun.evaluate()
initvals = dict([(name,float(initfun.output(k)[0,0])) for k,name in enumerate(names0)])
for name in initvals:
ocp.guess(name,float(initvals[name]),timestep=j)
# finalfun = C.MXFunction([ocp.lookup('phaseF')],[crosswind[name] for name in namesF])
# finalfun.init()
# finalfun.setInput([phaseFGuess])
# finalfun.evaluate()
# finalvals = dict([(name,float(finalfun.output(k)[0,0])) for k,name in enumerate(namesF)])
# initvals['delta'] = 0
# initvals['ddelta'] = 0
# finalvals['delta'] = 0
# finalvals['ddelta'] = 0
# for name in namesF:
# for k in range(nk+1):
# alpha = float(k)/nk
# ocp.guess(name,(1-alpha)*initvals[name] + alpha*finalvals[name], timestep=k)
#
# for j,name in enumerate(namesF):
# for k in range(nk+1):
# alpha = float(k)/nk
# finalfun.setInput([phaseFGuess])
# finalval = float(finalfun.output(k)[0,0])) for k,name in enumerate(namesF)])
# ocp.guess(name,(1-alpha)*initvals[name] + alpha*finalvals[name], timestep=k)
ocp.guess('aileron',0)
ocp.guess('elevator',0)
ocp.guess('ddr',0)
ocp.guess('motor_torque',0)
ocp.guess('phase0',phase0Guess)
ocp.guess('phaseF',phaseFGuess)
ocp.guess('endTime',8)
ocp.guess('w0',10)
# objective function
obj = 0
for k in range(nk):
u = ocp.uVec(k)
ddr = ocp.lookup('ddr',timestep=k)
tc = ocp.lookup('motor_torque',timestep=k)
torqueSigma = 1000.0
aileron = ocp.lookup('aileron',timestep=k)
elevator = ocp.lookup('elevator',timestep=k)
aileronSigma = 0.1
elevatorSigma = 0.1
torqueSigma = 1000.0
ddrSigma = 5.0
ailObj = aileron*aileron / (aileronSigma*aileronSigma)
eleObj = elevator*elevator / (elevatorSigma*elevatorSigma)
winchObj = ddr*ddr / (ddrSigma*ddrSigma)
torqueObj = tc*tc / (torqueSigma*torqueSigma)
obj += ailObj + eleObj + winchObj + torqueObj
ocp.setObjective( 1e1*obj/nk + ocp.lookup('endTime') )
oldKiteProtos = []
mcc = rawe.kite_pb2.MultiCarousel().FromString(startup.multiCarousel)
oldKiteProtos.extend(mcc.horizon)
mcc = rawe.kite_pb2.MultiCarousel().FromString(crosswind.multiCarousel)
oldKiteProtos.extend(mcc.horizon)
for okp in oldKiteProtos:
okp.zt = conf['zt']
okp.rArm = conf['rArm']
okp.lineTransparency = 0.0
okp.kiteTransparency = 0.3
# for k in range(0,startupfits['x'].Mx.size):
# oldKiteProtos.append( toFourierKiteProto(startupfits,k,zt=conf['kite']['zt'], rArm=conf['carousel']['rArm'],kiteAlpha=0.3,lineAlpha=0,dz=0) )
# for k in range(0,crosswindfits['x'].Mx.size):
# oldKiteProtos.append( toFourierKiteProto(crosswindfits,k,zt=conf['kite']['zt'], rArm=conf['carousel']['rArm'],kiteAlpha=0.3,lineAlpha=0,dz=0) )
# callback function
def myCallback(traj,myiter,ocp_,conf_):
kiteProtos = []
for k in range(0,ocp.nk):
for nicpIdx in range(0,ocp.nicp):
for degIdx in [0]:
# for degIdx in range(ocp.deg+1):
lookup = lambda name: traj.lookup(name,timestep=k,nicpIdx=nicpIdx,degIdx=degIdx)
kiteProtos.append( kiteproto.toKiteProto(lookup,
lineAlpha=0.2) )
kiteProtos += oldKiteProtos
mc = kite_pb2.MultiCarousel()
mc.horizon.extend(list(kiteProtos))
mc.messages.append("w0: "+str(traj.lookup('w0')))
mc.messages.append("iter: "+str(myiter))
mc.messages.append("endTime: "+str(traj.lookup('endTime')))
mc.messages.append("average power: "+str(traj.lookup('quadrature energy',timestep=-1)/traj.lookup('endTime'))+" W")
mc.messages.append("phase0: "+str(traj.lookup('phase0')/pi)+" * pi")
mc.messages.append("phaseF: "+str(traj.lookup('phaseF')/pi)+" * pi")
# # bounds feedback
# lbx = ocp.solver.input(C.NLP_LBX)
# ubx = ocp.solver.input(C.NLP_UBX)
# ocp._bounds.printBoundsFeedback(xOpt,lbx,ubx,reportThreshold=0)
#
# # constraints feedback
# lbg = ocp.solver.input(C.NLP_LBG)
# ubg = ocp.solver.input(C.NLP_UBG)
# g = numpy.array(f.input(C.NLP_G))
# ocp._constraints.printViolations(g,lbg,ubg,reportThreshold=0)
return mc.SerializeToString()
callback = rawe.telemetry.startTelemetry(ocp, conf, callbacks=[(myCallback,'multi-carousel')])
# solver
solverOptions = [ ("expand_f",True)
, ("expand_g",True)
, ("generate_hessian",True)
# ,("qp_solver",C.NLPQPSolver)
# ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
, ("linear_solver","ma57")
, ("max_iter",1000)
, ("tol",1e-9)
# , ("Timeout", 1e6)
# , ("UserHM", True)
# , ("ScaleConIter",True)
# , ("ScaledFD",True)
# , ("ScaledKKT",True)
# , ("ScaledObj",True)
# , ("ScaledQP",True)
]
print "setting up solver..."
ocp.setupSolver( solverOpts=solverOptions,
callback=callback )
return ocp
def setupOcp(dae,conf,nk,nicp,deg,collPoly):
def addCosts():
dddr = dae['dddr']
daileron = dae['daileron']
delevator = dae['delevator']
drudder = dae['drudder']
dflaps = dae['dflaps']
daileronSigma = 0.001
delevatorSigma = 0.8
dddrSigma = 10.0
drudderSigma = 0.1
dflapsSigma = 0.1
fudgeFactor = 1e-1
nkf = float(nk)
dae['daileronCost'] = fudgeFactor*daileron*daileron / (daileronSigma*daileronSigma*nkf)
dae['delevatorCost'] = fudgeFactor*delevator*delevator / (delevatorSigma*delevatorSigma*nkf)
dae['drudderCost'] = fudgeFactor*drudder*drudder / (drudderSigma*drudderSigma)
dae['dflapsCost'] = fudgeFactor*dflaps*dflaps / (dflapsSigma*dflapsSigma)
dae['dddrCost'] = fudgeFactor*dddr*dddr / (dddrSigma*dddrSigma*nkf)
addCosts()
ocp = rawe.collocation.Coll(dae, nk=nk, nicp=nicp, deg=deg, collPoly=collPoly)
print "setting up collocation..."
ocp.setupCollocation(ocp.lookup('endTime'))
print "moar setting up ocp..."
# constrain invariants
def constrainInvariantErrs():
R_n2b = ocp.lookup('R_n2b',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))
constrainInvariantErrs()
# constrain line angle
for k in range(0,nk):
for j in range(0,ocp.deg+1):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k,degIdx=j),'>=',C.cos(65*pi/180), tag=('line angle',k))
# constrain airspeed
def constrainAirspeedAlphaBeta():
for k in range(0,nk):
ocp.constrain(ocp.lookup('airspeed',timestep=k), '>=', 10, tag=('airspeed',k))
for j in range(0,ocp.deg+1):
ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha(deg)',k))
ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k), (-9,9), tag=('beta(deg)',k))
constrainAirspeedAlphaBeta()
#def constrainCl():
# for k in range(0,nk):
# ocp.constrain(ocp.lookup('cL',timestep=k), '<=', 2.0, tag=('cL',k))
#constrainCl()
# constrain tether force
for k in range(nk):
# ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension positive',k))
# ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension positive',k))
for j in range(1,ocp.deg+1):
ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=j), '>=', 0, tag=('tether tension positive',k))
# # real motor constraints
# for k in range(nk):
## ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=1), '<=', 150, tag=('motor torque',k))
## ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=ocp.deg), '<=', 150, tag=('motor torque',k))
# ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=1), '<=', 78, tag=('motor torque',k))
# ocp.constrain( ocp.lookup('torque',timestep=k,degIdx=ocp.deg), '<=', 78, tag=('motor torque',k))
#
# ocp.constrain( ocp.lookup('rpm',timestep=k), '<=', 1500, tag=('rpm',k))
# ocp.constrain( -1500, '<=', ocp.lookup('rpm',timestep=k), tag=('rpm',k))
# make it periodic
for name in [ "y","z",
"dy","dz",
"w_bn_b_x","w_bn_b_y","w_bn_b_z",
"r","dr","ddr",
'aileron','elevator','rudder','flaps'
]:
ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic diff state \"'+name+'"',None))
# periodic attitude
rawekite.kiteutils.periodicDcm(ocp)
# bounds
ocp.bound('aileron', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('elevator',(numpy.radians(-10),numpy.radians(10)))
ocp.bound('rudder', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('flaps', (numpy.radians(0),numpy.radians(0)))
# can't bound flaps==0 AND have periodic flaps at the same time
# bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
ocp.bound('daileron',(-2.0,2.0))
ocp.bound('delevator',(-2.0,2.0))
ocp.bound('drudder',(-2.0,2.0))
ocp.bound('dflaps',(-2.0,2.0))
ocp.bound('x',(-2000,2000))
ocp.bound('y',(-2000,2000))
if 'minAltitude' in conf:
ocp.bound('z',(-2000, -conf['minAltitude']))
else:
ocp.bound('z',(-2000, -0.05))
ocp.bound('r',(1,500))
ocp.bound('dr',(-30,30))
ocp.bound('ddr',(-500,500))
ocp.bound('dddr',(-50000,50000))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-200,200))
for w in ['w_bn_b_x',
'w_bn_b_y',
'w_bn_b_z']:
ocp.bound(w,(-6*pi,6*pi))
# ocp.bound('endTime',(0.5,12))
ocp.bound('endTime',(0.5,numLoops*7.5))
ocp.bound('w0',(10,10))
# boundary conditions
ocp.bound('y',(0,0),timestep=0,quiet=True)
# guesses
ocp.guess('endTime',5.4)
ocp.guess('w0',10)
# objective function
obj = 0
for k in range(nk):
# control regularization
obj += ocp.lookup('daileronCost',timestep=k)
obj += ocp.lookup('delevatorCost',timestep=k)
obj += ocp.lookup('drudderCost',timestep=k)
obj += ocp.lookup('dflapsCost',timestep=k)
obj += ocp.lookup('dddrCost',timestep=k)
ocp.setQuadratureDdt('mechanical_energy', 'mechanical_winch_power')
ocp.setQuadratureDdt('electrical_energy', 'electrical_winch_power')
ocp.setObjective( obj + ocp.lookup(powerType+'_energy',timestep=-1)/ocp.lookup('endTime') )
return ocp
def setupOcp(dae,conf,nk,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
print "setting up collocation..."
ocp.setupCollocation(ocp.lookup('endTime'))
for k in range(ocp.nk):
for j in range(1,ocp.deg+1): #[1]:
ocp.constrain( ocp('x',timestep=k,degIdx=j), '>=', ocp('y',timestep=k,degIdx=j), tag=('x>y',k))
ocp.constrain( ocp('x',timestep=k,degIdx=j), '>=', -ocp('y',timestep=k,degIdx=j), tag=('x>-y',k))
# constrain invariants
def constrainInvariantErrs():
R_c2b = ocp.lookup('R_c2b',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, R_c2b)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c 0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot 0',None))
ocp.constrain(ocp('sin_delta',timestep=0)**2 + ocp('cos_delta',timestep=0)**2,
'==', 1, tag=('sin**2 + cos**2 == 1',None))
constrainInvariantErrs()
# constrain line angle
for k in range(0,nk):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(45*pi/180), tag=('line angle',k))
# constrain airspeed
def constrainAirspeedAlphaBeta():
for k in range(0,nk):
for j in range(0,deg+1):
ocp.constrainBnds(ocp.lookup('airspeed',timestep=k,degIdx=j),
(8,35), tag=('airspeed',(k,j)))
ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,7.5), tag=('alpha(deg)',nk))
ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-6,6), tag=('beta(deg)',nk))
ocp.constrain(ocp.lookup('cL',timestep=k,degIdx=j), '>=', 0, tag=('CL positive',nk))
constrainAirspeedAlphaBeta()
# constrain tether force
constrainTetherForce(ocp)
realMotorConstraints(ocp)
# make it periodic
for name in [ "y","z",
"dy","dz",
"w_bn_b_x","w_bn_b_y","w_bn_b_z",
"r","dr",'ddr',
'ddelta',
'aileron','elevator','rudder','flaps',
'sin_delta','motor_torque'
]:
ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic '+name,None))
# periodic attitude
rawekite.kiteutils.periodicDcm(ocp)
# bounds
ocp.bound('aileron', (numpy.radians(-8),numpy.radians(8)))
ocp.bound('elevator',(numpy.radians(-8),numpy.radians(8)))
ocp.bound('rudder', (numpy.radians(-8),numpy.radians(8)))
ocp.bound('flaps', (numpy.radians(0),numpy.radians(0)))
# can't bound flaps==0 AND have periodic flaps at the same time
# bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
ocp.bound('daileron', (numpy.radians(-20), numpy.radians(20)))
ocp.bound('delevator', (numpy.radians(-20), numpy.radians(20)))
ocp.bound('drudder', (numpy.radians(-20), numpy.radians(20)))
ocp.bound('dflaps', (numpy.radians(-20), numpy.radians(20)))
ocp.bound('ddelta',(-0.98*2*pi, 0.98*2*pi))
ocp.bound('x',(-2000,2000))
ocp.bound('y',(-2000,2000))
ocp.bound('z',(-3, -0.3))
ocp.bound('r',(5,8))
# can't bound r AND have periodic r at the same time
ocp.bound('r',(0,100),timestep=-1)
ocp.bound('dr',(-1,1))
ocp.bound('ddr',(-15,15))
ocp.bound('dddr',(-15,15))
ocp.bound('motor_torque',(-50,50))
ocp.bound('dmotor_torque',(-1,1))
ocp.bound('cos_delta',(-1.5,1.5))
ocp.bound('sin_delta',(-1.5,1.5))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-70,70))
for w in ['w_bn_b_x',
'w_bn_b_y',
'w_bn_b_z']:
ocp.bound(w,(-3*pi,3*pi))
ocp.bound('endTime',(0.5,4.0))
ocp.guess('endTime',1)
ocp.bound('w0',(10,10))
# boundary conditions
ocp.bound('sin_delta', (0,0), timestep=0, quiet=True)
ocp.bound('cos_delta', (0.5,1.5), timestep=0, quiet=True)
ocp.bound('cos_delta', (0.5,1.5), timestep=-1, quiet=True)
return ocp
def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf["kite"]["mass"] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf["env"]["g"] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
# INERTIA MATRIX (Kurt's direct measurements)
j1 = conf["kite"]["j1"]
j31 = conf["kite"]["j31"]
j2 = conf["kite"]["j2"]
j3 = conf["kite"]["j3"]
# Carousel Friction & inertia
jCarousel = conf["carousel"]["jCarousel"]
cfric = conf["carousel"]["cfric"]
zt = conf["kite"]["zt"]
rA = conf["carousel"]["rArm"]
########### model integ ###################
e11 = dae.x("e11")
e12 = dae.x("e12")
e13 = dae.x("e13")
e21 = dae.x("e21")
e22 = dae.x("e22")
e23 = dae.x("e23")
e31 = dae.x("e31")
e32 = dae.x("e32")
e33 = dae.x("e33")
x = dae.x("x")
y = dae.x("y")
z = dae.x("z")
dx = dae.x("dx")
dy = dae.x("dy")
dz = dae.x("dz")
w1 = dae.x("w1")
w2 = dae.x("w2")
w3 = dae.x("w3")
delta = 0
ddelta = 0
r = dae.x("r")
dr = dae.x("dr")
ddr = dae.u("ddr")
# wind
z0 = conf["wind shear"]["z0"]
zt_roughness = conf["wind shear"]["zt_roughness"]
zsat = 0.5 * (z + C.sqrt(z * z))
wind_x = dae.p("w0") * C.log((zsat + zt_roughness + 2) / zt_roughness) / C.log(z0 / zt_roughness)
dae.addOutput("wind at altitude", wind_x)
dae.addOutput("w0", dae.p("w0"))
dp_carousel_frame = C.veccat([dx - ddelta * y, dy + ddelta * (rA + x), dz]) - C.veccat(
[C.cos(delta) * wind_x, C.sin(delta) * wind_x, 0]
)
R_c2b = C.veccat(dae.x(["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"])).reshape((3, 3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul(R_c2b, dp_carousel_frame)
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(
dae,
conf,
dp_carousel_frame,
dpE,
dae.x(("w1", "w2", "w3")),
dae.x(("e21", "e22", "e23")),
dae.u(("aileron", "elevator")),
)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0, 0] = m
mm[0, 1] = 0
mm[0, 2] = 0
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = x + zt * e31
mm[1, 0] = 0
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = y + zt * e32
mm[2, 0] = 0
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = z + zt * e33
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = j1
mm[3, 4] = 0
mm[3, 5] = j31
mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j2
mm[4, 5] = 0
mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = j31
mm[5, 4] = 0
mm[5, 5] = j3
mm[5, 6] = 0
mm[6, 0] = x + zt * e31
mm[6, 1] = y + zt * e32
mm[6, 2] = z + zt * e33
mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
mm[6, 5] = 0
mm[6, 6] = 0
# right hand side
zt2 = zt * zt
rhs = C.veccat(
[
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m,
f3 - g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r
- (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
- dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 * (w2 - ddelta * e23))
- dy * (dy - zt * e22 * (w1 - ddelta * e13) + zt * e12 * (w2 - ddelta * e23))
- dz * (dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 * (w2 - ddelta * e23))
+ dr * dr
+ (w1 - ddelta * e13)
* (
e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e22 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
- (w2 - ddelta * e23)
* (
e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 * (w2 - ddelta * e23))
+ e12 * (zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 * (w2 - ddelta * e23))
+ zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
),
]
)
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31) ** 2 / 2 + (y + zt * e32) ** 2 / 2 + (z + zt * e33) ** 2 / 2 - r ** 2 / 2
cdot = (
dx * (x + zt * e31)
+ dy * (y + zt * e32)
+ dz * (z + zt * e33)
+ zt * (w2 - ddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (w1 - ddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- r * dr
)
ddx = dae.z("ddx")
ddy = dae.z("ddy")
ddz = dae.z("ddz")
dw1 = dae.z("dw1")
dw2 = dae.z("dw2")
dddelta = 0
cddot = (
-(w1 - ddelta * e13)
* (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1)
+ zt
* e33
* (
w1 * z
+ zt * e33 * w1
+ ddelta * e11 * x
+ ddelta * e12 * y
+ zt * ddelta * e11 * e31
+ zt * ddelta * e12 * e32
)
+ zt * e21 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e22 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ zt * e31 * (x + zt * e31) * (w1 - ddelta * e13)
+ zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
)
+ (w2 - ddelta * e23)
* (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1)
- zt
* e33
* (
w2 * z
+ zt * e33 * w2
+ ddelta * e21 * x
+ ddelta * e22 * y
+ zt * ddelta * e21 * e31
+ zt * ddelta * e22 * e32
)
+ zt * e11 * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ zt * e12 * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
- zt * e31 * (x + zt * e31) * (w2 - ddelta * e23)
- zt * e32 * (y + zt * e32) * (w2 - ddelta * e23)
)
- ddr * r
+ (
zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
+ zt * w2 * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
)
* (w3 - ddelta * e33)
+ dx * (dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 + zt * ddelta * e13 * e21)
+ dy * (dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22)
+ dz * (dz + zt * e13 * w2 - zt * e23 * w1)
+ ddx * (x + zt * e31)
+ ddy * (y + zt * e32)
+ ddz * (z + zt * e33)
- dr * dr
+ zt * (dw2 - dddelta * e23) * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 + zt * e13 * e33)
- zt * (dw1 - dddelta * e13) * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 + zt * e23 * e33)
- zt
* dddelta
* (
e11 * e23 * x
- e13 * e21 * x
+ e12 * e23 * y
- e13 * e22 * y
+ zt * e11 * e23 * e31
- zt * e13 * e21 * e31
+ zt * e12 * e23 * e32
- zt * e13 * e22 * e32
)
)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae.addOutput("c", c)
dae.addOutput("cdot", cdot)
dae.addOutput("cddot", cddot)
return (mm, rhs, dRexp)
def plot_heuristics(model, x_all, u_all, n_last=2):
n_interm_points = 301
n = len(x_all[:])
t_all = np.linspace(0, (n - 1) * model.dt, n)
t_all_dense = np.linspace(t_all[0], t_all[-1], n_interm_points)
fig, ax = plt.subplots(2, 2, figsize=(10, 10))
# ---------------- Optic acceleration cancellation ----------------- #
oac = []
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_bc_xy = ca.norm_2(x_b - x_c)
z_b = x_all[k, 'z_b']
tan_phi = ca.arctan2(z_b, r_bc_xy)
oac.append(tan_phi)
# Fit a line for OAC
fit_oac = np.polyfit(t_all[:-n_last], oac[:-n_last], 1)
fit_oac_fn = np.poly1d(fit_oac)
# Plot OAC
ax[0, 0].plot(t_all[:-n_last], oac[:-n_last],
label='Simulation', lw=3)
ax[0, 0].plot(t_all, fit_oac_fn(t_all), '--k', label='Linear fit')
# ------------------- Constant bearing angle ----------------------- #
cba = []
d = ca.veccat([ca.cos(model.m0['phi']),
ca.sin(model.m0['phi'])])
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_cb = x_b - x_c
cos_gamma = ca.mul(d.T, r_cb) / ca.norm_2(r_cb)
cba.append(np.rad2deg(np.float(ca.arccos(cos_gamma))))
# Fit a const for CBA
fit_cba = np.polyfit(t_all[:-n_last], cba[:-n_last], 0)
fit_cba_fn = np.poly1d(fit_cba)
# Smoothen the trajectory
t_part_dense = np.linspace(t_all[0], t_all[-n_last-1], 301)
cba_smooth = spline(t_all[:-n_last], cba[:-n_last], t_part_dense)
ax[1, 0].plot(t_part_dense, cba_smooth, lw=3, label='Simulation')
# Plot CBA
# ax[1, 0].plot(t_all[:-n_last], cba[:-n_last],
# label='$\gamma \\approx const$')
ax[1, 0].plot(t_all, fit_cba_fn(t_all), '--k', label='Constant fit')
# ---------- Generalized optic acceleration cancellation ----------- #
goac_smooth = spline(t_all,
model.m0['phi'] - x_all[:, 'phi'],
t_all_dense)
n_many_last = n_last *\
n_interm_points / (t_all[-1] - t_all[0]) * model.dt
# Delta
ax[0, 1].plot(t_all_dense[:-n_many_last],
np.rad2deg(goac_smooth[:-n_many_last]), lw=3,
label=r'Rotation angle $\delta$')
# Gamma
ax[0, 1].plot(t_all[:-n_last], cba[:-n_last], '--', lw=2,
label=r'Bearing angle $\gamma$')
# ax[0, 1].plot([t_all[0], t_all[-1]], [30, 30], 'k--',
# label='experimental bound')
# ax[0, 1].plot([t_all[0], t_all[-1]], [-30, -30], 'k--')
# ax[0, 1].yaxis.set_ticks(range(-60, 70, 30))
# Derivative of delta
# ax0_twin = ax[0, 1].twinx()
# ax0_twin.step(t_all,
# np.rad2deg(np.array(ca.veccat([0, u_all[:, 'w_phi']]))),
# 'g-', label='derivative $\mathrm{d}\delta/\mathrm{d}t$')
# ax0_twin.set_ylim(-90, 90)
# ax0_twin.yaxis.set_ticks(range(-90, 100, 30))
# ax0_twin.set_ylabel('$\mathrm{d}\delta/\mathrm{d}t$, deg/s')
# ax0_twin.yaxis.label.set_color('g')
# ax0_twin.legend(loc='lower right')
# -------------------- Linear optic trajectory --------------------- #
lot_beta = []
x_b = model.m0[ca.veccat, ['x_b', 'y_b']]
for k in range(n):
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
d = ca.veccat([ca.cos(x_all[k, 'phi']),
ca.sin(x_all[k, 'phi'])])
r = x_b - x_c
cos_beta = ca.mul(d.T, r) / ca.norm_2(r)
beta = ca.arccos(cos_beta)
tan_beta = ca.tan(beta)
lot_beta.append(tan_beta)
# lot_beta.append(np.rad2deg(np.float(ca.arccos(cos_beta))))
# lot_alpha = np.rad2deg(np.array(x_all[:, 'psi']))
lot_alpha = ca.tan(x_all[:, 'psi'])
# Fit a line for LOT
fit_lot = np.polyfit(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last], 1)
fit_lot_fn = np.poly1d(fit_lot)
# Plot
ax[1, 1].scatter(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last],
label='Simulation')
ax[1, 1].plot(lot_alpha[model.n_delay:-n_last],
fit_lot_fn(lot_alpha[model.n_delay:-n_last]),
'--k', label='Linear fit')
fig.tight_layout()
return fig, ax
def setupOcp(dae,conf,nk,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
print "setting up collocation..."
ocp.setupCollocation(ocp.lookup('endTime'))
# constrain invariants
def constrainInvariantErrs():
R_n2b = ocp.lookup('R_n2b',timestep=0)
rawekite.kiteutils.makeOrthonormal(ocp, R_n2b)
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c 0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot 0',None))
constrainInvariantErrs()
# constrain line angle
for k in range(0,nk):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(55*pi/180), tag=('line angle',k))
# constrain airspeed
def constrainAirspeedAlphaBeta():
for k in range(0,nk):
for j in range(0,deg+1):
ocp.constrain(ocp.lookup('airspeed',timestep=k,degIdx=j), '>=', 20, tag=('airspeed',nk))
ocp.constrainBnds(ocp.lookup('alpha_deg',timestep=k,degIdx=j), (-4.5,8.5), tag=('alpha',nk))
ocp.constrainBnds(ocp.lookup('beta_deg', timestep=k,degIdx=j), (-10,10), tag=('beta',nk))
constrainAirspeedAlphaBeta()
# constrain tether force
for k in range(nk):
ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=1), '>=', 0, tag=('tether tension',(nk,0)))
ocp.constrain( ocp.lookup('tether_tension',timestep=k,degIdx=ocp.deg), '>=', 0, tag=('tether tension',(nk,1)))
# make it periodic
for name in [ "r_n2b_n_y","r_n2b_n_z",
"v_bn_n_y","v_bn_n_z",
"w_bn_b_x","w_bn_b_y","w_bn_b_z",
"r","dr",'ddr',
'aileron','elevator','rudder','flaps',
]:
ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1), tag=('periodic '+name,None))
# periodic attitude
rawekite.kiteutils.periodicDcm(ocp)
# bounds
ocp.bound('aileron', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('elevator',(numpy.radians(-10),numpy.radians(10)))
ocp.bound('rudder', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('flaps', (numpy.radians(0),numpy.radians(0)))
# can't bound flaps==0 AND have periodic flaps at the same time
# bounding flaps (-1,1) at timestep 0 doesn't really free them, but satisfies LICQ
ocp.bound('flaps', (-1,1),timestep=0,quiet=True)
ocp.bound('daileron',(-2.0,2.0))
ocp.bound('delevator',(-2.0,2.0))
ocp.bound('drudder',(-2.0,2.0))
ocp.bound('dflaps',(-2.0,2.0))
ocp.bound('r_n2b_n_x',(-2000,2000))
ocp.bound('r_n2b_n_y',(-2000,2000))
if 'minAltitude' in conf:
ocp.bound('r_n2b_n_z',(-2000, -conf['minAltitude']))
else:
ocp.bound('r_n2b_n_z',(-2000, -0.5))
ocp.bound('r',(1,2000))
ocp.bound('dr',(-30,30))
ocp.bound('ddr',(-15,15))
ocp.bound('dddr',(-15,15))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']:
ocp.bound(d,(-70,70))
for w in ['w_bn_b_x',
'w_bn_b_y',
'w_bn_b_z']:
ocp.bound(w,(-4*pi,4*pi))
ocp.bound('endTime',(4.0,4.0))
ocp.guess('endTime',4.0)
ocp.bound('w0',(10,10))
# boundary conditions
ocp.bound('r_n2b_n_y',(0,0),timestep=0,quiet=True)
return ocp
def setupOcp(dae,conf,publisher,nk=50,nicp=1,deg=4):
ocp = Coll(dae, nk=nk,nicp=nicp,deg=deg)
# constrain invariants
def invariantErrs():
dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')])
, C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')])
, C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')])
] ).trans()
err = C.mul(dcm.trans(), dcm)
dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ])
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('c'),dae.output('cdot'),dcmErr]
)
f.setOption('name','invariant errors')
f.init()
return f
[c0,cdot0,dcmError0] = invariantErrs().call([ocp.xVec(0),ocp.uVec(0),ocp.pVec()])
ocp.constrain(c0,'==',0)
ocp.constrain(cdot0,'==',0)
ocp.constrain(dcmError0,'==',0)
# constrain line angle
for k in range(0,nk+1):
ocp.constrain(kiteutils.getCosLineAngle(ocp,k),'>=',C.cos(55*pi/180))
# constrain airspeed
def constrainAirspeedAlphaBeta():
f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()]
, [dae.output('airspeed'),dae.output('alpha(deg)'),dae.output('beta(deg)')]
)
f.setOption('name','airspeed/alpha/beta')
f.init()
for k in range(0,nk):
[airspeed,alphaDeg,betaDeg] = f.call([ocp.xVec(k),ocp.uVec(k),ocp.pVec()])
ocp.constrainBnds(airspeed,(10,50))
ocp.constrainBnds(alphaDeg,(-5,10))
ocp.constrainBnds(betaDeg,(-10,10))
constrainAirspeedAlphaBeta()
# make it periodic
for name in [ #"x" # state 0
"y" # state 1
, "z" # state 2
# , "e11" # state 3
# , "e12" # state 4
# , "e13" # state 5
# , "e21" # state 6
# , "e22" # state 7
# , "e23" # state 8
# , "e31" # state 9
# , "e32" # state 10
# , "e33" # state 11
# , "dx" # state 12
, "dy" # state 13
, "dz" # state 14
, "w1" # state 15
, "w2" # state 16
, "w3" # state 17
, "r" # state 20
, "dr" # state 21
]:
ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1))
# periodic attitude
kiteutils.periodicDcm(ocp)
# bounds
ocp.bound('aileron',(-0.04,0.04))
ocp.bound('elevator',(-0.1,0.1))
ocp.bound('x',(-2,200))
ocp.bound('y',(-200,200))
ocp.bound('z',(0.5,200))
ocp.bound('r',(1,30))
ocp.bound('dr',(-10,10))
ocp.bound('ddr',(-2.5,2.5))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-70,70))
for w in ['w1','w2','w3']:
ocp.bound(w,(-4*pi,4*pi))
ocp.bound('endTime',(0.5,5))
ocp.bound('w0',(10,10))
ocp.bound('energy',(-1e6,1e6))
# boundary conditions
ocp.bound('energy',(0,0),timestep=0,quiet=True)
ocp.bound('y',(0,0),timestep=0,quiet=True)
# objective function
obj = 0
for k in range(nk):
u = ocp.uVec(k)
ddr = ocp.lookup('ddr',timestep=k)
aileron = ocp.lookup('aileron',timestep=k)
elevator = ocp.lookup('elevator',timestep=k)
aileronSigma = 0.1
elevatorSigma = 0.1
ddrSigma = 5.0
ailObj = aileron*aileron / (aileronSigma*aileronSigma)
eleObj = elevator*elevator / (elevatorSigma*elevatorSigma)
winchObj = ddr*ddr / (ddrSigma*ddrSigma)
obj += ailObj + eleObj + winchObj
ocp.setObjective( 1e1*C.sumAll(obj)/float(nk) + ocp.lookup('energy',timestep=-1)/ocp.lookup('endTime') )
# callback function
class MyCallback:
def __init__(self):
self.iter = 0
def __call__(self,f,*args):
self.iter = self.iter + 1
xOpt = numpy.array(f.input(C.NLP_X_OPT))
opt = ocp.devectorize(xOpt)
xup = opt['vardict']
kiteProtos = []
for k in range(0,nk):
j = nicp*(deg+1)*k
kiteProtos.append( kiteproto.toKiteProto(C.DMatrix(opt['x'][:,j]),
C.DMatrix(opt['u'][:,j]),
C.DMatrix(opt['p']),
conf['kite']['zt'],
conf['carousel']['rArm'],
zeroDelta=True) )
# kiteProtos = [kiteproto.toKiteProto(C.DMatrix(opt['x'][:,k]),C.DMatrix(opt['u'][:,k]),C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm'], zeroDelta=True) for k in range(opt['x'].shape[1])]
mc = kite_pb2.MultiCarousel()
mc.css.extend(list(kiteProtos))
mc.messages.append("endTime: "+str(xup['endTime']))
mc.messages.append("w0: "+str(xup['w0']))
mc.messages.append("iter: "+str(self.iter))
# bounds feedback
# lbx = ocp.solver.input(C.NLP_LBX)
# ubx = ocp.solver.input(C.NLP_UBX)
# violations = boundsFeedback(xOpt,lbx,ubx,ocp.bndtags,tolerance=1e-9)
# for name in violations:
# violmsg = "violation!: "+name+": "+str(violations[name])
# mc.messages.append(violmsg)
publisher.send_multipart(["multi-carousel", mc.SerializeToString()])
# solver
solverOptions = [ ("expand_f",True)
, ("expand_g",True)
, ("generate_hessian",True)
# ,("qp_solver",C.NLPQPSolver)
# ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}})
, ("linear_solver","ma57")
, ("max_iter",1000)
, ("tol",1e-9)
# , ("Timeout", 1e6)
# , ("UserHM", True)
# , ("ScaleConIter",True)
# , ("ScaledFD",True)
# , ("ScaledKKT",True)
# , ("ScaledObj",True)
# , ("ScaledQP",True)
]
# initial guess
# ocp.guessX(x0)
# for k in range(0,nk+1):
# val = 2.0*pi*k/nk
# ocp.guess('delta',val,timestep=k,quiet=True)
#
# ocp.guess('aileron',0)
# ocp.guess('elevator',0)
# ocp.guess('tc',0)
ocp.guess('endTime',5.4)
#
# ocp.guess('ddr',0)
ocp.guess('w0',10)
print "setting up collocation..."
ocp.setupCollocation(ocp.lookup('endTime'))
print "setting up solver..."
ocp.setupSolver( solverOpts=solverOptions,
callback=MyCallback() )
return ocp