def __init__(self, bodyA, bodyB): Joint.__init__(self, bodyA, bodyB) self.Xj = sv.Xrot(np.matrix([[1.0,0.0,0.0], [0.0,1.0,0.0], [0.0,0.0,1.0]]))*sv.Xtrans([0.0,0.0,0.0]) self.S = np.matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]).transpose()
def __init__(self, transform, a, b, qe = None, qr = None): Joint.__init__(self, transform, a, b) self.qe = qe self.qr = qr if qe is None: theta = 0.0 ux = 0.0 uy = 0.0 uz = 0.0 p0 = np.cos(theta/2) p1 = np.sin(theta/2)*ux p2 = np.sin(theta/2)*uy p3 = np.sin(theta/2)*uz self.qe = np.matrix([p0, p1, p2, p3]).transpose() if qr is None: self.qr = np.matrix([0.0,0.0,0.0]).transpose() self.qe_norm = normalize(self.qe) self.qd = np.resize(0.0, (6,1)) self.qdd = np.resize(0.0, (6,1)) self.describe(" 6DoF Joint") self.update(self.qe, self.qr)
def __init__(self, bodyA, bodyB, q, d): Joint.__init__(self, bodyA, bodyB) self.update(q, d)