Contact forces seem unstable when accessed in mjcb_control, but not in main thread

[vmstavens]

Hi,

I'm a Research Assistant and I'm trying to use MuJoCo for modeling contacts.

I am currently building on top of the code provided here produced by this issue.

I am running Ubuntu 20.04.6 LTS and MuJoCo 3.2.0

My problem occurs when i try to get the contact data between two objects from within the controller callback as opposed to the main thread. When I get the contact data within the controller callback as registered with mj.set_mjcb_control, the contact forces seem unstable with very large spikes in the millions of Newton. When I get the contact data from within the main thread, a spike can still be seen in the beginning (which I am also interested in eliminating), but the behavior seems much more stable and gives reasonable results. Here is the MJCF and Python code I have used to generate the results:

My code

minimal Python code

```Python import mujoco as mj import numpy as np from mujoco import viewer with_vis = 1 model = mj.MjModel.from_xml_path("franka_force.xml") data = mj.MjData(model) if with_vis: _viewer = viewer.launch_passive(model, data) # _viewer = viewer.MujocoViewer(model, data) _viewer.cam.azimuth = 153 _viewer.cam.distance = 0.33 _viewer.cam.elevation = -49 _viewer.cam.lookat = [-0.00099796, -0.00137387, 0.04537828] # fingers start in open position data.joint("finger_joint_l").qpos = 0.045 data.joint("finger_joint_r").qpos = 0.045 steps = 10_000 forces_l = np.zeros( (5, steps) ) # collect all individual pad forces, summation is done below forces_r = np.zeros((5, steps)) data_js = {"main": [], "cb": []} def controller_callback(model, data): for j, c in enumerate(data.contact): name1 = data.geom(c.geom1).name name2 = data.geom(c.geom2).name if name1 != "object": continue # lowest ID geoms come first if name2[:3] != "pad": continue # geoms for force measurements need to have "pad" in their name c_ft = np.zeros((6,)) mj.mj_contactForce(model, data, j, c_ft) data_js["cb"].append(np.linalg.norm(c_ft)) mj.set_mjcb_control(controller_callback) for i in range(steps): # data.ctrl = 2 * [0.0] if i % 1000 == 0: # data.actuator("actuator8").ctrl = 0 data.actuator("actuator8").ctrl = ( 255 if data.actuator("actuator8").ctrl != 255 else 0 ) for j, c in enumerate(data.contact): name1 = data.geom(c.geom1).name name2 = data.geom(c.geom2).name if name1 != "object": continue # lowest ID geoms come first if name2[:3] != "pad": continue # geoms for force measurements need to have "pad" in their name c_ft = np.zeros((6,)) mj.mj_contactForce(model, data, j, c_ft) data_js["main"].append(np.linalg.norm(c_ft)) # print(c_ft) # print( # np.linalg.norm(c_ft) # if np.linalg.norm(c_ft) < 1000 # else f"\033[91m{np.linalg.norm(c_ft)}\033[0m" # ) # if np.linalg.norm(c_ft) > 1000: # exit() # f = c_ft[0] # only consider normal force mj.mj_step(model, data) if with_vis: _viewer.sync() if with_vis: _viewer.close() import matplotlib.pyplot as plt fig, ax = plt.subplots(nrows=2) ax[0].plot(data_js["cb"], label="mjcb_control") ax[1].plot(data_js["main"], label="main thread") ax[0].legend() ax[1].legend() # plt.plot(range(steps), np.sum(forces_l, axis=0), label="left") # plt.plot(range(steps), np.sum(forces_r, axis=0), label="right") ax[0].set_title("Grasping Force from mjcb_control") ax[0].set_xlabel("t") ax[0].set_ylabel("F(t)") ax[1].set_title("Grasping Force from main thread") ax[1].set_xlabel("t") ax[1].set_ylabel("F(t)") plt.legend() plt.tight_layout() plt.savefig("graphs.png") plt.show() ```

My MJCF

minimal XML

```XML ```

Picture of forces graphs

For my use case I am interested in getting the contact forces from within the controller callback while keeping the contacts stable, is this possible? or am I misunderstanding or lacking some crucial information about how to apply the MuJoCo API correctly? any and all help is very much appreciated!

Thanks in advance!

[Balint-H]

Hello!

This is almost certainly related to this: #1360

Manual access of contact data after step1 (where mjcb_control is called) will give correct kinematics (position, orientation) but uninitialized dynamics (forces), which is calculated in step2, after the callback. This is for contacts that are newly added in the most recent step1.

Here's the relevant docs.

You need to either perform the relevant dynamics calls to calculate the forces, or hold over readings from the last frame's step2. That's what sensors do as well. You can potentially set up smart contact filtering on the geoms, and possibly duplicate some geoms to get this kind of specificity with the built in sensors as well.

Lastly, you can set up custom sensors and submit a sensor callback.

[vmstavens]

@Balint-H thank you so much for the quick response! Yeah, your post seems very similar to what I am experiencing, in the mean time I will attempt to figure out which dynamics functions to call in order to get the desired contact forces :)