Accurate external forces at the moment of calling `mjcb_act_dyn`

[VasilyRakche]

Hey MuJoCo team,

At the moment when mjcb_act_dyn is called from mj_fwdActuation(m, d) all external forces are still not available (up to date), since further, in mj_fwdConstraint(m, d) qfrc_constraint forces are recomputed and based on them qacc are updated.

As a result, one can not perceive all external forces at the moment when mjcb_act_dyn is called. My goal is to implement custom dynamic and for that its absolutely crucial to have this info. Can you recommend what can I do in order to get updated forces at the moment of calling mjcb_act_dyn ?

[erez-tom]

Actuation is computed before contact forces because in many cases the actuation signal directly affects the forces in the system (and therefore affects the resulting contact forces). When you have second-order actuators, the actuation input only changes some actuation state variables (called act), and only act affects the forces in the system. In this specific case, you could choose to compute the actuation (which will change act) either before or after the physics step was computed (which integrates the effect of act's value).

Does this description match your case? Would you be content with the actuation signal at time t to have no effect on the state of the multibody system at time t (only on the actuator state, which will only affect the next step)?

[I can perhaps suggest some ideas in my next post, but I first want to make sure I understand your situation adequately]

[VasilyRakche]

Hey, thanks for your answer. It took a bit longer to provide feedback since I wanted to write a bit of code (minimal example) to show what I mean. Its not straight forward, but I hope it can clarify details.

First, regarding your comments. As I far as I see, you mean implementing my own ode which can be handled through act_dot being integrated? Thats kinda what I am doing already, but the problem is that I need to get an accurate force feedback from the environment in order to represent the actuator dynamics properly. Within my custom dynamics, I also update the qfrc_applied within mjcb_act_dyn.

What I try to represent in a system is a clutch which would block the movement of the actuator:

- Clutch engaged:
      - qvel = 0
      - qacc = 0
- Clutch disengaged
      - no constraints on qvel 
      - no constraints on qacc 

The way I am trying to do is:

• Having a Cp as a selector matrix (indicating which qvel should be 0) one can follow derivation from this paper [1] in order to get the forces ($\lambda$) that should be applied in order to compensate all external forces:

This successfully works in this code repo. (One can notice that the printed joint_0 position is not changing) However, as soon as I add constraints to the joint_1 as follows:

                <joint name="joint_1" range="-3.14 3.14" axis = "1 0 0" pos="0 0 -0.2"/>

You will notice that from when qfrc_constraint becomes non-zero for any of the actuators (that's valid due to coupling), I can not accurately estimate and compensate the forces. Thus joint_0 will start moving (check the printed messages in the terminal). I am not sure how it can be handled by delaying the input with act.

Minimal example provided here

[1] D. Ossadnik et al., "BSA - Bi-Stiffness Actuation for optimally exploiting intrinsic compliance and inertial coupling effects in elastic joint robots," 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022, pp. 3536-3543, doi: 10.1109/IROS47612.2022.9981928.

[yuvaltassa]

What you want to do, at least in the way you are currently phrasing it, is impossible. Given any controlled dynamical system where the state $x$ evolves like $\dot x = f(x,u)$, there will be some measured values which are a function of the control $u$; you cannot make all measured values be solely a function of $x$ (trivially, a measurement of $u$ cannot be solely a function of $x$).

More specifically, in a 2nd-order mechanical dynamical system where $x=(q, \dot q)$, forces are not part of the state, they are something that is computed as a function of both the state and the applied forces. So your desire to "know the forces in the system in order to compute the actuation forces" is logically flawed: the forces in the system are themselves a function of the actuation forces, so there is a circular, non-causal dependency.

Regarding the paper you linked to, the un-physicality is manifested in assumption "A2: the impact is instantaneous". This implies that velocities change instantaneously, which is not how 2nd order systems operate, only accelerations are instantaneous. To be clear, this is an assumption made by "velocity stepping" or "position based dynamics" simulators (since it has many numerical benefits), but MuJoCo adheres to the 2nd-orderness of real mechanics and does not take such shortcuts.

What @erez-tom proposed, which I believe you understood correctly, is the only valid solution. By making the actuation forces themselves a state variable (and the full system 3rd-order, rather than 2nd order), you can make the forces in the system not directly affected by the control. Of course in this case the actuator states (mjData.act or your own thing, if you like) will be a function of the control $u$, so the assertion in the first paragraph is inescapable.

Another thing you could do, if you really wanted to, is:

1. Call mj_forward with some guess of the controls.
2. Compute whatever you want in order to get your control $u$, copy that into ctrl.
3. Call mj_step with your new controls.

Of course your new controls would be compensating for forces that did not take these very controls into account, so you might find you need to repeat steps 1 and 2 until you've (hopefully) reached a fixed point. However, like I hope the above made clear, this would be a non-causal controller which "looks into the future", and would not be applicable to real hardware. It might have theoretical significance, I don't really know the details of your work.

Closing for now. Feel free to continue the conversation here if you like.