[gym] Unable to run simple Atlas simulation because truncated is true immediately

[mwulfman]

Reproduction scripts:

import numpy as np
from gym_jiminy.envs.atlas import AtlasJiminyEnv
from jiminy_py import tree

env = AtlasJiminyEnv()
for i in range(100):
    obs, _ = env.reset(seed = i)
    terminated, truncated = False, False
    while not (terminated or truncated):
        assert not any(np.isnan(e).any() for e in tree.flatten(obs)), "Nan in observation"
        obs, _, terminated, truncated, _ = env.step(env.action_space.sample())
        print(terminated, truncated, not env._contains_observation(), env.observation_space.contains(obs))
        # truncated is true immediately because of env._contains_observation()
        truncated = env.stepper_state.t >= env.simulation_duration_max - env.step_dt # comment this line to make it work
    env.replay()

I am not able to run an Atlas simulation for more than step_dt because truncated is set to true by env._contains_observation() after the first step. I am not sure this is the desired behavior. Since the observation is clipped by the observation_space after being returned by step, it seems contradictory to return truncated = True when the unclipped observation is outside the observation_space. Even though, even without clipping the observation, the simulator should not return observation outside the observation_space, no ?

Difference between obs and env.observation (respectively clipped and unclipped) are just observed on ["states"]["agent"]["v"] and the v_measurement of ["measurements"]["EncoderSensor"].

[duburcqa]

It is not clear to me if we should set terminated = True if the observation is out of bounds. After checking, you are violating the maximum velocity limit. You are exceeding the hardware specification, so it cannot be executed on the robot. I have the felling it is a real problem and the simulation should be terminated for this.

[duburcqa]

Maybe the actual problem is coming from the fact that those specifications are not really working this way in really. In the real world, velocity limit does not exist. If you have some external source of torque, you could spine the joint as fast as you want. In practice, there is a relation between motor torque and angular velocity which limits the measured velocity ultimately. Without external source of energy, the joint velocity is therefore limited.

What I said previous still stand. I don't know what we want to do in this scenario. It was specified in the URDF that the joints are not supposed to spine faster than some maximum velocity, and I think we should honour that by default. Whether some command torque is capable of invalidating the current state is an another issue. Here it is the case because the actuators are very powerful. But I'm also expecting the system to break if you start standing random torques to the actuators in reality, so it is not a false positive.

[duburcqa]

In my view, It is the responsibility of the practitioner to send sensible commands to the robot to avoid invalidating the robot state. As much as you cannot explore randomly in reality, you should be careful when exploring in simulation. Such issue would not appear when plugging a low-level PD controller with velocity bounds for instance.

[mwulfman]

Would it be possible to send an additional warning message when the commanded torque is unfeasible given the velocity limits ? I feel like this behavior is not explicit enough, especially in the learning situation.

[duburcqa]

How to detect this is not straightforward as it depends on the dynamics of the whole system. Maybe some estimate based on the apparent inertia could be computed but it would only be rough approximation as it also involves all the external forces, mechanical constraints...etc.

I agree it would be nice to detect such issue differently than just detecting than the observation is out of bounds but I'm not sure it can be predicted reliably. I'm still convinced that controlling a robot in torque is tricky and should be avoided if possible, one of the reason being unreasonable joint velocities when sending random commands.

[mwulfman]

Ok, yes I understand your point. Does Atlas has default acceleration limit as well ?
The same issue appears with this new code (unless I lower the action sampling range)

import numpy as np
from gym_jiminy.common.bases import DT_EPS
from gym_jiminy.common.blocks.proportional_derivative_controller import (
    pd_adapter,
    pd_controller,
)
from gym_jiminy.common.utils import is_breakpoint, sample
from gym_jiminy.envs.atlas import PD_FULL_KD, PD_FULL_KP, AtlasJiminyEnv
from jiminy_py import tree

PD_KP = np.array(PD_FULL_KP)
PD_KD = np.array(PD_FULL_KD)


env = AtlasJiminyEnv(step_dt=0.005)

