correctness and speed of TriFinger._safety_torque_check

[michaelfeil]

Issue:

• _safety_torque_check is slow and possibly incorrect

  Proposed solution:

• implement a fix speeds up the function (fix 1 or fix 2)
• optionally also change logic (fix 2) of function (would break backward compatability)

I profiled causalworlld env.step() function. It seems roughly 66% is speed in the robot apply action, whereas most in self.step_simulation()

https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/envs/robot/trifinger.py#L324-L371

Of that, 6% of the step time (40 mu s per call) is spent on this safety check implemented in numpy. I did not understand the current implementation of the torque check from a logical point and also believe its rather slow https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/envs/robot/trifinger.py#L415-L438

First of all, this function performs exactly the same function in roughly 5-10 times faster. (5 mu s per call) Reason is that self._max_motor_torque is broadcasted and np.clip is slow. step() Simulation time would speed up by ~4% Fix 1

    def _safety_torque_check(self, desired_torques):
        """

        :param desired_torques: (nd.array) the desired torque commands to be applied
                                            to the robot.

        :return: (nd.array) the modified torque commands after applying a safety check.
        """       
        applied_torques = np.maximum(
            np.minimum(
                np.asarray(desired_torques),
                +self._max_motor_torque,
            ),
            -self._max_motor_torque,
        )

        applied_torques -=  self._safety_kd * self._latest_full_state[
            'velocities']

        applied_torques = np.maximum(
            np.minimum(
                applied_torques,
                +self._max_motor_torque,
            ),
            -self._max_motor_torque,
        )

        return applied_torques.tolist()

In my opinion, this function in general does not implement a reasonable logic. Currently, the safety factor _safety_kd is only applied where there are positive torques and positive velocities. The torque limits are asymmetric, I don't understand how this makes sense on the robot. Maybe, the upper (or respectively; lower ) bounds should be tightened when moving with positive (or respectively; negative) velocity. Fix 2

    def _safety_torque_check(self, desired_torques):
        applied_torques = np.asarray(desired_torques)

        # alternative 
        # torque_bound = self._max_motor_torque - np.abs(self._safety_kd * self._latest_full_state['velocities'])

        torque_bound_upper = self._max_motor_torque - self._safety_kd * np.maximum(self._latest_full_state['velocities'],0.)
        torque_bound_lower = - self._max_motor_torque - self._safety_kd * np.minimum(self._latest_full_state['velocities'],0.)

        # clip upper torque
        applied_torques = np.minimum(
            applied_torques, 
            torque_bound_upper, 
        )
        # clip lower torque
        applied_torques = np.maximum(
            applied_torques, 
            torque_bound_lower, 
        )

        return applied_torques.tolist()

[luator]

Thanks a lot for tracking this down!

Regarding fix 1: I did some quick benchmark and can confirm that np.clip is indeed much slower than calling np.minimum and np.maximum (factor 8 on my machine...), so fixing this makes a lot of sense. From a discussion on the numpy repo I learned that there is also np.core.umath.clip which has overall best performance, so I would suggest using that instead of np.minimum/maximum (it also gives more readable code).

Regarding fix 2: I'm not sure about this change, let's discuss it in the meeting tomorrow.

[michaelfeil]

Thanks for your comment! I was not aware of np.core.umath.clip. It seems like from numpy version 1.16 to 1.17 this got ~11 times slower

timeit.timeit("np.clip(a, -3, +3)",setup="import numpy as np;a=np.asarray([1,1,2,4,-5])", number=1000000)

np.core.umath.clip is not part of the public api of numpy and not available for versions before numpy 1.17. It has comparable speed to np.minimum/maximum (around 5% faster) and is more readable.

For speed across versions, I think the np.minimum/maximum might be still the way to go.

[luator]

Hm, how about something like

try:
    clip = np.core.umath.clip
except AttributeError:
    clip = np.clip

at the top of the file? For <=1.16 np.clip should still be good and for later versions there is np.core.umath.clip.

I'd really prefer to avoid minimum/maxium as I think it makes the code much less readable (and thus easier to miss mistakes).

[michaelfeil]

I think this is a good idea! As this also happens on 3 other parts in CausalWorld (https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/envs/robot/observations.py#L260, https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/envs/scene/observations.py#L260 and https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/envs/robot/action.py#L96), which are called for every step()

I would add your proposal to the utils. https://github.com/rr-learning/CausalWorld/blob/7e2958a1db3af4f801d6d6df4fd1e507304ca537/causal_world/utils/env_utils.py

[michaelfeil]

Is resolved, the logic with 2x clipping works as damping for higher velocities and #102