neo

jhavl · jhavl · commit 2c66c697b6d2 · 2020-10-27T09:09:47.000+10:00
diff --git a/examples/neo.py b/examples/neo.py
@@ -17,23 +17,30 @@
 panda = rtb.models.Panda()
 
 # Set joint angles to ready configuration
-panda.q = [-0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173]
+# panda.q = [
+    # -0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173, 0.0, 0.0]
+panda.q = panda.qr
+
+# Number of joint in the panda which we are controlling
+n = 7
 
 # Make two obstacles with velocities
-s0 = rtb.Shape.Sphere(
+s0 = rtb.Sphere(
     radius=0.05,
     base=sm.SE3(0.45, 0.4, 0.3)
 )
 s0.v = [0.01, -0.2, 0, 0, 0, 0]
 
-s1 = rtb.Shape.Sphere(
+s1 = rtb.Sphere(
     radius=0.05,
     base=sm.SE3(0.1, 0.35, 0.65)
 )
 s1.v = [0, -0.2, 0, 0, 0, 0]
 
+collisions = [s0, s1]
+
 # Make a target
-s2 = rtb.Shape.Sphere(
+target = rtb.Sphere(
     radius=0.02,
     base=sm.SE3(0.6, -0.2, 0.0)
 )
@@ -42,15 +49,12 @@
 env.add(panda)
 env.add(s0)
 env.add(s1)
-env.add(s2)
-time.sleep(1)
+env.add(target)
 
-# Number of joint in the panda which we are controlling
-n = 7
-
-# Set the desired end-effector pose to the location of s2
+# Set the desired end-effector pose to the location of target
 Tep = panda.fkine()
-Tep.A[:3, 3] = s2.base.t
+Tep.A[:3, 3] = target.base.t
+Tep.A[2, 3] += 0.1
 
 arrived = False
 
@@ -67,7 +71,7 @@
 
     # Calulate the required end-effector spatial velocity for the robot
     # to approach the goal. Gain is set to 1.0
-    v, arrived = rtb.p_servo(Te, Tep, 1.0)
+    v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.05)
 
     # Gain term (lambda) for control minimisation
     Y = 0.01
@@ -85,42 +89,42 @@
     Aeq = np.c_[panda.jacobe(), np.eye(6)]
     beq = v.reshape((6,))
 
-    # The inequality constraints for joint limit avoidance
-    Ain = np.zeros((n + 6, n + 6))
-    bin = np.zeros(n + 6)
-
-    # The minimum angle (in radians) in which the joint is allowed to approach
-    # to its limit
-    ps = 0.05
-
-    # The influence angle (in radians) in which the velocity damper
-    # becomes active
-    pi = 0.9
-
-    # Form the joint limit velocity damper
-    Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
-
-    for link in links:
-        if link.jtype == link.VARIABLE:
-            j += 1
-        for col in link.collision:
-            obj = s0
-            l_Ain, l_bin, ret, wTcp = link_calc(link, col, obj, q[:j])
-            if ret < closest:
-                closest = ret
-                closest_obj = obj
-                closest_p = wTcp
-
-            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]
-
-                if bin is None:
-                    bin = np.array(l_bin)
-                else:
-                    bin = np.r_[bin, l_bin]
+    # # The inequality constraints for joint limit avoidance
+    # Ain = np.zeros((n + 6, n + 6))
+    # bin = np.zeros(n + 6)
+
+#     # The minimum angle (in radians) in which the joint is allowed to approach
+#     # to its limit
+#     ps = 0.05
+
+#     # The influence angle (in radians) in which the velocity damper
+#     # becomes active
+#     pi = 0.9
+
+#     # Form the joint limit velocity damper
+#     Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
+
+    Ain = None
+    bin = None
+
+    for collision in collisions:
+
+        c_Ain, c_bin = panda.link_collision_damper(
+            collision, panda.q[:n], 0.3, 0.05,
+            panda.elinks['panda_joint1'], panda.elinks['panda_hand_joint'])
+
+        if c_Ain is not None and c_bin is not None:
+            c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 6))]
+
+            if Ain is None:
+                Ain = c_Ain
+            else:
+                Ain = np.r_[Ain, c_Ain]
+
+            if bin is None:
+                bin = np.array(c_bin)
+            else:
+                bin = np.r_[bin, c_bin]
 
     # Linear component of objective function: the manipulability Jacobian
     c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]
diff --git a/roboticstoolbox/backend/urdf/utils.py b/roboticstoolbox/backend/urdf/utils.py
@@ -57,7 +57,7 @@ def parse_origin(node):
         if 'rpy' in origin_node.attrib:
             rpy = np.fromstring(origin_node.attrib['rpy'], sep=' ')
             matrix.A[:3, :3] = sm.SE3.RPY(rpy).R
-   
+
     return matrix.A, rpy
 
 
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1142,39 +1142,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
-
-    #     if links is None:
-    #         links = self.links[1:]
-
-    #     ret = p.getClosestPoints(col.co, ob.co, dii)
-
-    #     if len(ret) > 0:
-    #         ret = ret[0]
-    #         wTlp = sm.SE3(ret[5])
-    #         wTcp = sm.SE3(ret[6])
-    #         lpTcp = wTlp.inv() * wTcp
-
-    #         d = ret[8]
-
-    #     if d < di:
-    #         n = lpTcp.t / d
-    #         nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
-
-    #         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
+    def link_collision_damper(
+            self, shape, q=None, di=0.3, ds=0.05,
+            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)
+
+        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 = (0.8 * (d - ds) / (di - ds)) + dp
+            else:
+                l_Ain = None
+                l_bin = None
+
+            return l_Ain, l_bin, d, wTcp
+
+        for link in links:
+            if link.jtype == link.VARIABLE:
+                j += 1
+
+            for link_col in link.collision:
+                l_Ain, l_bin, d, wTcp = indiv_calculation(
+                                                link, link_col, q[:j])
+
+                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]
+
+                    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):
         '''