nmotors = env.robot.nmotors
# Limits
position_limit_lower = env.robot.position_limit_lower[env.robot.motor_position_indices].squeeze()
position_limit_upper = env.robot.position_limit_upper[env.robot.motor_position_indices].squeeze()
velocity_limit = env.robot.velocity_limit[env.robot.motor_velocity_indices]
motors_effort_limit = env.robot.command_limit[env.robot.motor_velocity_indices]
acceleration_limit = np.full((nmotors,), float("inf"))
# Buffers
desired_command_acceleration = np.full((nmotors,), float("nan"))
command_torque = np.full((nmotors,), 0.0)
command_state = np.full((3, nmotors,), 0.0)
command_state_lower = np.stack((position_limit_lower, -velocity_limit, -acceleration_limit))
command_state_upper =  np.stack((position_limit_upper, velocity_limit, acceleration_limit))

# Target position
neutral_motor_position = env._neutral()[env.robot.motor_position_indices].squeeze()
target_dt = 0.05

for i in range(100):
    obs, _ = env.reset(seed = i)
    terminated, truncated = False, False
    while not (terminated or truncated):
        assert not any(np.isnan(e).any() for e in tree.flatten(obs)), "Nan in observation"
        q_measured, v_measured = env.observation["measurements"]["EncoderSensor"]
        if env.stepper_state.t < env.step_dt:
            command_state[0] = q_measured
            command_state[1] = v_measured
        if is_breakpoint(env.stepper_state.t, target_dt, DT_EPS):
            action = sample(low=-velocity_limit, high=velocity_limit, rg=env.np_random)
            pd_adapter(action, 1,
                    command_state,
                    command_state_lower,
                    command_state_upper,
                    target_dt,
                    desired_command_acceleration)
        command_state[2] = desired_command_acceleration
        pd_controller(q_measured,
                      v_measured,
                      command_state,
                      command_state_lower,
                      command_state_upper,
                      PD_KP,
                      PD_KD,
                      motors_effort_limit,
                      env.step_dt,
                      command_torque)
        obs, _, terminated, truncated, _ = env.step(command_torque)
    env.simulator.stop()
    env.replay()

[duburcqa]

You should have a look to AtlasPDControlJiminyEnv pipeline environment. Max velocity and acceleration are specified to avoid this issues. The base AtlasJiminyEnv is exposed for expert but it is not really supposed to be used as-is directly.

[mwulfman]

Ok thanks, I will have a look.

[duburcqa]

when I am respecting the position, velocity and acceleration limits

You should be more restrictive than that. Satisfying the limits for the command is not enough to make sure that the measure is not out of bounds. That is why the threshold are much lower in AtlasPDControlJiminyEnv.

[mwulfman]

I tried adding the same margin to the command_state_lower/upper but it resulted in similar behavior.

[duburcqa]

I converted this issue in a discussion since what to do it this point is not clear to me.

[mwulfman]

So I have tried doing a step with DigitPDControlJiminyEnv, but I also get a truncated at reset.

Reproduction script:

from gym_jiminy.envs.digit import DigitPDControlJiminyEnv

if __name__ == '__main__':
    env = DigitPDControlJiminyEnv()
    obs, _ = env.reset()
    env.step(env.action_space.sample()*0.0)
    print(env.has_terminated())

It is truncated due to the difference:

print(obs["measurements"]["EffortSensor"] - env.observation["measurements"]["EffortSensor"])

[mwulfman]

Same thing with CassiePDControlJiminyEnv

[mwulfman]

