Exemplo n.º 1
0
 def __init__(self, nodes=None, transform=None):
     if transform is None:
         transform = identity()
     if nodes is None:
         nodes = []
     self.transform = transform
     self.nodes = nodes
Exemplo n.º 2
0
 def __init__(self, nodes=None, transform=None):
     if transform is None:
         transform = identity()
     if nodes is None:
         nodes = []
     self.transform = transform
     self.nodes = nodes
Exemplo n.º 3
0
 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)
Exemplo n.º 4
0
    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()
Exemplo n.º 7
0
 def fixed(self):
     return geometry.identity()
Exemplo n.º 8
0
 def draw(self, image):
     for scene_object in self.traverse(identity()):
         scene_object.draw(image)
Exemplo n.º 9
0
 def draw(self, image):
     for scene_object in self.traverse(identity()):
         scene_object.draw(image)
Exemplo n.º 10
0
 def draw(self, image):
     for shape in self.traverse(identity()):
         shape.draw(image)