Merge pull request #425 from LonelyPaprika/feature/add_q_attribute_vel_damper

added the attribute q to joint velocity damper method

Author: jhavl

roboticstoolbox/robot/Robot.py

@@ -1355,6 +1355,7 @@ def collided(self, q, shape: Shape, skip: bool = False) -> bool:
 
     def joint_velocity_damper(
         self,
+        q=None,
         ps: float = 0.05,
         pi: float = 0.1,
         n: Union[int, None] = None,
@@ -1392,16 +1393,19 @@ def joint_velocity_damper(
 
         if n is None:
             n = self.n
+        
+        if q is None:
+            q = np.copy(self.q)
 
         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))
+                Bin[i] = -gain * (((self.qlim[0, i] - 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)
+                Bin[i] = gain * ((self.qlim[1, i] - q[i]) - ps) / (pi - ps)
                 Ain[i, i] = 1
 
         return Ain, Bin