def __init__(self, nodes=None, transform=None): if transform is None: transform = identity() if nodes is None: nodes = [] self.transform = transform self.nodes = nodes
def joint_to_base(self, joint): if isinstance(joint, str): joint = self.joints[joint] Tinv = geometry.identity() j = joint while j is not self.base and j.parent is not None: Tinv = j.parent.this_link_to_this_joint().dot( j.this_joint_to_parent_link.dot(Tinv)) j = j.parent if j: return Tinv else: raise Exception('Joint %s has no path to base frame' % joint)
def __init__(self, name, parent=None, type='fixed', getter=(lambda: 0), description='A kinematic joint', qmin=-math.inf, qmax=math.inf, d=0, theta=0, r=0, alpha=0, collision_model=None, ctransform=geometry.identity()): self.name = name self.parent = parent self.type = type if type == 'fixed': self.apply_q = self.fixed elif type == 'revolute': self.apply_q = self.revolute elif type == 'prismatic': self.apply_q = self.prismatic elif type == 'world': self.apply_q = self.world_joint else: raise ValueError( "Type must be 'fixed', 'revolute', 'prismatic', or 'world'.") self.getter = getter self.description = description self.children = [] self.d = d self.theta = theta self.r = r self.alpha = alpha self.children = [] self.collision_model = collision_model self.q = 0 self.qmin = qmin self.qmax = qmax self.parent_link_to_this_joint = geometry.dh_matrix( -d, -theta, -r, -alpha) self.this_joint_to_parent_link = np.linalg.inv( self.parent_link_to_this_joint) self.solver = None
def clear(self): self.mvMat = geo.identity() self.pMat = geo.identity()
def __init__(self): self.mvMat = geo.identity() self.pMat = geo.identity()
def fixed(self): return geometry.identity()
def draw(self, image): for scene_object in self.traverse(identity()): scene_object.draw(image)
def draw(self, image): for shape in self.traverse(identity()): shape.draw(image)