gripper implemented

Author: jhavl

roboticstoolbox/models/URDF/Panda.py

@@ -38,18 +38,17 @@ def __init__(self):
             elinks,
             name=name,
             manufacturer='Franka Emika',
-            ee_links=elinks[7],
             gripper_links=elinks[9]
         )
 
         self.qdlim = np.array([
             2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 3.0, 3.0])
 
         self.addconfiguration("qz", np.array(
-            [0, 0, 0, 0, 0, 0, 0, 0, 0]))
+            [0, 0, 0, 0, 0, 0, 0]))
 
         self.addconfiguration("qr", np.array(
-            [0, -0.3, 0, -2.2, 0, 2.0, np.pi/4, 0, 0]))
+            [0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]))
 
 
 if __name__ == '__main__':   # pragma nocover

roboticstoolbox/robot/ERobot.py

@@ -125,6 +125,12 @@ def __init__(
         # Make a gripper object for each gripper
         for link in gripper_links:
             g_links = self.dfs_links(link)
+
+            # Remove gripper links from the robot
+            for g_link in g_links:
+                elinks.remove(g_link)
+
+            # Save the gripper object
             self.grippers.append(Gripper(g_links))
 
         # Subtract the n of the grippers from the n of the robot
@@ -139,24 +145,19 @@ def __init__(
                 if len(link.child) == 0:
                     # no children, must be an end-effector
                     self.ee_links.append(link)
-        # else:
-        #     for link = self.gripper_links:
-        #         # use the passed in value
-        #         self.ee_links.append(link.)
-
+        else:
+            for link in gripper_links:
+                # use the passed in value
+                self.ee_links.append(link.parent)
 
         # assign the joint indices
         if all([link.jindex is None for link in elinks]):
-            # no jindex set, assign them
-            # if len(self.ee_links) > 1:
-            #     raise ValueError('for a robot with multiple end-effectors must assign joint indices')
-            # maybe do the DFS in where somewhere
 
             jindex = [0]  # "mutable integer" hack
 
             def visit_link(link, jindex):
                 # if it's a joint, assign it a jindex and increment it
-                if link.isjoint:
+                if link.isjoint and link in elinks:
                     link.jindex = jindex[0]
                     jindex[0] += 1
 
@@ -166,7 +167,7 @@ def visit_link(link, jindex):
 
         elif all([link.jindex is not None for link in elinks]):
             # jindex set on all, check they are unique and sequential
-            jset = set(range(n))
+            jset = set(range(self._n))
             for link in elinks:
                 if link.jindex not in jset:
                     raise ValueError(
@@ -267,6 +268,32 @@ def to_dict(self):
 
             ob['links'].append(li)
 
+        # Do the grippers now
+        for gripper in self.grippers:
+            for link in gripper.links:
+                li = {
+                    'axis': [],
+                    'eta': [],
+                    'geometry': [],
+                    'collision': []
+                }
+
+                for et in link.ets():
+                    li['axis'].append(et.axis)
+                    li['eta'].append(et.eta)
+
+                if link.v is not None:
+                    li['axis'].append(link.v.axis)
+                    li['eta'].append(link.v.eta)
+
+                for gi in link.geometry:
+                    li['geometry'].append(gi.to_dict())
+
+                for gi in link.collision:
+                    li['collision'].append(gi.to_dict())
+
+                ob['links'].append(li)
+
         return ob
 
     def fk_dict(self):
@@ -276,6 +303,7 @@ def fk_dict(self):
 
         self.fkine_all()
 
+        # Do the robot
         for link in self.links:
 
             li = {
@@ -291,6 +319,22 @@ def fk_dict(self):
 
             ob['links'].append(li)
 
+        # Do the grippers now
+        for gripper in self.grippers:
+            for link in gripper.links:
+                li = {
+                    'geometry': [],
+                    'collision': []
+                }
+
+                for gi in link.geometry:
+                    li['geometry'].append(gi.fk_dict())
+
+                for gi in link.collision:
+                    li['collision'].append(gi.fk_dict())
+
+                ob['links'].append(li)
+
         return ob
 
     # @classmethod
@@ -628,37 +672,41 @@ def fkine_all(self, q=None):
         else:
             q = getvector(q, self.n)
 
-        # t = self.base
-        # Tall = SE3()
-        j = 0
+        for link in self.links:
+            if link.isjoint:
+                t = link.A(q[link.jindex])
+            else:
+                t = link.A()
 
-        if self.links[0].isjoint:
-            self.links[0]._fk = self.base * self.links[0].A(q[j])
-            j += 1
-        else:
-            self.links[0]._fk = self.base * self.links[0].A()
+            if link.parent is None:
+                link._fk = self.base * t
+            else:
+                link._fk = link.parent._fk * t
 
-        for col in self.links[0].collision:
-            col.wT = self.links[0]._fk
+            # Update the collision objects transform as well
+            for col in link.collision:
+                col.wT = link._fk
 
-        for gi in self.links[0].geometry:
-            gi.wT = self.links[0]._fk
+            for gi in link.geometry:
+                gi.wT = link._fk
 
-        for i in range(1, len(self.links)):
-            if self.links[i].isjoint:
-                t = self.links[i].A(q[j])
-                j += 1
-            else:
-                t = self.links[i].A()
+        # Do the grippers now
+        for gripper in self.grippers:
+            for link in gripper.links:
+                # print(link.jindex)
+                if link.isjoint:
+                    t = link.A(gripper.q[link.jindex])
+                else:
+                    t = link.A()
 
-            self.links[i]._fk = self.links[i].parent._fk * t
+                link._fk = link.parent._fk * t
 
-            # Update the collision objects transform as well
-            for col in self.links[i].collision:
-                col.wT = self.links[i]._fk
+                # Update the collision objects transform as well
+                for col in link.collision:
+                    col.wT = link._fk
 
-            for gi in self.links[i].geometry:
-                gi.wT = self.links[i]._fk
+                for gi in link.geometry:
+                    gi.wT = link._fk
 
     # def jacob0(self, q=None):
     #     """

roboticstoolbox/robot/Gripper.py

@@ -3,6 +3,9 @@
 @author Jesse Haviland
 """
 
+import numpy as np
+from spatialmath.base.argcheck import getvector
+
 
 class Gripper():
 
@@ -17,6 +20,106 @@ def __init__(
             if link.isjoint:
                 self._n += 1
 
+        self.q = np.zeros(self.n)
+        self._links = elinks
+
+        # assign the joint indices
+        if all([link.jindex is None for link in elinks]):
+
+            jindex = [0]  # "mutable integer" hack
+
+            def visit_link(link, jindex):
+                # if it's a joint, assign it a jindex and increment it
+                if link.isjoint:
+                    link.jindex = jindex[0]
+                    jindex[0] += 1
+
+            # visit all links in DFS order
+            self.dfs_links(
+                elinks[0], lambda link: visit_link(link, jindex))
+
+        elif all([link.jindex is not None for link in elinks]):
+            # jindex set on all, check they are unique and sequential
+            jset = set(range(self.n))
+            for link in elinks:
+                if link.jindex not in jset:
+                    raise ValueError(
+                        'gripper joint index {link.jindex} was '
+                        'repeated or out of range')
+                jset -= set(link.jindex)
+            if len(jset) > 0:
+                raise ValueError('gripper joints {jset} were not assigned')
+        else:
+            # must be a mixture of ELinks with/without jindex
+            raise ValueError(
+                'all gripper links must have a jindex, or none have a jindex')
+
+    def dfs_links(self, start, func=None):
+        """
+        Visit all links from start in depth-first order and will apply
+        func to each visited link
+
+        :param start: the link to start at
+        :type start: ELink
+        :param func: An optional function to apply to each link as it is found
+        :type func: function
+
+        :returns: A list of links
+        :rtype: list of ELink
+        """
+        visited = []
+
+        def vis_children(link):
+            visited.append(link)
+            if func is not None:
+                func(link)
+
+            for li in link.child:
+                if li not in visited:
+                    vis_children(li)
+
+        vis_children(start)
+
+        return visited
+
     @property
     def n(self):
         return self._n
+
+    @property
+    def q(self):
+        """
+        Get/set gripper joint configuration
+
+        - ``gripper.q`` is the gripper joint configuration
+
+        :return: gripper joint configuration
+        :rtype: ndarray(n,)
+
+        - ``gripper.q = ...`` checks and sets the joint configuration
+
+        .. note::  ???
+        """
+        return self._q
+
+    @q.setter
+    def q(self, q_new):
+        self._q = getvector(q_new, self.n)
+
+# --------------------------------------------------------------------- #
+
+    @property
+    def links(self):
+        """
+        Gripper links
+
+        :return: A list of link objects
+        :rtype: list of Link subclass instances
+
+        .. note:: It is probably more concise to index the robot object rather
+            than the list of links, ie. the following are equivalent::
+
+                robot.links[i]
+                robot[i]
+        """
+        return self._links