Force direction detected by sensor

[zichunxx]

Hi!

I've been confused about the force direction detected by the force sensor for a while and made a MWE.zip to illustrated my confusion with the impedance controller made by @kevinzakka.

First of all, I found that creating a dummy body is a great way to gather force information from here.

Second, as illustrated in the document, the detected force is expressed in the site frame and points from the child body to the parent body.

For the above MWE, I create a dummy body containing a sensor site on the tip of the peg and simulate a process of moving vertically downward toward the ground and hitting the ground. Running the python script in the MWE can reproduce the process directly without any tuning.

Here is the force information in the z-direction:

For my example, I think the force is negative because the z-axis of the sensor site frame points downward, and the force points from the dummy body to the box_eef body, which is upward. You can also find body labels in the above figure.

1. But why the detected force is positive?

2. I also found the dummy body should be in the right position but the position of the sensor site does not matter, why?

I have found some related issues but they didn't solve my confusion. Here is the list:

• force sensor problem #1249
• Some details and questions about the force-aspect sensors #399
• force and torque sensor #997

Thanks in advance for your precious time in checking the MWE. Any comment would be appreciated.

[kevinzakka]

Hi @zichunxx, I'm looking into this and will get back to you soon.

[zichunxx]

Hi @zichunxx, I'm looking into this and will get back to you soon.

Thanks in advance for your generous help.

[zichunxx]

Hi! @kevinzakka Sorry for the interruption. Did you find anything wrong?

[egordon]

Chiming in a little late, but I am experiencing something similar.

Loading this XML:
default.txt

spherebot1 is on the +x side of the box, while spherebot2 is on the -x side of the box. They should start in collision.

When I check the contact sensor output using the following code (v3.3.4) it appears to return the opposite of what I would expect given the output of mj_contactForce. I apologize if I have some misunderstanding, but at the moment in more complicated code I just have to take -1 * sensordata.

import mujoco
import numpy as np
mj_model = mujoco.MjModel.from_xml_path("default.txt")
mj_data = mujoco.MjData(mj_model)
mujoco.mj_step(mj_model, mj_data)
print(f"Spherebot1 [+x] -> Box Geometry ID: {mj_data.geom('spherebot1-geom').id} -> {mj_data.geom('object-geom').id}") # 2 -> 1
print(f"Contact 4 Geometry ID: {mj_data.contact[4].geom}") # [2 1]
result = np.zeros(6)
mujoco.mj_contactForce(mj_model, mj_data, 4, result)
world_frame_force_on_box = mj_data.contact[4].frame.reshape((3,3)).T @ result[:3].T
# Note: diagonal matrix in this case so removing '.T' doesn't change result

print(f"Contact force on Box (world frame): {world_frame_force_on_box}") # [-4... 0... 2....]
print(f"Contact force on Spherebot1 (world frame): {-world_frame_force_on_box}") # [4... 0... -2....]
print(f"Site Frame (== world frame): {mj_data.site("spherebot1-site").xmat.reshape((3,3))}") # Identity

#### This seems backwards
start = mj_model.sensor("spherebot1-contact").adr[0]
end = start + mj_model.sensor("spherebot1-contact").dim[0]
print(f"Spherebot1 contact sensordata (site == world frame): {mj_data.sensordata[start:end]}") # [-4... 0... 2....]
####