def acc_rhs(self, q, qdot):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
qi_dot = qdot[self.i_body.dic.index]
qj_dot = qdot[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
v1 = self.v3
v2 = self.v2
Biv1 = B(betai, v1)
Bjv2 = B(betaj, v1)
Hiv1 = B(betai_dot, v1)
Hjv2 = B(betaj_dot, v2)
rhs = acc_dp1_rhs(v1, Ai, Biv1, Hiv1, bid, v2, Aj, Bjv2, Hjv2,
bjd) + self.acc
return rhs
def jacobian_j(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
v1 = Ai.dot(self.vii)
v2 = Ai.dot(self.vij)
Hjv3 = B(betaj, self.vjk)
Hjv4 = B(betaj, self.vjj)
Hjup = B(betaj, self.u_j)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat([[Z, v1.T.dot(Hjv3)], [Z, v2.T.dot(Hjv3)],
[-v1.T, -v1.T.dot(Hjup)], [-v2.T, -v2.T.dot(Hjup)],
[Z, v1.T.dot(Hjv4)]],
format='csr')
return jac
def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
v1 = Ai.dot(self.vii)
v2 = Ai.dot(self.vij)
v3 = Aj.dot(self.vjk)
v4 = Aj.dot(self.vjj)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j) + 10 * v3
Hiv1 = B(betai, self.vii)
Hiv2 = B(betai, self.vij)
Hiup = B(betai, self.u_i)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat(
[[Z, v3.T.dot(Hiv1)], [Z, v3.T.dot(Hiv2)],
[v1.T, rij.T.dot(Hiv1) + v1.T.dot(Hiup)],
[v2.T, rij.T.dot(Hiv2) + v2.T.dot(Hiup)], [Z, v4.T.dot(Hiv1)]],
format='csr')
return jac
def acc_rhs(self, q, qdot):
qi_dot = qdot[self.i_body.dic.index]
qj_dot = qdot[self.j_body.dic.index]
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
Hip = B(betai_dot, self.u_i)
Hjp = B(betaj_dot, self.u_j)
return acc_sph_rhs(betai_dot, Hip, Hjp, betaj_dot)
def equation(self, q, qdot):
qi = q[self.bodyi.dic.index]
qj = q[self.bodyj.dic.index]
qi_dot = qdot[self.bodyi.dic.index]
qj_dot = qdot[self.bodyj.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
l = np.linalg.norm(rij)
defflection = max([0, self.lf - l])
nij = rij / l
self.defflection = defflection
Ri_dot = qi_dot[0:3].values.reshape((3, 1))
Rj_dot = qj_dot[0:3].values.reshape((3, 1))
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
Bip = B(betai, self.u_i)
Bjp = B(betaj, self.u_j)
rij_dot = Ri_dot + Bip.dot(bid) - Rj_dot - Bjp.dot(bjd)
velocity = nij.T.dot(rij_dot)
self.velocity = velocity
self.springforce = self.k * defflection
self.damperforce = self.c * velocity
force = self.k * (defflection) + self.c * velocity + self.h
fi = float(force) * nij
ni = 2 * G(betai).T.dot(vec2skew(self.u_i).dot(Ai.T.dot(fi)))
Qi = np.bmat([[fi], [ni]])
fj = -float(force) * nij
nj = 2 * G(betaj).T.dot(vec2skew(self.u_j).dot(Aj.T.dot(fj)))
Qj = np.bmat([[fj], [nj]])
return Qi, Qj
def acc_rhs(self, q, qdot):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
qi_dot = qdot[self.i_body.dic.index]
qj_dot = qdot[self.j_body.dic.index]
Ri = qi[0:3].values.reshape((3, 1))
Rj = qj[0:3].values.reshape((3, 1))
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
Ri_dot = qi_dot[0:3].values.reshape((3, 1))
Rj_dot = qj_dot[0:3].values.reshape((3, 1))
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
v1 = self.vii
v2 = self.vij
v3 = self.vjk
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
Biv1 = B(betai, v1)
Biv2 = B(betai, v2)
Bjv3 = B(betaj, v3)
Bip = B(betai, self.u_i)
Bjp = B(betaj, self.u_j)
Hiv1 = B(betai_dot, v1)
Hiv2 = B(betai_dot, v2)
Hjv3 = B(betaj_dot, v3)
Hip = B(betai_dot, self.u_i)
Hjp = B(betaj_dot, self.u_j)
rij_dot = Ri_dot + Bip.dot(bid) - Rj_dot - Bjp.dot(bjd)
rhs1 = acc_dp1_rhs(v1, Ai, Biv1, Hiv1, bid, v3, Aj, Bjv3, Hjv3, bjd)
rhs2 = acc_dp1_rhs(v2, Ai, Biv2, Hiv2, bid, v3, Aj, Bjv3, Hjv3, bjd)
rhs3 = acc_dp2_rhs(v1, Ai, Biv1, Hiv1, bid, rij, Hip, Hjp, bjd,
rij_dot)
rhs4 = acc_dp2_rhs(v2, Ai, Biv2, Hiv2, bid, rij, Hip, Hjp, bjd,
rij_dot)
return np.concatenate([rhs1, rhs2, rhs3, rhs4])
def jacobian_j(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
h1 = Ai.dot(self.h_i)
I = sparse.eye(3, format='csr')
Hjup = B(betaj, self.u_j)
Hjh2 = B(betaj, self.h_j)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat([[-I, -Hjup], [Z, h1.T.dot(Hjh2)]], format='csr')
return jac
def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Aj = ep2dcm(betaj)
h2 = Aj.dot(self.h_j)
I = sparse.eye(3, format='csr')
Hiup = B(betai, self.u_i)
Hih1 = B(betai, self.h_i)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat([[I, Hiup], [Z, h2.T.dot(Hih1)]], format='csr')
return jac
def jacobian_j(self, q):
qj = q[self.j_body.dic.index]
betaj = qj[3:]
Hp = B(betaj, self.u_j)
I = sparse.eye(3, format='csr')
jac = sparse.bmat([[-I, -Hp]], format='csr')
return jac
def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
betai = qi[3:]
Hp = B(betai, self.u_i)
I = sparse.eye(3, format='csr')
jac = sparse.bmat([[I, Hp]], format='csr')
return jac
def acc_rhs(self, q, qdot):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
qi_dot = qdot[self.i_body.dic.index]
qj_dot = qdot[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
h1 = self.h_i
h2 = self.h_j
Bih1 = B(betai, h1)
Bjh2 = B(betaj, h2)
Hih1 = B(betai_dot, h1)
Hjh2 = B(betaj_dot, h2)
Hip = B(betai_dot, self.u_i)
Hjp = B(betaj_dot, self.u_j)
rhs123 = acc_sph_rhs(betai_dot, Hip, Hjp, betaj_dot)
rhs4 = acc_dp1_rhs(h1, Ai, Bih1, Hih1, bid, h2, Aj, Bjh2, Hjh2, bjd)
return np.concatenate([rhs123, rhs4])
def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Aj = ep2dcm(betaj)
v3 = Aj.dot(self.vjk)
I = sparse.eye(3, format='csr')
Hiup = B(betai, self.u_i)
Hiv1 = B(betai, self.vii)
Hiv2 = B(betai, self.vij)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat(
[[I, Hiup], [Z, v3.T.dot(Hiv1)], [Z, v3.T.dot(Hiv2)]],
format='csr')
return jac
def jacobian_j(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
v1 = Ai.dot(self.v3)
Hjv2 = B(betaj, self.v2)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat([[Z, v1.T.dot(Hjv2)]], format='csr')
# print('jacj = %s'%jac.A)
return jac
def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Aj = ep2dcm(betaj)
v2 = Aj.dot(self.v2)
Hiv1 = B(betai, self.v3)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat([[Z, v2.T.dot(Hiv1)]], format='csr')
# print('jaci = %s'%jac.A)
return jac
def acc_rhs(self, q, qdot):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
qi_dot = qdot[self.i_body.dic.index]
qj_dot = qdot[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
v1 = self.vii
v2 = self.vij
v3 = self.vjk
Biv1 = B(betai, v1)
Biv2 = B(betai, v2)
Bjv3 = B(betaj, v3)
Hiv1 = B(betai_dot, v1)
Hiv2 = B(betai_dot, v2)
Hjv3 = B(betaj_dot, v3)
Hip = B(betai_dot, self.u_i)
Hjp = B(betaj_dot, self.u_j)
rhs123 = acc_sph_rhs(betai_dot, Hip, Hjp, betaj_dot)
rhs4 = acc_dp1_rhs(v1, Ai, Biv1, Hiv1, bid, v3, Aj, Bjv3, Hjv3, bjd)
rhs5 = acc_dp1_rhs(v2, Ai, Biv2, Hiv2, bid, v3, Aj, Bjv3, Hjv3, bjd)
return np.concatenate([rhs123, rhs4, rhs5])
