def makeOrthonormal(ocp_, R):
ocp_.constrain(C.mul(R[0, :], R[0, :].T),
'==',
1,
tag=('R1[0]: e1^T * e1 == 1', None))
ocp_.constrain(C.mul(R[1, :], R[0, :].T),
'==',
0,
tag=('R1[0]: e2^T * e1 == 0', None))
ocp_.constrain(C.mul(R[1, :], R[1, :].T),
'==',
1,
tag=('R1[0]: e2^T * e2 == 1', None))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
ocp_.constrain(rhon[0],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
ocp_.constrain(rhon[2],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
ocp_.constrain(rhon[1],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
def belief_dynamics(belief, control, # current (b, u)
F, dt, A_fcn, C_fcn, # dynamics
Q, obs_cov_fcn): # covariances Q and R(x)
belief_next = cat.struct_SX(belief)
# Predict the mean
[mu_bar] = F([ belief['m'], control, dt ])
# Compute linearization
[A,_] = A_fcn([ belief['m'], control, dt ])
[C,_] = C_fcn([ mu_bar ])
# Predict the covariance
S_bar = ca.mul([ A, belief['S'], A.T ]) + Q
# Get the observations noise covariance
[R] = obs_cov_fcn([ mu_bar ])
# Compute the inverse
P = ca.mul([ C, S_bar, C.T ]) + R
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([ S_bar, C.T, P_inv ])
# Update equations
nx = belief['m'].size()
belief_next['m'] = mu_bar
belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
# (belief, control) -> next_belief
return ca.SXFunction('Belief dynamics',[belief,control],[belief_next])
def _create_BF(self):
"""Belief dynamics"""
b_next = cat.struct_SX(self.b)
# Compute the mean
[mu_bar] = self.F([self.b['m'], self.u])
# Compute linearization
[A, _] = self.Fj_x([self.b['m'], self.u])
[C, _] = self.hj_x([mu_bar])
# Get system and observation noises, as if the state was mu_bar
[M] = self.M([self.b['m'], self.u])
[N] = self.N([mu_bar])
# Predict the covariance
S_bar = ca.mul([A, self.b['S'], A.T]) + M
# Compute the inverse
P = ca.mul([C, S_bar, C.T]) + N
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([S_bar, C.T, P_inv])
# Update equations
b_next['m'] = mu_bar
b_next['S'] = ca.mul(ca.DMatrix.eye(self.nx) - ca.mul(K, C), S_bar)
# (b, u) -> b_next
op = {'input_scheme': ['b', 'u'],
'output_scheme': ['b_next']}
return ca.SXFunction('Belief dynamics',
[self.b, self.u], [b_next], op)
def makeOrthonormal(self, g, R):
g.add(C.mul(R[0, :], R[0, :].T), '==', 1, tag = ('R1[0]: e1^T * e1 == 1', None))
g.add(C.mul(R[1, :], R[0, :].T), '==', 0, tag = ('R1[0]: e2^T * e1 == 0', None))
g.add(C.mul(R[1, :], R[1, :].T), '==', 1, tag = ('R1[0]: e2^T * e2 == 1', None))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
g.add(rhon[0], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
g.add(rhon[2], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
g.add(rhon[1], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
def T2W(T, p, dp):
"""
w_101 = T2W(T_10,p,dp)
"""
R = T2R(T)
dR = c.reshape(c.mul(c.jacobian(R, p), dp), (3, 3))
return invskew(c.mul(R.T, dR))
def T2W(T,p,dp): """ w_101 = T2W(T_10,p,dp) """ R = T2R(T) dR = c.reshape(c.mul(c.jacobian(R,p),dp),(3,3)) return invskew(c.mul(R.T,dR))
def makeOrthonormal(g,R):
g.add(C.mul(R[0,:],R[0,:].T),'==',1, tag=('R1[0]: e1^T * e1 == 1',None))
g.add(C.mul(R[1,:],R[0,:].T),'==',0, tag=('R1[0]: e2^T * e1 == 0',None))
g.add(C.mul(R[1,:],R[1,:].T),'==',1, tag=('R1[0]: e2^T * e2 == 1',None))
rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
g.add(rhon[0],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0',None))
g.add(rhon[2],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0',None))
g.add(rhon[1],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0',None))
def get_orthonormal_constraints(R):
ret = []
ret.append( (C.mul(R[0,:],R[0,:].T) - 1, 'R1[0]: e1^T * e1 - 1 == 0') )
ret.append( (C.mul(R[1,:],R[0,:].T), 'R1[0]: e2^T * e1 == 0') )
ret.append( (C.mul(R[1,:],R[1,:].T) - 1, 'R1[0]: e2^T * e2 - 1 == 0') )
rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
ret.append( (rhon[0,0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0') )
ret.append( (rhon[0,2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0') )
ret.append( (rhon[0,1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0') )
return ret
def get_orthonormal_constraints(R):
ret = []
ret.append((C.mul(R[0, :], R[0, :].T) - 1, 'R1[0]: e1^T * e1 - 1 == 0'))
ret.append((C.mul(R[1, :], R[0, :].T), 'R1[0]: e2^T * e1 == 0'))
ret.append((C.mul(R[1, :], R[1, :].T) - 1, 'R1[0]: e2^T * e2 - 1 == 0'))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
ret.append((rhon[0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0'))
ret.append((rhon[2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0'))
ret.append((rhon[1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0'))
return ret
def setQuadratureDdt(self,quadratureStateName,quadratureStateDotName,lookup,lagrangePoly,h,symbolicDvs):
if quadratureStateName in self._quadratures:
raise ValueError(quadratureStateName+" is not unique")
qStates = np.resize(np.array([None],dtype=C.MX),(self._nk+1,self._nicp,self._deg+1))
# set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid
try:
qStates[0,0,0] = 0*lookup(quadratureStateDotName,timestep=0,nicpIdx=0,degIdx=1)
except ValueError:
raise ValueError('quadrature derivative name \"'+quadratureStateDotName+
'\" is not a valid x/z/u/p/output')
ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:,1:])
ld0 = lagrangePoly.lDotAtTauRoot[1:,0]
l1 = lagrangePoly.lAtOne
# print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0]
breakQuadratureIntervals = True
if breakQuadratureIntervals:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
for j in range(1,self._deg+1)])
Qs = C.mul( ldInv, dQs)
for j in range(1,self._deg+1):
qStates[k,i,j] = qStates[k,i,0] + Qs[j-1]
m = C.mul( Qs.T, l1[1:])
if i < self._nicp - 1:
qStates[k,i+1,0] = qStates[k,i,0] + m
else:
qStates[k+1,0,0] = qStates[k,i,0] + m
else:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j)
for j in range(1,self._deg+1)])
Qs = C.mul( ldInv, dQs - ld0*qStates[k,i,0] )
for j in range(1,self._deg+1):
qStates[k,i,j] = Qs[j-1]
m = C.veccat( [qStates[k,i,0], Qs] )
m = C.mul( m.T, l1)
if i < self._nicp - 1:
qStates[k,i+1,0] = m
else:
qStates[k+1,0,0] = m
self._quadratures[quadratureStateName] = qStates
self._setupQuadratureFunctions(symbolicDvs)
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 makeModel(conf, propertiesDir = '../properties'):
print "\n#### File '%s' is old. It does not use the NED convention you should use carouselModel in offline_mhe_stuff instead.\n" \
% inspect.getfile(inspect.currentframe())
#input("Press Enter if you wish to continue...")
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
rA = conf['rArm']
g = conf['g']
pIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat')))
RIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat')))
ddpIMU = C.mul(R.T,ddp) - ddelta**2*C.mul(R.T,C.vertcat([x+rA,y,0])) + 2*ddelta*C.mul(R.T,C.vertcat([-dy,dx,0])) + dddelta*C.mul(R.T,C.vertcat([-y,x+rA,0])) + C.mul(R.T,C.vertcat([0,0,g]))
aBridle = C.cross(ddt_w_bn_b,pIMU)
dae['IMU_acceleration'] = C.mul(RIMU,ddpIMU+aBridle)
dae['IMU_angular_velocity'] = C.mul(RIMU,dae['w_bn_b'])
camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC1.dat'))),
'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC2.dat'))),
'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC1.dat'))),
'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC2.dat'))),
'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body1.dat'))),
'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body2.dat'))),
'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body3.dat')))}
dae['marker_positions'] = camModel.fullCamModel(dae,camConf)
dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
#dae['ConstDelta'] = dae['cos_delta']*dae['cos_delta'] + dae['sin_delta']*dae['sin_delta'] - 1
dae['ConstDelta'] = ( dae['cos_delta']**2 + dae['sin_delta']**2 - 1 )
return dae
def prior_construct(self, prior_info):
Pnlp = self._Pnlp
if prior_info['type'] == 'Ridge':
L2 = prior_info['L2']
prior = cas.mul(Pnlp.T, Pnlp) * L2
elif prior_info['type'] == 'normal':
mean = prior_info['mean']
cov = prior_info['cov']
dev = Pnlp - mean
prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev)
elif prior_info['type'] == 'normal_std':
mean = prior_info['mean']
std = prior_info['std']
dev = Pnlp - mean
prior = cas.sum_square(dev) / (2 * std**2)
elif prior_info['type'] == 'uniform':
prior = 0
elif prior_info['type'] == 'GP':
BEmean = prior_info['BEmean']
BEcov = prior_info['BEcov']
linear_BE2Ea = prior_info['BE2Ea']
Eacov = prior_info['Eacov']
_BE = Pnlp[self._NEa:]
_Ea = Pnlp[:self._NEa]
Eamean = cas.mul(linear_BE2Ea, _BE)
dev_BE = _BE - BEmean
dev_Ea = _Ea - Eamean
prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \
cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea)
self._prior_ = prior
return prior
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 prior_construct(self, prior_info):
Pnlp = self._Pnlp
if prior_info['type'] == 'Ridge':
L2 = prior_info['L2']
prior = cas.mul(Pnlp.T, Pnlp) * L2
elif prior_info['type'] == 'Gaussian':
mean = prior_info['mean']
cov = prior_info['cov']
dev = Pnlp - mean
prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev)
elif prior_info['type'] == 'GP':
BEmean = prior_info['BEmean']
BEcov = prior_info['BEcov']
linear_BE2Ea = prior_info['BE2Ea']
Eacov = prior_info['Eacov']
_BE = Pnlp[self._NEa:]
_Ea = Pnlp[:self._NEa]
Eamean = cas.mul(linear_BE2Ea, _BE)
dev_BE = _BE - BEmean
dev_Ea = _Ea - Eamean
prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \
cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea)
self._prior_ = prior
self.prob_func = cas.MXFunction('prob_func', [Pnlp], [prior])
return prior
def set_path(self, path, r2r=False):
"""Define an analytic expression of the geometric path.
Note: The path must be defined as a function of self.s[0].
Args:
path (list of SXMatrix): An expression of the geometric path as
a function of self.s[0]. Its dimension must equal self.sys.ny
r2r (boolean): Reparameterize path such that a rest to rest
transition is performed
Example:
>>> S = FlatSystem(2, 4)
>>> P = PathFollowing(S)
>>> P.set_path([P.s[0], P.s[0]])
"""
if isinstance(path, list):
path = cas.vertcat(path)
if r2r:
path = cas.substitute(path, self.s[0], self._r2r())
self.path[:, 0] = path
dot_s = cas.vertcat([self.s[1:], 0])
for i in range(1, self.sys.order + 1):
self.path[:,
i] = cas.mul(cas.jacobian(self.path[:, i - 1], self.s),
dot_s)
def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def evalspline(s, x):
# Evaluate spline with symbolic variable
# This is possible not the best way to implement this. The conditional node
# from casadi should be considered
Bl = s.basis
coeffs = s.coeffs
k = Bl.knots
basis = [[]]
for i in range(len(k) - 1):
if i < Bl.degree + 1 and Bl.knots[0] == Bl.knots[i]:
basis[-1].append((x >= Bl.knots[i])*(x <= Bl.knots[i + 1]))
else:
basis[-1].append((x > Bl.knots[i])*(x <= Bl.knots[i + 1]))
for d in range(1, Bl.degree + 1):
basis.append([])
for i in range(len(k) - d - 1):
b = 0 * x
bottom = k[i + d] - k[i]
if bottom != 0:
b = (x - k[i]) * basis[d - 1][i] / bottom
bottom = k[i + d + 1] - k[i + 1]
if bottom != 0:
b += (k[i + d + 1] - x) * basis[d - 1][i + 1] / bottom
basis[-1].append(b)
result = 0.
for l in range(len(Bl)):
result += mul(coeffs[l], basis[-1][l])
return result
def markers_from_pose_casadi(pose): T_body_to_anchor = unvectorize_se3(pose) markers_body = [m1_body, m2_body, m3_body] markers_body = map(append_one, markers_body) markers = SXMatrix.zeros(12,1) RPCinvs = [RPC1inv, RPC2inv] # These map points in arm frames to camera frames fs = [f1,f2] cs = [c1,c2] for ci in xrange(2): for mi in xrange(3): m_anchor = mul(T_body_to_anchor, markers_body[mi]) m_cam = mul(RPCinvs[ci],m_anchor) uv = project(fs[ci],cs[ci],m_cam) starti = ci*6+mi*2 markers[starti:starti+2,0] = uv return markers
def constrainInvariantErrs():
dcm = dae['dcm']
err = C.mul(dcm.T,dcm)
g.add( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
("initial dcm orthonormal",None))
g.add(dae['c'], '==', 0, tag=('c(0)==0',None))
g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
def set_path(self, path, r2r=False):
"""Define an analytic expression of the geometric path.
Note: The path must be defined as a function of self.s[0].
Args:
path (list of SXMatrix): An expression of the geometric path as
a function of self.s[0]. Its dimension must equal self.sys.ny
r2r (boolean): Reparameterize path such that a rest to rest
transition is performed
Example:
>>> S = FlatSystem(2, 4)
>>> P = PathFollowing(S)
>>> P.set_path([P.s[0], P.s[0]])
"""
if isinstance(path, list):
path = cas.vertcat(path)
if r2r:
path = cas.substitute(path, self.s[0], self._r2r())
self.path[:, 0] = path
dot_s = cas.vertcat([self.s[1:], 0])
for i in range(1, self.sys.order + 1):
self.path[:, i] = cas.mul(
cas.jacobian(self.path[:, i - 1], self.s), dot_s)
def getSteadyStateNlpFunctions(self, dae, ref_dict = {}):
# make steady state model
g = Constraints()
g.add(dae.getResidual(), '==', 0, tag = ('dae residual', None))
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(g, R_c2b)
g.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
g.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
constrainInvariantErrs()
# Rotational velocity time derivative
# NOTE: In the following constraint omega0 was hard-coded.
g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
for name in ['alpha_deg', 'beta_deg', 'cL']:
if name in ref_dict:
g.addBnds(dae[name], ref_dict[name], tag = (name, None))
dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
obj = 0
# for name in ['aileron', 'elevator', 'rudder', 'flaps']:
for name in dae.uNames():
if name in dae:
obj += dae[ name ] ** 2
return dvs, obj, g.getG(), g.getLb(), g.getUb()
def setupImplicitFunction(operAlpha, An, geom):
#setup function to obtain B and RHS for the Fourier series calc
def getBandRHS(alphaiLoc):
alphaLoc = geom.alphaGeometric(operAlpha) - alphaiLoc
clLoc = geom.clLoc(alphaLoc)
RHS = clLoc*geom.chordLoc
B = numpy.sin(numpy.outer(geom.thetaLoc, geom.sumN))*(4.0*geom.bref)
return (B,RHS)
alphaiLoc = []
for i in range(geom.n):
alphaiLoc.append(
C.inner_prod(geom.sumN*numpy.sin(geom.sumN*geom.thetaLoc[i])/numpy.sin(geom.thetaLoc[i]), An))
alphaiLoc = C.veccat(alphaiLoc)
(B,RHS) = getBandRHS(alphaiLoc)
makeMeZero = RHS - C.mul(B, An)
CL = An[0]*numpy.pi*geom.AR
CDi = 0.0
for i in range(geom.n):
k = geom.sumN[i]
CDi += k * An[i]**2 / An[0]**2
CDi *= numpy.pi*geom.AR*An[0]**2
return (makeMeZero, alphaiLoc, CL, CDi)
def constrainInvariantErrs():
dcm = ocp.lookup('dcm',timestep=0)
err = C.mul(dcm.T,dcm)
ocp.constrain( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
("initial dcm orthonormal",None))
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))
def matchDcms(ocp,R0,Rf):
err = C.mul(R0.T, Rf)
ocp.constrain(err[0,1], '==', 0, tag=('dcm matching',"01"))
ocp.constrain(err[0,2], '==', 0, tag=('dcm matching',"02"))
ocp.constrain(err[1,2], '==', 0, tag=('dcm matching',"12"))
ocp.constrain(err[0,0], '>=', 0.5, tag=('dcm matching',"00"))
ocp.constrain(err[1,1], '>=', 0.5, tag=('dcm matching',"11"))
def ext_belief_dynamics(ext_belief, control,
F, dt, A_fcn, C_fcn,
Q, obs_cov_fcn):
ext_belief_next = cat.struct_SX(ext_belief)
# Predict the mean
[mu_bar] = F([ ext_belief['m'], control, dt ])
# Compute linearization
[A,_] = A_fcn([ ext_belief['m'], control, dt ])
[C,_] = C_fcn([ mu_bar ])
# Predict the covariance
S_bar = ca.mul([ A, ext_belief['S'], A.T ]) + Q
# Get the observations noise covariance
[R] = obs_cov_fcn([ mu_bar ])
# Compute the inverse
P = ca.mul([ C, S_bar, C.T ]) + R
P_inv = ca.inv(P)
# Kalman gain
K = ca.mul([ S_bar, C.T, P_inv ])
# Update equations
nx = ext_belief['m'].size()
ext_belief_next['m'] = mu_bar
ext_belief_next['S'] = ca.mul(ca.DMatrix.eye(nx) - ca.mul(K,C), S_bar)
ext_belief_next['L'] = ca.mul([ A, ext_belief['L'], A.T ]) + \
ca.mul([ K, C, S_bar ])
# (ext_belief, control) -> next_ext_belief
return ca.SXFunction('Extended-belief dynamics',
[ext_belief,control],[ext_belief_next])
def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False):
if self._thermo_constraint_expression is None:
self.build_thermo_constraint(thermoTem=298.15)
Pnlp = self._Pnlp
ini_p = np.hstack([dE_start])
dev = Pnlp - ini_p
if py == 2:
object_fxn = cas.mul(dev.T, dev)
elif py == 3:
object_fxn = cas.dot(dev, dev)
nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression)
nlpopts = dict()
if py == 2:
nlpopts['max_iter'] = 500
nlpopts['tol'] = 1e-8
nlpopts['expect_infeasible_problem'] = 'yes'
nlpopts['hessian_approximation'] = 'exact'
nlpopts['jac_d_constant'] = 'yes'
elif py == 3:
nlpopts['ipopt.max_iter'] = 500
nlpopts['ipopt.tol'] = 1e-8
nlpopts['ipopt.expect_infeasible_problem'] = 'yes'
nlpopts['ipopt.hessian_approximation'] = 'exact'
nlpopts['ipopt.jac_d_constant'] = 'yes'
if py == 2:
solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts)
elif py == 3:
solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts)
# Bounds and initial guess
lbP = -np.inf * np.ones(self._Np)
ubP = np.inf * np.ones(self._Np)
for i in range(len(fix_Ea)):
if check_index(fix_Ea[i], self.dEa_index):
lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)]
for i in range(len(fix_BE)):
if check_index(fix_BE[i], self.dBE_index):
lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)]
lbG = 0 * np.ones(2 * self.nrxn)
ubG = np.inf * np.ones(2 * self.nrxn)
if not print_screen:
old_stdout = sys.stdout
sys.stdout = tempfile.TemporaryFile()
solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP)
dE_corr = solution['x'].full().T[0].tolist()
if not print_screen:
sys.stdout = old_stdout
return(dE_corr)
def matchDcms(ocp, R0, Rf):
err = C.mul(R0.T, Rf)
ocp.constrain(err[0, 1], '==', 0, tag=('dcm matching', "01"))
ocp.constrain(err[0, 2], '==', 0, tag=('dcm matching', "02"))
ocp.constrain(err[1, 2], '==', 0, tag=('dcm matching', "12"))
ocp.constrain(err[0, 0], '>=', 0.5, tag=('dcm matching', "00"))
ocp.constrain(err[1, 1], '>=', 0.5, tag=('dcm matching', "11"))
ocp.constrain(err[2, 2], '>=', 0.5, tag=('dcm matching', "22"))
def setup_oed(self, outputs, parameters, sigma, time_points, design="A"):
"""
Transforms an Optimization Problem into an Optimal Experimental Design problem.
Parameters::
outputs --
List of names for outputs.
Type: [string]
parameters --
List of names for parameters to estimate.
Type: [string]
sigma --
Experiment variance matrix.
Type: [[float]]
time_points --
List of measurement time points.
Type: [float]
design --
Design criterion.
Possible values: "A", "T"
"""
# Augment sensitivities and add timed variables
self.augment_sensitivities(parameters)
timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points)
# Create sensitivity and Fisher matrices
Q = []
for j in xrange(len(outputs)):
Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]])
for i in xrange(len(time_points))]))
Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs))
for j in xrange(len(outputs))])
# Define the objective
if design == "A":
b = casadi.MX.sym("b", Fisher.shape[1], 1)
Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b)
obj = casadi.trace(Fisher_inv)
elif design == "T":
obj = -casadi.trace(Fisher)
elif design == "D":
obj = -casadi.det(Fisher)
raise NotImplementedError("D-optimal design is not supported.")
else:
raise ValueError("Invalid design %s." % design)
old_obj = self.getObjective()
self.setObjective(old_obj + obj)
def constrainInvariantErrs():
dcm = mhe.lookup('dcm', timestep=0)
err = C.mul(dcm.T, dcm)
mhe.constrain(
C.veccat([
err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1],
err[0, 2], err[1, 2]
]), '==', 0)
mhe.constrain(mhe.lookup('c', timestep=0), '==', 0)
mhe.constrain(mhe.lookup('cdot', timestep=0), '==', 0)
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(ginv, R_c2b)
ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
ginv.add(di, '==', 0, tag = ('delta invariant', None))
ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
def construct_upd_x(self, build):
# define parameters
z_i = self.define_parameter('z_i', self.q_i_struct.shape[0])
z_ji = self.define_parameter('z_ji', self.q_ji_struct.shape[0])
l_i = self.define_parameter('l_i', self.q_i_struct.shape[0])
l_ji = self.define_parameter('l_ji', self.q_ji_struct.shape[0])
rho = self.define_parameter('rho')
# put them in the struct format
z_i = self.q_i_struct(z_i)
z_ji = self.q_ji_struct(z_ji)
l_i = self.q_i_struct(l_i)
l_ji = self.q_ji_struct(l_ji)
# get time info
t = self.define_symbol('t')
T = self.define_symbol('T')
t0 = t/T
# get (part of) variables
x_i = self._get_x_variables()
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([z_ji, l_ji], tf, self.q_ji)
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label][name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
for nghb in self.q_ji.keys():
z = z_ji[str(nghb), child.label, name]
l = l_ji[str(nghb), child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
self.define_objective(obj)
# construct problem
self.father = OptiFather(self.group.values())
prob, compile_time = self.father.construct_problem(self.options, build)
self.problem_upd_x = prob
self.father.init_transformations(self.problem.init_primal_transform,
self.problem.init_dual_transform)
self.init_var_admm()
def constrainInvariantErrs():
dcm = ocp.lookup("dcm", timestep=0)
err = C.mul(dcm.T, dcm)
ocp.constrain(
C.veccat([err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2], err[1, 2]]),
"==",
0,
tag=("initial dcm", None),
)
ocp.constrain(ocp.lookup("c", timestep=0), "==", 0, tag=("initial c", None))
ocp.constrain(ocp.lookup("cdot", timestep=0), "==", 0, tag=("initial cdot", None))
def _create_final_cost(self, w_cl):
# Final position
x_b = self.x[ca.veccat, ['x_b', 'y_b']]
x_c = self.x[ca.veccat, ['x_c', 'y_c']]
dx_bc = x_b - x_c
final_cost = 0.5 * ca.mul(dx_bc.T, dx_bc)
op = {'input_scheme': ['x'],
'output_scheme': ['cl']}
return ca.SXFunction('Final cost', [self.x],
[w_cl * final_cost], op)
def set_path(self, paths):
"""The path is defined as the convex combination of the paths in paths.
Args:
paths (list of lists of SXMatrix): The path is taken as the
convex combination of the paths in paths.
Example:
The path is defined as the convex combination of
(s, 0.5*s) and (2, 2*s):
>>> P.set_path([(P.s[0], 0.5 * P.s[0]), [P.s[0], 2 * P.s[0]]])
"""
l = len(paths)
self.h = cas.ssym("h", l, self.sys.order + 1)
self.path[:, 0] = np.sum(cas.SXMatrix(paths) * cas.horzcat([self.h[:, 0]] * len(paths[0])), axis=0)
dot_s = cas.vertcat([self.s[1:], 0])
dot_h = cas.horzcat([self.h[:, 1:], cas.SXMatrix.zeros(l, 1)])
for i in range(1, self.sys.order + 1): # Chainrule
self.path[:, i] = (cas.mul(cas.jacobian(self.path[:, i - 1], self.s), dot_s) +
sum([cas.mul(cas.jacobian(self.path[:, i - 1], self.h[j, :]), dot_h[j, :].trans()) for j in range(l)]) * self.s[1])
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
T, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mul(Tinv, cfs_sym)
fun = SXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
return fun([cfs, t_shift])[0]
else:
T, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def shift_knot1_fwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = MX.sym('cfs', cfs.shape)
t_shift_sym = MX.sym('t_shift')
T = shiftfirstknot_T(basis, t_shift_sym)
cfs2_sym = mul(T, cfs_sym)
fun = MXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
return fun([cfs, t_shift])[0]
else:
T = shiftfirstknot_T(basis, t_shift)
return T.dot(cfs)
def invert_se3(T): R = T[:3,:3] t = T[:3,3] if type(T)==casadi.SXMatrix: Tinv = casadi.SXMatrix(4,4) Tinv[:3,3] = -casadi.mul(R.T,t) else: Tinv = numpy.zeros((4,4)) Tinv[:3,3] = -numpy.dot(R.T,t) Tinv[:3,:3] = R.T Tinv[3,3] = 1 return Tinv
def fullCamModel(dae,conf):
PdatC1 = conf['PdatC1']
PdatC2 = conf['PdatC2']
RPC1 = conf['RPC1']
RPC2 = conf['RPC2']
pos_marker_body1 = conf['pos_marker_body1']
pos_marker_body2 = conf['pos_marker_body2']
pos_marker_body3 = conf['pos_marker_body3']
RpC1 = C.DMatrix.eye(4)
RpC1[0:3,0:3] = RPC1[0:3,0:3].T
RpC1[0:3,3] = C.mul(-RPC1[0:3,0:3].T,RPC1[0:3,3])
RpC2 = C.DMatrix.eye(4);
RpC2[0:3,0:3] = RPC2[0:3,0:3].T
RpC2[0:3,3] = C.mul(-RPC2[0:3,0:3].T,RPC2[0:3,3])
PC1 = C.SXMatrix(3,3)
PC1[0,0] = PdatC1[0]
PC1[1,1] = PdatC1[1]
PC1[0,2] = PdatC1[2]
PC1[1,2] = PdatC1[3]
PC1[2,2] = 1.0
PC2 = C.SXMatrix(3,3)
PC2[0,0] = PdatC2[0]
PC2[1,1] = PdatC2[1]
PC2[0,2] = PdatC2[2]
PC2[1,2] = PdatC2[3]
PC2[2,2] = 1.0
p = C.vertcat([dae['x'],dae['y'],dae['z']])
R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
uv_all = C.vertcat([C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body1)) ,\
C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body2)) ,\
C.vec(singleCamModel(p,R,RpC1[0:3,:],PC1,pos_marker_body3)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body1)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body2)) ,\
C.vec(singleCamModel(p,R,RpC2[0:3,:],PC2,pos_marker_body3))])
return uv_all
def getRobustSteadyStateNlpFunctions(self, dae, ref_dict = {}):
xDotSol, zSol = dae.solveForXDotAndZ()
ginv = Constraints()
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
self.makeOrthonormal(ginv, R_c2b)
ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
ginv.add(di, '==', 0, tag = ('delta invariant', None))
ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
("Rotational velocities", None))
constrainInvariantErrs()
invariants = ginv.getG()
J = C.jacobian(invariants,dae.xVec())
# make steady state model
g = Constraints()
xds = C.vertcat([xDotSol[ name ] for name in dae.xNames()])
jInv = C.mul(J.T,C.solve(C.mul(J,J.T),invariants))
g.add(xds - jInv - dae.xDotVec(), '==', 0, tag = ('dae residual', None))
for name in ['alpha_deg', 'beta_deg', 'cL']:
if name in ref_dict:
g.addBnds(dae[name], ref_dict[name], tag = (name, None))
dvs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
obj = 0
for name in dae.uNames() + ['aileron', 'elevator']:
if name in dae:
obj += dae[ name ] ** 2
return dvs, obj, g.getG(), g.getLb(), g.getUb(), zSol
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
def matchDcms(ocp, R0, Rf, tag=None):
err = C.mul(R0.T, Rf)
if tag is None:
tag = ''
else:
tag = ' ' + tag
ocp.constrain(err[0, 1], '==', 0, tag=('dcm matching 01' + tag, None))
ocp.constrain(err[0, 2], '==', 0, tag=('dcm matching 02' + tag, None))
ocp.constrain(err[1, 2], '==', 0, tag=('dcm matching 12' + tag, None))
ocp.constrain(err[0, 0], '>=', 0.5, tag=('dcm matching 00' + tag, None))
ocp.constrain(err[1, 1], '>=', 0.5, tag=('dcm matching 11' + tag, None))
ocp.constrain(err[2, 2], '>=', 0.5, tag=('dcm matching 22' + tag, None))
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
def T2WJ(T, p):
"""
w_101 = T2WJ(T_10,p).diff(p,t)
"""
R = T2R(T)
RT = R.T
temp = []
for i, k in [(2, 1), (0, 2), (1, 0)]:
#temp.append(c.mul(c.jacobian(R[:,k],p).T,R[:,i]).T)
temp.append(c.mul(RT[i, :], c.jacobian(R[:, k], p)))
return c.vertcat(temp)
def fwd(x_all, u_all, k_all, K_all, alpha, F, dt, state, control):
n_sim = len(u_all[:])
xk = x_all[0]
x_new = [xk]
u_new = []
for k in range(n_sim):
uk = u_all[k] + alpha * k_all[k] + ca.mul(K_all[k], xk - x_all[k])
u_new.append(uk)
[xk_next] = F([xk, uk, dt])
x_new.append(xk_next)
xk = xk_next
x_new = state.repeated(ca.horzcat(x_new))
u_new = control.repeated(ca.horzcat(u_new))
return [x_new, u_new]
def getWind():
if 'wind_model' not in conf:
return C.veccat([0, 0, 0])
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
# altitude is -z - altitude0, where altitude0 is a user parameter
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
altitude = -z - conf['wind_model']['altitude0']
wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
dae['wind_at_altitude'] = wind
return C.mul([R_g2c, C.veccat([wind, 0, 0])])
elif conf['wind_model']['name'] in ['constant','hardcoded']:
# constant wind
dae['wind_at_altitude'] = dae['w0']
return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
elif conf['wind_model']['name'] == 'random_walk':
dae.addU('delta_wind_x')
dae.addU('delta_wind_y')
dae.addU('delta_wind_z')
wind_x = dae.addX('wind_x')
wind_y = dae.addX('wind_y')
wind_z = dae.addX('wind_z')
dae['wind_at_altitude'] = wind_x
return C.veccat([wind_x, wind_y, wind_z])
def dt(self, expr):
"""Return time derivative of expr
The time derivative is computed using the chainrule:
df/dt = df/dy * dy/dt
Args:
expr (SXMatrix): casadi expression that is differentiated wrt time
Returns:
SXMatrix. The time derivative of expr
Example:
>>> S = FlatSystem(2, 4)
>>> dy00 = S.dt(S.y[0, 0])
"""
return cas.mul(cas.jacobian(expr, self.y), self._dy[:])
def constrainInvariantErrs():
dcm = ocp.lookup('dcm', timestep=0)
err = C.mul(dcm.T, dcm)
ocp.constrain(C.veccat([
err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2],
err[1, 2]
]),
'==',
0,
tag=("initial dcm orthonormal", None))
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))
def _make_path(self):
"""Rewrite the path as a function of the optimization variables.
Substitutes the time derivatives of s in the expression of the path by
expressions that are function of b and its path derivatives by
repeatedly applying the chainrule
Returns:
* SXMatrix. The substituted path
* SXMatrix. b and the path derivatives
* SXMatrix. The derivatives of s as a function of b
"""
b = cas.ssym("b", self.sys.order)
db = cas.vertcat((b[1:], 0))
Ds = cas.SXMatrix.nan(self.sys.order) # Time derivatives of s
Ds[0] = cas.sqrt(b[0])
Ds[1] = b[1] / 2
# Apply chainrule for finding higher order derivatives
for i in range(1, self.sys.order - 1):
Ds[i + 1] = (cas.mul(cas.jacobian(Ds[i], b), db) * self.s[1] +
cas.jacobian(Ds[i], self.s[1]) * Ds[1])
Ds = cas.substitute(Ds, self.s[1], cas.sqrt(b[0]))
return cas.substitute(self.path, self.s[1:], Ds), b, Ds
def setUp(self):
# System
self.u = ca.MX.sym("u", 2)
self.p = ca.MX.sym("p", 2)
self.phi = self.u[0] * self.p[0] + self.u[1] * self.p[1]**2
self.g = (2 - ca.mul(self.p.T, self.p))
self.bsys = pecas.systems.BasicSystem(u = self.u, p = self.p, \
phi = self.phi, g = self.g)
# Inputs
self.tu = np.linspace(0, 3, 4)
self.invalidpargs = [[0, 1, 2], [[2, 2, 3], [2, 2, 3]], \
np.asarray([1, 2, 2]), np.asarray([[2, 3, 3], [2, 3, 3]])]
self.validpargs = [None, [0, 1], np.asarray([1, 1]), \
np.asarray([1, 2]).T, np.asarray([[2], [2]])]
self.invaliduargs = [np.ones((self.u.size(), \
self.tu.size - 1)), \
np.ones((self.tu.size - 1, self.u.size()))]
self.validuargs = [None, np.ones((self.u.size(), self.tu.size)), \
np.ones((self.tu.size, self.u.size()))]
self.yN = np.asarray([2.23947, 2.84568, 4.55041, 5.08583])
self.wv = np.asarray([1.0 / (0.5**2)] * 4)
self.uN = np.vstack([np.ones(4), np.linspace(1, 4, 4)])
self.pinit = [1, 1]
self.phat = np.atleast_2d([0.961943, 1.03666]).T
def aeroForcesTorques(dae, conf, v_bw_n, v_bw_b, w_bn_b, (eTe1, eTe2, eTe3)):
rho = conf['rho']
alpha0 = conf['alpha0deg'] * C.pi / 180
sref = conf['sref']
bref = conf['bref']
cref = conf['cref']
# airfoil speed in wind frame
v_bw_n_x = v_bw_n[0]
v_bw_n_y = v_bw_n[1]
v_bw_n_z = v_bw_n[2]
vKite2 = C.mul(v_bw_n.trans(), v_bw_n) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
dae['airspeed'] = vKite
# Lift axis, normed to airspeed
eLe_v = C.cross(C.veccat([eTe1, eTe2, eTe3]), v_bw_n)
# sideforce axis, normalized to airspeed^2
eYe_v2 = C.cross(eLe_v, -v_bw_n)
# Relative wind speed in Airfoil's referential 'E'
v_bw_b_x = v_bw_b[0]
v_bw_b_y = v_bw_b[1]
v_bw_b_z = v_bw_b[2]
if conf['alpha_beta_computation'] == 'first_order':
def getSteadyState(dae, conf, omega0, r0, z0):
# make steady state model
g = Constraints()
g.add(dae.getResidual(), '==', 0, tag=('dae residual', None))
def constrainInvariantErrs():
R_c2b = dae['R_c2b']
makeOrthonormal(g, R_c2b)
g.add(dae['c'], '==', 0, tag=('c(0)==0', None))
g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0', None))
constrainInvariantErrs()
# Rotational velocity time derivative
g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, omega0]),
'==',
0,
tag=("Rotational velocities", None))
g.addBnds(dae['alpha_deg'], (-4.0, 8.0), tag=("alpha deg", None))
g.addBnds(dae['beta_deg'], (-7.0, 7.0), tag=("beta deg", None))
dvs = C.veccat(
[dae.xVec(),
dae.zVec(),
dae.uVec(),
dae.pVec(),
dae.xDotVec()])
# ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
ffcn = C.SXFunction([dvs], [(dae['cL'] - 0.5)**2])
gfcn = C.SXFunction([dvs], [g.getG()])
ffcn.init()
gfcn.init()
guess = {
'x': r0,
'y': 0,
'z': 0,
'r': r0,
'dr': 0,
'e11': 0,
'e12': 1,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': -1,
'e31': -1,
'e32': 0,
'e33': 0,
'dx': 0,
'dy': 0,
'dz': 0,
'w_bn_b_x': 0,
'w_bn_b_y': -omega0,
'w_bn_b_z': 0,
'ddelta': omega0,
'cos_delta': 1,
'sin_delta': 0,
'aileron': 0,
'elevator': 0,
'daileron': 0,
'delevator': 0,
'nu': 100,
'motor_torque': 10,
'dmotor_torque': 0,
'ddr': 0,
'dddr': 0.0,
'w0': 0.0
}
dotGuess = {
'x': 0,
'y': 0,
'z': 0,
'dx': 0,
'dy': 0,
'dz': 0,
'r': 0,
'dr': 0,
'e11': 0,
'e12': 0,
'e13': 0,
'e21': 0,
'e22': 0,
'e23': 0,
'e31': 0,
'e32': 0,
'e33': 0,
'w_bn_b_x': 0,
'w_bn_b_y': 0,
'w_bn_b_z': 0,
'ddelta': 0,
'cos_delta': 0,
'sin_delta': omega0,
'aileron': 0,
'elevator': 0,
'motor_torque': 0,
'ddr': 0
}
guessVec = C.DMatrix([
guess[n]
for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
] + [dotGuess[n] for n in dae.xNames()])
bounds = {
'x': (0.01, r0 * 2),
'y': (-r0, r0),
'z': (z0, z0),
'dx': (-50, 50),
'dy': (0, 0),
'dz': (0, 0),
'r': (r0, r0),
'dr': (0, 0),
'e11': (-2, 2),
'e12': (-2, 2),
'e13': (-2, 2),
'e21': (-2, 2),
'e22': (-2, 2),
'e23': (-2, 2),
'e31': (-2, 0),
'e32': (-2, 2),
'e33': (-2, 2),
'w_bn_b_x': (-50, 50),
'w_bn_b_y': (-50, 50),
'w_bn_b_z': (-50, 50),
'ddelta': (omega0, omega0),
'cos_delta': (1, 1),
'sin_delta': (0, 0),
'aileron': (-0.2, 0.2),
'elevator': (-0.2, 0.2),
'daileron': (0, 0),
'delevator': (0, 0),
'nu': (0, 3000),
'motor_torque': (0, 1000),
'ddr': (0, 0),
'dmotor_torque': (0, 0),
'dddr': (0, 0),
'w0': (0, 0)
}
dotBounds = {
'x': (-50, 50),
'y': (-50, 50),
'z': (-50, 50),
'dx': (0, 0),
'dy': (-50, 50),
'dz': (0, 0),
'r': (-1, 1),
'dr': (-1, 1),
'e11': (-50, 50),
'e12': (-50, 50),
'e13': (-50, 50),
'e21': (-50, 50),
'e22': (-50, 50),
'e23': (-50, 50),
'e31': (-50, 50),
'e32': (-50, 50),
'e33': (-50, 50),
'w_bn_b_x': (0, 0),
'w_bn_b_y': (0, 0),
'w_bn_b_z': (0, 0),
'ddelta': (0, 0),
'cos_delta': (-1, 1),
'sin_delta': (omega0 - 1, omega0 + 1),
'aileron': (-1, 1),
'elevator': (-1, 1),
'motor_torque': (-1000, 1000),
'ddr': (-100, 100)
}
boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
[dotBounds[n] for n in dae.xNames()]
# gfcn.setInput(guessVec)
# gfcn.evaluate()
# ret = gfcn.output()
# for k,v in enumerate(ret):
# if math.isnan(v):
# print 'index ',k,' is nan: ',g._tags[k]
# import sys; sys.exit()
# context = zmq.Context(1)
# publisher = context.socket(zmq.PUB)
# publisher.bind("tcp://*:5563")
# class MyCallback:
# def __init__(self):
# import rawekite.kiteproto as kiteproto
# import rawekite.kite_pb2 as kite_pb2
# self.kiteproto = kiteproto
# self.kite_pb2 = kite_pb2
# self.iter = 0
# def __call__(self,f,*args):
# x = C.DMatrix(f.input('x'))
# sol = {}
# for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
# sol[name] = x[k].at(0)
# lookup = lambda name: sol[name]
# kp = self.kiteproto.toKiteProto(lookup,
# lineAlpha=0.2)
# mc = self.kite_pb2.MultiCarousel()
# mc.horizon.extend([kp])
# mc.messages.append("iter: "+str(self.iter))
# self.iter += 1
# publisher.send_multipart(["multi-carousel", mc.SerializeToString()])
solver = C.IpoptSolver(ffcn, gfcn)
def addCallback():
nd = len(boundsVec)
nc = g.getLb().size()
c = C.PyFunction(
MyCallback(),
C.nlpsolverOut(x=C.sp_dense(nd, 1),
f=C.sp_dense(1, 1),
lam_x=C.sp_dense(nd, 1),
lam_p=C.sp_dense(0, 1),
lam_g=C.sp_dense(nc, 1),
g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)])
c.init()
solver.setOption("iteration_callback", c)
# addCallback()
solver.setOption('max_iter', 10000)
# solver.setOption('tol',1e-14)
# solver.setOption('suppress_all_output','yes')
# solver.setOption('print_time',False)
solver.init()
solver.setInput(g.getLb(), 'lbg')
solver.setInput(g.getUb(), 'ubg')
solver.setInput(guessVec, 'x0')
lb, ub = zip(*boundsVec)
solver.setInput(C.DMatrix(lb), 'lbx')
solver.setInput(C.DMatrix(ub), 'ubx')
solver.solve()
ret = solver.getStat('return_status')
assert ret in ['Solve_Succeeded',
'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret
# publisher.close()
# context.destroy()
xOpt = solver.output('x')
k = 0
sol = {}
for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
sol[name] = xOpt[k].at(0)
k += 1
# print name+':\t',sol[name]
dotSol = {}
for name in dae.xNames():
dotSol[name] = xOpt[k].at(0)
k += 1
# print 'DDT('+name+'):\t',dotSol[name]
return sol, dotSol
def setQuadratureDdt(self, quadratureStateName, quadratureStateDotName,
lookup, lagrangePoly, h, symbolicDvs):
if quadratureStateName in self._quadratures:
raise ValueError(quadratureStateName + " is not unique")
qStates = np.resize(np.array([None], dtype=C.MX),
(self._nk + 1, self._nicp, self._deg + 1))
# set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid
try:
qStates[0, 0, 0] = 0 * lookup(
quadratureStateDotName, timestep=0, nicpIdx=0, degIdx=1)
except ValueError:
raise ValueError('quadrature derivative name \"' +
quadratureStateDotName +
'\" is not a valid x/z/u/p/output')
ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:, 1:])
ld0 = lagrangePoly.lDotAtTauRoot[1:, 0]
l1 = lagrangePoly.lAtOne
# print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0]
breakQuadratureIntervals = True
if breakQuadratureIntervals:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h * C.veccat([
lookup(quadratureStateDotName,
timestep=k,
nicpIdx=i,
degIdx=j) for j in range(1, self._deg + 1)
])
Qs = C.mul(ldInv, dQs)
for j in range(1, self._deg + 1):
qStates[k, i, j] = qStates[k, i, 0] + Qs[j - 1]
m = C.mul(Qs.T, l1[1:])
if i < self._nicp - 1:
qStates[k, i + 1, 0] = qStates[k, i, 0] + m
else:
qStates[k + 1, 0, 0] = qStates[k, i, 0] + m
else:
for k in range(self._nk):
for i in range(self._nicp):
dQs = h * C.veccat([
lookup(quadratureStateDotName,
timestep=k,
nicpIdx=i,
degIdx=j) for j in range(1, self._deg + 1)
])
Qs = C.mul(ldInv, dQs - ld0 * qStates[k, i, 0])
for j in range(1, self._deg + 1):
qStates[k, i, j] = Qs[j - 1]
m = C.veccat([qStates[k, i, 0], Qs])
m = C.mul(m.T, l1)
if i < self._nicp - 1:
qStates[k, i + 1, 0] = m
else:
qStates[k + 1, 0, 0] = m
self._quadratures[quadratureStateName] = qStates
self._setupQuadratureFunctions(symbolicDvs)
import casadi as c
import numpy as n
N = 5000
# We work with a sparse diaginal matrix with 5000 elements
x = DM(sp_diag(N), 5)
x_s = x.toCsr_matrix()
t = time()
dummy = n.dot(x_s.T, x_s)
print "Scipy pure = %.4f s" % (time() - t)
t = time()
dummy = c.mul(x.T, x)
print "CasADi pure = %.4f s" % (time() - t)
X = MX("X", x.sparsity())
t = time()
f = MXFunction([X], [c.mul(X.T, X)])
f.init()
print "CasADi MX wrapped init overhead = %.4f s" % (time() - t)
f.setInput(x)
t = time()
f.evaluate()
print "CasADi MX wrapped = %.4f s" % (time() - t)
t = time()
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']+'"')
if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
dae.addP( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
else:
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
def setupModel(dae, conf):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
mass matrix columns:
ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
rhs:
forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
'''
# Parameters
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
# Frames
# ==========================================
# World frame (n) NED North-East-Down
#
# Wind carrying frame (w)
# - Translates along the world frame's x axis with the wind speed
#
# Carousel frame (c)
# - Origin coincides with the world origin
# - Makes and angle delta
#
# Carousel tip frame (a)
# - Origin sits at tip of arm
# - e_x extends radially outwards
# - e_z points downwards
#
# Body frame (b)
# - attached to body of aircraft
# - e_x extends to the nose
# - e_z points downwards
#
# Vector naming convention
# =========================
# v_ab_c
# Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy)
# Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology)
# States
# Components that make up the rotation matrix from carousel frame to body frame R_c2b [-]
# e11 e12 e13
# e21 e22 e23
# e31 e32 e33
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x'] # Aircraft position in carousel tip frame coordinates [m]
y = dae['y']
z = dae['z']
dx = dae['dx'] # Time derivatives of x [m/s]
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2]
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta'] # Carousel turning speed [rad/s]
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
if 'xt' in conf:
t_xt = dae['tether_tension'] * conf['xt']
else:
t_xt = 0.0
R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0],
[dae['sin_delta'], dae['cos_delta'], 0.0],
[0.0, 0.0, 1.0]])
# wind model
def getWind():
if 'wind_model' not in conf:
return C.veccat([0, 0, 0])
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
# altitude is -z - altitude0, where altitude0 is a user parameter
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
altitude = -z - conf['wind_model']['altitude0']
wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
dae['wind_at_altitude'] = wind
return C.mul([R_g2c, C.veccat([wind, 0, 0])])
elif conf['wind_model']['name'] in ['constant','hardcoded']:
# constant wind
dae['wind_at_altitude'] = dae['w0']
return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
elif conf['wind_model']['name'] == 'random_walk':
dae.addU('delta_wind_x')
dae.addU('delta_wind_y')
dae.addU('delta_wind_z')
wind_x = dae.addX('wind_x')
wind_y = dae.addX('wind_y')
wind_z = dae.addX('wind_z')
dae['wind_at_altitude'] = wind_x
return C.veccat([wind_x, wind_y, wind_z])
wind = getWind()
# Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame
v_bw_c = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - wind
# Velocity of aircraft w.r.t wind carrying frame, expressed in body frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( dae['R_c2b'], v_bw_c )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates
)
# f1..f3 expressed in carrousel coordinates
# t1..t3 expressed in body coordinates
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
fx_dist = 0
fy_dist = 0
fz_dist = 0
mx_dist = 0
my_dist = 0
mz_dist = 0
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 == 'random_walk':
if _which[ 0 ]:
dae.addU('df1_disturbance')
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
dae.addU('df2_disturbance')
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
dae.addU('df3_disturbance')
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addP('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addP('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addP('f3_disturbance')
else:
raise ValueError("WTF?")
f1 += fx_dist
f2 += fy_dist
f3 += fz_dist
dae["aero_fx_dist"] = fx_dist
dae["aero_fy_dist"] = fy_dist
dae["aero_fz_dist"] = fz_dist
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 ]:
dae.addU('dt1_disturbance')
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
dae.addU('dt2_disturbance')
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
dae.addU('dt3_disturbance')
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance')
else:
raise ValueError("WTF?")
t1 += mx_dist
t2 += my_dist
t3 += mz_dist
dae["aero_mx_dist"] = mx_dist
dae["aero_my_dist"] = my_dist
dae["aero_mz_dist"] = mz_dist
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(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)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, 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) + t_xt
, 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
# The cable constraint
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['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
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['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
