brought linkdamper up

jhavl · jhavl · commit eafff14a102e · 2021-01-25T14:08:05.000+10:00
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -1779,46 +1779,6 @@ def jacob0v(
 
         return Jv
 
-    def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
-        '''
-        Formulates an inequality contraint which, when optimised for will
-        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
-        :param pi: The influence angle (in radians) in which the velocity
-            damper becomes active
-        :type pi: float
-        :param n: The number of joints to consider. Defaults to all joints
-        :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)
-        '''
-
-        if n is None:
-            n = self.n
-
-        Ain = np.zeros((n, n))
-        Bin = np.zeros(n)
-
-        for i in range(n):
-            if self.q[i] - self.qlim[0, i] <= pi:
-                Bin[i] = -gain * (
-                    ((self.qlim[0, i] - self.q[i]) + ps) / (pi - ps))
-                Ain[i, i] = -1
-            if self.qlim[1, i] - self.q[i] <= pi:
-                Bin[i] = gain * (
-                    (self.qlim[1, i] - self.q[i]) - ps) / (pi - ps)
-                Ain[i, i] = 1
-
-        return Ain, Bin
-
     def link_collision_damper(
             self, shape, q=None, di=0.3, ds=0.05, xi=1.0,
             endlink=None, startlink=None):
diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py
@@ -1481,3 +1481,43 @@ def collided(self, shape):
                 return True
 
         return False
+
+    def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
+        '''
+        Formulates an inequality contraint which, when optimised for will
+        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
+        :param pi: The influence angle (in radians) in which the velocity
+            damper becomes active
+        :type pi: float
+        :param n: The number of joints to consider. Defaults to all joints
+        :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)
+        '''
+
+        if n is None:
+            n = self.n
+
+        Ain = np.zeros((n, n))
+        Bin = np.zeros(n)
+
+        for i in range(n):
+            if self.q[i] - self.qlim[0, i] <= pi:
+                Bin[i] = -gain * (
+                    ((self.qlim[0, i] - self.q[i]) + ps) / (pi - ps))
+                Ain[i, i] = -1
+            if self.qlim[1, i] - self.q[i] <= pi:
+                Bin[i] = gain * (
+                    (self.qlim[1, i] - self.q[i]) - ps) / (pi - ps)
+                Ain[i, i] = 1
+
+        return Ain, Bin
