collision convenience

jhavl · jhavl · commit 99517fed6355 · 2020-10-23T13:54:59.000+10:00
diff --git a/examples/neo.py b/examples/neo.py
@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import roboticstoolbox as rtb
+import spatialmath as sm
+import numpy as np
+import qpsolvers as qp
+import time
+
+# Launch the simulator Swift
+env = rtb.backend.Swift()
+env.launch()
+
+# Create a Panda robot object
+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]
+
+# Make two obstacles with velocities
+s0 = rtb.Shape.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(
+    radius=0.05,
+    base=sm.SE3(0.1, 0.35, 0.65)
+)
+s1.v = [0, -0.2, 0, 0, 0, 0]
+
+# Make a target
+s2 = rtb.Shape.Sphere(
+    radius=0.02,
+    base=sm.SE3(0.6, -0.2, 0.0)
+)
+
+# Add the Panda and shapes to the simulator
+env.add(panda)
+env.add(s0)
+env.add(s1)
+env.add(s2)
+time.sleep(1)
+
+# Number of joint in the panda which we are controlling
+n = 7
+
+# Set the desired end-effector pose to the location of s2
+Tep = panda.fkine()
+Tep.A[:3, 3] = s2.base.t
+
+arrived = False
+
+while not arrived:
+
+    # The pose of the Panda's end-effector
+    Te = panda.fkine()
+
+    # Transform from the end-effector to desired pose
+    eTep = Te.inv() * Tep
+
+    # Spatial error
+    e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi/180]))
+
+    # 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)
+
+    # Gain term (lambda) for control minimisation
+    Y = 0.01
+
+    # Quadratic component of objective function
+    Q = np.eye(n + 6)
+
+    # Joint velocity component of Q
+    Q[:n, :n] *= Y
+
+    # Slack component of Q
+    Q[n:, n:] = (1 / e) * np.eye(6)
+
+    # The equality contraints
+    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)
+
+    # Linear component of objective function: the manipulability Jacobian
+    c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]
+
+    # The lower and upper bounds on the joint velocity and slack variable
+    lb = -np.r_[panda.qdlim[:n], 10 * np.ones(6)]
+    ub = np.r_[panda.qdlim[:n], 10 * np.ones(6)]
+
+    # Solve for the joint velocities dq
+    qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub)
+
+    # Apply the joint velocities to the Panda
+    panda.qd[:n] = qd[:n]
+
+    # Step the simulator by 50 ms
+    env.step(50)
diff --git a/roboticstoolbox/robot/ELink.py b/roboticstoolbox/robot/ELink.py
@@ -501,3 +501,55 @@ def friction(self, qd):
         tau = -np.abs(self.G) * tau
 
         return tau
+
+    def closest_point(self, shape, inf_dist=1.0):
+        '''
+        closest_point(shape, inf_dist) returns the minimum euclidean
+        distance between this link and shape, provided it is less than
+        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
+            the shape
+        :type inf_dist: float
+        :returns: d, p1, p2 where d is the distance between the shapes,
+            p1 and p2 are the points in the world frame on the respective
+            shapes
+        :rtype: float, SE3, SE3
+        '''
+
+        d = 10000
+        p1 = None,
+        p2 = None
+
+        for col in self.collision:
+            td, tp1, tp2 = col.closest_point(shape, inf_dist)
+
+            if td is not None and td < d:
+                d = td
+                p1 = tp1
+                p2 = tp2
+
+        if d == 10000:
+            d = None
+
+        return d, p1, p2
+
+    def collided(self, shape):
+        '''
+        collided(shape) checks if this link and shape have collided
+
+        :param shape: The shape to compare distance to
+        :type shape: Shape
+        :returns: True if shapes have collided
+        :rtype: bool
+        '''
+
+        for col in self.collision:
+            if col.collided(shape):
+                return True
+
+        return False
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1120,6 +1120,88 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
 
         return Ain, bin
 
