def __init__(self, o2w=None, w2o=None):
if o2w is None:
o2w = SE3.identity()
if w2o is None:
w2o = SE3.identity()
self.o2w = o2w
self.w2o = w2o
self.parent = None
def __init__(self, name=None):
super(Link, self).__init__(name=name)
self.q = None # dofs
self.xi = [] # joint twists
self.g_wl0 = SE3.identity() # transform at q=0
self.parent = None
self.childs = []
def set_state(self, q):
self.q = q[self.mask]
exp = SE3.identity()
for i in range(len(self.xi)):
q = self.q[i, 0]
xi = self.xi[i]
exp = exp * SE3.exp(xi * q)
g_wl = exp * self.g_wl0
self.set_transform_world(g_wl)
def get_spatial_jacobian(self):
J = np.zeros((6, len(self.xi)))
exp = SE3.identity()
for i in range(len(self.xi)):
if i - 1 >= 0:
exp = exp * SE3.exp(self.xi[i - 1] * self.q[i - 1, 0])
J[:, i, None] = SE3.Ad(exp) @ self.xi[i]
J_s = np.zeros((6, len(self.mask)))
J_s[:, self.mask] = J
return J_s
def init(self):
self.reindex()
self.set_tf_world(SE3.identity())
self.init_geometry()
self.init_opengl()
def __init__(self, name=None):
super(Proxy, self).__init__(name)
self.body = None
self.g_bp = SE3.identity()
self.m = 0
self.n_dofs = 1
def __init__(self, name=None):
self.g_wb = SE3.identity()
self.mask = np.array([True] * 6, bool)
self.name = name
self.contacts = []
self.qdot = np.zeros((6, 1))
def frame_B(self):
g_wb = SE3.identity()
g_wb.R.set_matrix(make_frame(self.normal))
g_wb.t = self.pts_B.copy()
return g_wb
def frame_A(self):
g_wa = SE3.identity()
g_wa.R.set_matrix(make_frame(self.normal))
g_wa.t = self.pts_A.copy()
return g_wa