From 4142b43305350baa2d1d7fc31527c3a5409e16d4 Mon Sep 17 00:00:00 2001 From: Theo Combelles Date: Tue, 12 Mar 2024 22:29:47 +0900 Subject: [PATCH] added the attribute q to joint velocity damper method --- roboticstoolbox/robot/Robot.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index eadab10d..1d5eacd1 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1331,6 +1331,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, @@ -1368,16 +1369,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