+    # def link_collision_damper(self, link, col, ob, q):
+    #     dii = 5
+    #     di = 0.3
+    #     ds = 0.05
+
+    #     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
+
+    #     return l_Ain, l_bin, d, wTcp
+
+    def closest_point(self, shape, inf_dist=1.0):
+        '''
+        closest_point(shape, inf_dist) returns the minimum euclidean
+        distance between this robot and shape, provided it is less than
+        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
+            the shape
+        :type inf_dist: float
+        :returns: d, p1, p2 where d is the distance between the shapes,
+            p1 and p2 are the points in the world frame on the respective
+            shapes
+        :rtype: float, SE3, SE3
+        '''
+
+        d = 10000
+        p1 = None,
+        p2 = None
+
+        for link in self.links:
+            td, tp1, tp2 = link.closest_point(shape, inf_dist)
+
+            if td is not None and td < d:
+                d = td
+                p1 = tp1
+                p2 = tp2
+
+        if d == 10000:
+            d = None
+
+        return d, p1, p2
+
+    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
+        :rtype: bool
+        '''
+
+        for link in self.links:
+            if link.collided(shape):
+                return True
+
+        return False
 
     # def teach(
     #         self, block=True, q=None, limits=None,
diff --git a/roboticstoolbox/robot/Shape.py b/roboticstoolbox/robot/Shape.py
@@ -207,7 +207,8 @@ def closest_point(self, shape, inf_dist=1.0):
         closest_point(shape, inf_dist) returns the minimum euclidean
         distance between self and shape, provided it is less than 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.
+        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
@@ -236,9 +237,9 @@ def closest_point(self, shape, inf_dist=1.0):
         ret = p.getClosestPoints(self.co, shape.co, inf_dist)
 
         if len(ret) == 0:
-            d = -1
-            p1 = SE3()
-            p2 = SE3()
+            d = None
+            p1 = None
+            p2 = None
         else:
             ret = ret[0]
             d = ret[8]
@@ -247,6 +248,23 @@ def closest_point(self, shape, inf_dist=1.0):
 
         return d, p1, p2
 
+    def collided(self, shape):
+        '''
+        collided(shape) checks if self and shape have collided
+
+        :param shape: The shape to compare distance to
+        :type shape: Shape
+        :returns: True if shapes have collided
+        :rtype: bool
+        '''
+
+        d, _, _ = self.closest_point(shape)
+
+        if d is not None and d <= 0:
+            return True
+        else:
+            return False
+
 
 class Mesh(Shape):
     """
diff --git a/tests/test_ELink.py b/tests/test_ELink.py
@@ -213,6 +213,31 @@ def test_geometry_fail(self):
         with self.assertRaises(TypeError):
             l0.geometry = 1
 
+    def test_dist(self):
+        s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
+        s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
+        p = rp.models.Panda()
+        link = p.links[3]
+
+        d0, _, _ = link.closest_point(s0)
+        d1, _, _ = link.closest_point(s1, 5)
+        d2, _, _ = link.closest_point(s1)
+
+        self.assertAlmostEqual(d0, -0.5599999999995913)
+        self.assertAlmostEqual(d1, 2.44)
+        self.assertAlmostEqual(d2, None)
+
+    def test_collided(self):
+        s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
+        s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
+        p = rp.models.Panda()
+        link = p.links[3]
+        c0 = link.collided(s0)
+        c1 = link.collided(s1)
+
+        self.assertTrue(c0)
+        self.assertFalse(c1)
+
 
 if __name__ == '__main__':
 
diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py
@@ -592,6 +592,30 @@ def test_control_type2(self):
     #     e2 = panda.teach2(block=False, q=panda.qr)
     #     e2.close()
 
+    def test_dist(self):
+        s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
+        s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
+        p = rp.models.Panda()
+
+        d0, _, _ = p.closest_point(s0)
+        d1, _, _ = p.closest_point(s1, 5)
+        d2, _, _ = p.closest_point(s1)
+
+        self.assertAlmostEqual(d0, -0.5599999999995913)
+        self.assertAlmostEqual(d1, 2.4275999999999995)
+        self.assertAlmostEqual(d2, None)
+
+    def test_collided(self):
+        s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
+        s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
+        p = rp.models.Panda()
+
+        c0 = p.collided(s0)
+        c1 = p.collided(s1)
+
+        self.assertTrue(c0)
+        self.assertFalse(c1)
+
 
 if __name__ == '__main__':
 
diff --git a/tests/test_Shape.py b/tests/test_Shape.py
@@ -30,7 +30,7 @@ def test_closest(self):
         self.assertAlmostEqual(d0, 0.5)
         self.assertAlmostEqual(d1, 4.698463840213662e-13)
         self.assertAlmostEqual(d2, 2.5)
-        self.assertAlmostEqual(d3, -1)
+        self.assertAlmostEqual(d3, None)
 
     def test_to_dict(self):
         s1 = rp.Cylinder(1, 1)
@@ -67,3 +67,14 @@ def test_fk_dict2(self):
     def test_mesh(self):
         ur = rp.models.UR5()
         ur.links[1].collision[0].closest_point(ur.links[2].collision[0])
+
+    def test_collision(self):
+        s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
+        s1 = rp.Box([1, 1, 1], sm.SE3(0.5, 0, 0))
+        s2 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
+
+        c0 = s0.collided(s1)
+        c1 = s0.collided(s2)
+
+        self.assertTrue(c0)
+        self.assertFalse(c1)
