diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py index 5e5c084ff..6859c8e66 100644 --- 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 index abe2a2487..039a698e9 100644 --- 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