The issue still comes from env.observation_space.contains(env.observation) = False when env.observation_space.contains(obs) = True (since the latter is clipped it's normal).

I think an error should be raised when the observation is outside the observation_space. It is never desirable to be in this situation, even if it happens only a few time during a training. I don't see any situation where this is a good thing. In my opinion, it is the responsibility of the practitioner to clip himself the observation if they don't want them outside a certain range or widen the observation_space so it can capture the observable behavior. But the practitioner needs to know that it is happening.
If the observable behavior is outside the expected one (i.e. real physical one) then this is a simulator physics issue and not an RL environment issue.

I believe Ray/RLlib reports an error when it notices this situation.

However, I understand that here the philosophy tends to be different with the Pipeline. What are you thoughts?

[duburcqa]

But the practitioner needs to know that it is happening. If the observable behavior is outside the expected one (i.e. real physical one) then this is a simulator physics issue and not an RL environment issue.

Here is the shortcoming with this reasoning. Determine whether the observation is out of bounds because the sensors are saturated or anything else for which clipping the observation makes sense, or because it violates the physics, is tricky and requires fairly advanced knowledge about the system at hand.

I believe Ray/RLlib reports an error when it notices this situation.

If it notices what exactly ?

However, I understand that here the philosophy tends to be different with the Pipeline.

What do you mean ?

What are you thoughts?

For now, what I already did on dev branch if removing manually added bounds on the observation space, ie which are not part of the physics. I am also considering removing the velocity limit, since it is not motivated by a physical reasoning in the first place. There is a slowdown effect at motor level but that is all. No strict limitation. I will think about it propose a patch in the coming days. Once the velocity limit issue is dealt with, there should be no more undesirable truncation I think.

[mwulfman]

Here's some insight about terminated vs truncated: https://farama.org/Gymnasium-Terminated-Truncated-Step-API

[duburcqa]

Could you share the model that is used at Wandercraft for limiting the output motor torque based on its velocity ? So that I can check whether it can be implemented at jiminy level without introducing too many advanced motor parameters. I will also have a look on my side.

[duburcqa]

@matthieuvigne What do you think about implementing a simple motor model with parameters:

• max torque at 0 velocity (U_max)
• max velocity at 0 torque (V_max)
• max velocity at max torque (V_th)

They are high-level parameters, easy to understand for anybody. From them, we can derive a simple linear model:

It seems better than nothing ? All it requires is introducing the new "V_th" parameter and clipping the current torque as follows:

I'm wondering what should be the default value for "v_th" if not specified by the user. It seems to vary significantly depending on the motor specifications (something between 0.1 * v_max, and 0.9 * v_max). I am inclined to be conservative and set it to 0.9.

I'm also considering implementing another, more realistic motor model, taking into account many more motor-level parameters, eventually some thermal effects at some point...etc as soon as jiminy supports integration of internal motor dynamics. But this is not going to happen any time soon though.

[matthieuvigne]

Sure, this model definitely makes sense - in particular because it's simpler to understand and thus less error-prone than inputting the true motor parameters (I can see a user specifying these values without advanced knowledge). It can even be seen physically as a simplified model where we neglect the effect of the stator inductance (which is the reason for the 'curved' shape of the velocity limit that is observed on real motors) - and only take into account the resistive effect, which is typically dominant in particular at low current.
As for the value to use, as you say these can vary quite significantly depending on the motor, but also on the requested drive current (driving the motor at higher current obviously reduces this value). From my experience 90% is quite optimistic, I'd rather use something like 70% - but I understand you don't want to limit the user too much.

This makes me wonder what is the expected use case for you: to have this as the default, always-on model, or only something that the user activates ? Both have pros and cons to me, activating the model by default could surprise a new user (why doesn't my motor spin at the specified max velocity?), but is more representative of real life. On the other hand, if it's a feature that the user actives on his own, then couldn't you just rely on him giving a value (if the user is advanced enough to know of this effect, then he can supply a value - but this makes assumptions on the user's level of understanding) ?

As a side note: I've seen on Maxxon's datasheet that they give the slope of this approximate limit (which they call speed/torque gradient, in Nm/rad/s). This could be an alternative parametrization: the nice thing for me is that this is closer to the physical truth and makes the velocity limit independent of the max torque (whereas if you say 'max speed is 1000rpm @ max torque', then change the max torque later on, you actually change the velocity behavior at low speed). The downside is that it's less intuitive than just giving a velocity.

[duburcqa]

to have this as the default, always-on model, or only something that the user activates ?

What I had in mind, was implementing an opt-in model with default values: no velocity limit by default, with max velocity parsed for the URDF file, and the slope initialised to some guess in Jiminy (may 90% to be conservative, or just an infinite slope if less confusing).

if the user is advanced enough to know of this effect, then he can supply a value

I would say not really, since the velocity limit is coming from the URDF. The user may only want to enable the velocity limit, without really knowing what it means under the hood.

they give the slope of this approximate limit (which they call speed/torque gradient, in Nm/rad/s)

Yes, I see the benefits. Let's go for it, something like "velocity_torque_slope".

[duburcqa]

Implemented in #798

[duburcqa]

The problem has been fixed for Atlas on dev branch ! I need to fix some issue with mechanical reduction handling and it should be OK for Cassie and Digit.

[duburcqa]

OK #801 fixes the issue for all PID controlled robotic environments . Closing !