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)