diff --git a/PYTHON/nodes b/PYTHON/nodes index 296455183..f7591cc7e 160000 --- a/PYTHON/nodes +++ b/PYTHON/nodes @@ -1 +1 @@ -Subproject commit 296455183e225444134f19b3f3f722e825ec6717 +Subproject commit f7591cc7ed5021c5fbc5d661559b4b94784e235c diff --git a/PYTHON/utils/mecademic_state/mecademic_calculations.py b/PYTHON/utils/mecademic_state/mecademic_calculations.py new file mode 100644 index 000000000..5a03d9876 --- /dev/null +++ b/PYTHON/utils/mecademic_state/mecademic_calculations.py @@ -0,0 +1,40 @@ +""" +This command limits the angular velocities of the robot joints. It affects the movements +generated by the MovePose and MoveJoints commands. +Arguments: +- p: percentage of maximum joint velocities, ranging from 1 to 100% +Default values: +By default, the limit is set to 25%. +Note that it is not possible to limit the velocity of only one joint. With this command, the +maximum velocities of all joints are limited proportionally. In other words, the maximum +velocity of each joint will be reduced to a percentage p of its top velocity. The top velocity +of joints 1 and 2 is 150◦/s, of joint 3 is 180◦/s, of joints 4 and 5 is 300◦/s, and of joint 6 is +500◦/s + + +Parameters + - current: the joints present position (6 element array of numbers) + - next: the next position the joints will go to + - timeDelta: the amount of time (in milliseconds) that the move should take +""" + + +def calculateLimitingMaxVel(current, next, timeDelta) -> float: + # TODO: we can add virtual points between the keyframes and enable blending for more smooth moves? + deltaPosititons = [abs(current[i] - next[i]) for i in range(len(current))] # the delta positions of each axis in degrees + velocities = [150, 150, 180, 300, 300, 500] # the max velocities of each axis in degrees per second + maxVelocities = [deltaPosititons[i] / (timeDelta / 1000) for i in range(len(deltaPosititons))] # the max velocities of each axis in degrees per second + highestVelRatio = (-1, -1) # the highest velocity ratio and the index of the axis + for i in range(len(maxVelocities)): + ratio = maxVelocities[i] / velocities[i] + if ratio > highestVelRatio[0]: + highestVelRatio = (ratio, i) + + idxOfAxisToLimit = highestVelRatio[1] + targetVelofAxisToLimit = maxVelocities[idxOfAxisToLimit] + ratioOfMaxtoTarget = velocities[idxOfAxisToLimit] / targetVelofAxisToLimit + if ratioOfMaxtoTarget > 1: + print("WARNING: The target velocity is higher than the max velocity of the axis. The axis will be limited to 100% of its max velocity.") + if ratioOfMaxtoTarget < 0: + print("WARNING: The target velocity is negative. The axis will be limited to 100% of its max velocity.") + return abs(min(1, ratioOfMaxtoTarget))