def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
v1 = self.vii
v2 = self.vij
v3 = self.vjk
v4 = self.vjj
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j) + 10 * v3
eq1 = np.linalg.multi_dot([v1.T, Ai.T, Aj, v3])
eq2 = np.linalg.multi_dot([v2.T, Ai.T, Aj, v3])
eq3 = np.linalg.multi_dot([v1.T, Ai.T, rij])
eq4 = np.linalg.multi_dot([v2.T, Ai.T, rij])
eq5 = np.linalg.multi_dot([v1.T, Ai.T, Aj, v4])
c = [eq1, eq2, eq3, eq4, eq5]
return np.array([c]).reshape((5, 1))
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 = 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 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 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
v4 = self.vjj
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)
Bjv4 = B(betaj, v4)
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)
Hjv4 = B(betaj_dot, v4)
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)
rhs5 = acc_dp1_rhs(v1, Ai, Biv1, Hiv1, bid, v4, Aj, Bjv4, Hjv4, bjd)
return np.concatenate([rhs1, rhs2, rhs3, rhs4, rhs5])
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 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 equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
eq1, eq2, eq3 = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
eq4 = np.linalg.multi_dot([self.h_i.T, Ai.T, Aj, self.h_j])
c = [float(i) for i in (eq1, eq2, eq3, eq4)]
return np.array([c]).reshape((4, 1))
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
ui = self.u_i
uj = self.u_j
r_p = Ri + Ai.dot(ui) - Rj - Aj.dot(uj)
return r_p
def joint_pos(self, q):
qi = q[self.i_body.dic.index]
Ri = vector(qi[0:3]).a
Ai = ep2dcm(qi[3:])
ui = self.u_i
r_p = Ri + Ai.dot(ui)
return r_p
def equation(self, q, v):
qi = q[self.bodyi.dic.index]
betai = qi[3:]
Ai = ep2dcm(betai)
F = self.F
M = 2 * G(betai).T.dot(vec2skew(self.u_i).dot(Ai.T.dot(F)))
Qi = np.bmat([[F], [M]])
return Qi
def equation(self, q, qdot):
qi = q[self.bodyi.dic.index]
betai = qi[3:]
Ai = ep2dcm(betai)
rw = qi[self.bodyi.name + '.z']
rzdot = qdot[self.bodyi.name + '.z']
x = max([0, self.r - rw])
F = np.array([[0, 0, self.k * x + self.c * rzdot]]).T
Z = np.zeros((4, 1))
M = 2 * G(betai).T.dot(vec2skew(self.u_i).dot(Ai.T.dot(F)))
Qi = np.bmat([[F], [Z]])
return Qi
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])
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
v1 = Ai.dot(self.vii)
v2 = Ai.dot(self.vij)
v3 = Aj.dot(self.vjk)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
eq1, eq2, eq3 = rij
eq4 = v1.T.dot(v3)
eq5 = v2.T.dot(v3)
c = [float(i) for i in (eq1, eq2, eq3, eq4, eq5)]
return np.array([c]).reshape((5, 1))
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
v1 = Ai.dot(self.v1)
v2 = Aj.dot(self.v2)
v3 = Ai.dot(self.v3)
c = float(v1.T.dot(v2))
s = float(v3.T.dot(v2))
# print('angle: %s'%self.pos)
#
## print("v1i = %s"%v1.T)
## print("v2j = %s"%v2.T)
# print("cos = %s"%c)
# print("sin = %s"%s)
## print("%s"%qi[3:])
# print("%s"%sum(qj[3:]**2))
# if s>=0 and c>=0:
# eq=np.arcsin(s)-np.deg2rad(self.pos)
# if s>=0 and c<0:
# eq=np.pi-np.arcsin(s)-np.deg2rad(self.pos)
# if s<0 and c<0:
# eq=np.pi-np.arcsin(s)-np.deg2rad(self.pos)
# if s<0 and c>=0:
# eq=2*np.pi+np.arcsin(s)-np.deg2rad(self.pos)
eq = float(v3.T.dot(v2)) - np.sin(np.deg2rad(self.pos))
return np.array([eq])
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 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 reactions(self, q, lamda):
l = lamda[self.index].values.reshape((self.nc, 1))
betai = q[self.i_body.dic.index][3:]
Ai = ep2dcm(betai)
ui = vector(Ai.dot(self.u_i))
jac = self.jacobian_i(q)
jacR = jac[:, :3]
jacP = jac[:, 3:]
F = -jacR.T.dot(l)
T = -jacP.T.dot(l)
cartesian_moment = 0.5 * E(betai).dot(T)
joint_torque = 0.5 * E(betai).dot(T) - vec2skew(ui).dot(F)
# print('Force = %s'%F)
# print('Cmoment = %s'%cartesian_moment)
# print('FTorque = %s'%vec2skew(ui).dot(F))
# print('JTorque = %s'%joint_torque)
return np.concatenate([[F], [joint_torque]]).reshape((6, ))
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 ep(self, value):
self._ep = value
self.dcm = ep2dcm(value)
