tests passing

Author: jhavl

examples/swifty.py

@@ -4,7 +4,7 @@
 @author Jesse Haviland
 """
 
-import roboticstoolbox as rtb
+import roboticstoolbox as rp
 import spatialmath as sm
 import numpy as np
 import qpsolvers as qp
@@ -14,7 +14,15 @@
 # env.launch()
 
 # Create a Panda robot object
-panda = rtb.models.ETS.Panda()
+# panda = rtb.models.ETS.Panda()
+
+rx = rp.ETS.rx(1.543)
+ry = rp.ETS.ry(1.543)
+tz = rp.ETS.tz(1)
+
+l0 = rp.ELink(rx * ry * tz)
+
+print(l0)
 
 # print(panda)
 # print(panda.base_link)

roboticstoolbox/robot/ERobot.py

@@ -1217,7 +1217,6 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
         make it impossible for the robot to run into joint limits. Requires
         the joint limits of the robot to be specified. See examples/mmc.py
         for use case
-
         :param ps: The minimum angle (in radians) in which the joint is
             allowed to approach to its limit
         :type ps: float
@@ -1228,7 +1227,6 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
         :type n: int
         :param gain: The gain for the velocity damper
         :type gain: float
-
         :returns: Ain, Bin as the inequality contraints for an optisator
         :rtype: ndarray(6), ndarray(6)
         '''
@@ -1251,39 +1249,69 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
 
         return Ain, Bin
 
-    # def link_collision_damper(self, links=None, col, ob, q):
-    #     dii = 5
-    #     di = 0.3
-    #     ds = 0.05
+    def link_collision_damper(
+            self, shape, q=None, di=0.3, ds=0.05, xi=1.0,
+            from_link=None, to_link=None):
+
+        if from_link is None:
+            from_link = self.base_link
+
+        if to_link is None:
+            to_link = self.ee_link
+
+        links, n = self.get_path(from_link, to_link)
+
+        if q is None:
+            q = np.copy(self.q)
+        else:
+            q = getvector(q, n)
 
-    #     if links is None:
-    #         links = self.links[1:]
+        j = 0
+        Ain = None
+        bin = None
+
+        def indiv_calculation(link, link_col, q):
+            d, wTlp, wTcp = link_col.closest_point(shape, di)
+
+            if d is not None:
+                lpTcp = wTlp.inv() * wTcp
+                norm = lpTcp.t / d
+                norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
+
+                Je = self.jacobe(
+                    q, from_link=self.base_link, to_link=link,
+                    offset=link_col.base)
+                n_dim = Je.shape[1]
+                dp = norm_h @ shape.v
+                l_Ain = np.zeros((1, n))
+                l_Ain[0, :n_dim] = norm_h @ Je
+                l_bin = (xi * (d - ds) / (di - ds)) + dp
+            else:
+                l_Ain = None
+                l_bin = None
 
-    #     ret = p.getClosestPoints(col.co, ob.co, dii)
+            return l_Ain, l_bin, d, wTcp
 
-    #     if len(ret) > 0:
-    #         ret = ret[0]
-    #         wTlp = sm.SE3(ret[5])
-    #         wTcp = sm.SE3(ret[6])
-    #         lpTcp = wTlp.inv() * wTcp
+        for link in links:
+            if link.jtype == link.VARIABLE:
+                j += 1
 
-    #         d = ret[8]
+            for link_col in link.collision:
+                l_Ain, l_bin, d, wTcp = indiv_calculation(
+                                                link, link_col, q[:j])
 
-    #     if d < di:
-    #         n = lpTcp.t / d
-    #         nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
+                if l_Ain is not None and l_bin is not None:
+                    if Ain is None:
+                        Ain = l_Ain
+                    else:
+                        Ain = np.r_[Ain, l_Ain]
 
-    #         Je = r.jacobe(q, from_link=r.base_link, to_link=link, offset=col.base)
-    #         n = Je.shape[1]
-    #         dp = nh @ ob.v
-    #         l_Ain = np.zeros((1, 13))
-    #         l_Ain[0, :n] = nh @ Je
-    #         l_bin = (0.8 * (d - ds) / (di - ds)) + dp
-    #     else:
-    #         l_Ain = None
-    #         l_bin = None
+                    if bin is None:
+                        bin = np.array(l_bin)
+                    else:
+                        bin = np.r_[bin, l_bin]
 
-    #     return l_Ain, l_bin, d, wTcp
+        return Ain, bin
 
     def closest_point(self, shape, inf_dist=1.0):
         '''
@@ -1292,7 +1320,6 @@ def closest_point(self, shape, inf_dist=1.0):
         inf_dist. It will also return the points on self and shape in the
         world frame which connect the line of length distance between the
         shapes. If the distance is negative then the shapes are collided.
-
         :param shape: The shape to compare distance to
         :type shape: Shape
         :param inf_dist: The minimum distance within which to consider
@@ -1324,7 +1351,6 @@ def closest_point(self, shape, inf_dist=1.0):
     def collided(self, shape):
         '''
         collided(shape) checks if this robot and shape have collided
-
         :param shape: The shape to compare distance to
         :type shape: Shape
         :returns: True if shapes have collided

tests/test_ELink.py

@@ -20,7 +20,7 @@ def test_str_ets(self):
 
         l0 = rp.ELink(rx * ry * tz)
 
-        ans = 'Rx(88.4074) * Ry(88.4074) * tz(1)'
+        ans = 'name[(): Rx(88.41°) * Ry(88.41°) * tz(1)] '
 
         self.assertEqual(str(l0), ans)
 
@@ -223,7 +223,7 @@ def test_dist(self):
         d1, _, _ = link.closest_point(s1, 5)
         d2, _, _ = link.closest_point(s1)
 
-        self.assertAlmostEqual(d0, -0.5599999999995913)
+        self.assertAlmostEqual(d0, -0.49)
         self.assertAlmostEqual(d1, 2.44)
         self.assertAlmostEqual(d2, None)