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)
