google-deepmind / google-deepmind/mujoco
Multiple force sensor placing
- Dominant language
- C++
- Stars
- 15.2k
- Forks
- 1.8k
- Avg merge
- 10d 16h
- Merged PRs (30d)
- 25
Description
### Intro
Hi!
I am a PhD student at ITMO university, I use MuJoCo for my research on legged robots.
### My setup
Mujoco 3.3.1, Windows, Ubuntu
### What's happening? What did you expect?
I'm working on a foot model with 4 single-axis force sensors (FSRs) embedded in the foot sole. My goal is to estimate the 3D force vector acting on the foot based on the readings from these sensors.
I’ve tried implementing the sensors in two different ways, but I’m getting inconsistent results:
In one setup, the sum of the forces from the 4 sensors is much smaller than the force measured by a reference force sensor (placed under the foot).
In another setup, the sum of the forces is significantly larger than the reference sensor.
I'm unsure whether the issue is in how I'm placing the sensors, how MuJoCo computes the force readings from sites, or something else.
Could you please clarify the correct way to define single-axis force sensors in MuJoCo so that their readings can be reliably used to reconstruct a full 3D force vector?
Thank you very much in advance!
### Steps for reproduction
1. Load the model below.
2. Run the code below.
3. Interact with model by applying external force to the model
4. Receive plot, I'm intresting why second plot not converge
### Minimal model for reproduction
Here is a tro model variants, the first one with interfere collision shapes between force sensors and platform:
```
```
The second one without interfere:
```
```
### Code required for reproduction
```python
import mujoco
import mujoco.viewer
import numpy as np
import matplotlib.pyplot as plt
side = "left"
xml_path = f"test_model.xml"
model = mujoco.MjModel.from_xml_path(xml_path)
data = mujoco.MjData(model)
def get_sensors_data(data):
return (
data.sensor(f"{side}_ankle_force_1").data[2],
data.sensor(f"{side}_ankle_force_2").data[2],
data.sensor(f"{side}_ankle_force_3").data[2],
data.sensor(f"{side}_ankle_force_4").data[2],
)
def get_sensors_force_sum(data):
return (
data.sensor(f"{side}_ankle_force_sum").data[0],
data.sensor(f"{side}_ankle_force_sum").data[1],
data.sensor(f"{side}_ankle_force_sum").data[2],
)
def setup_vis(model, data):
model.vis.scale.framelength = 0.3
model.vis.scale.framewidth = 0.03
viewer = mujoco.viewer.launch_passive(
model, data, show_left_ui=False, show_right_ui=False
)
viewer.opt.frame = mujoco.mjtFrame.mjFRAME_BODY
viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_PERTFORCE] = np.uint8(1)
return viewer
def get_plots(t, force_sensors_data, force_sum, force_sum_calc, side):
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8), sharex=True)
ax1.plot(t, force_sensors_data, label=['Force sensor 1', 'Force sensor 2', 'Force sensor 3', 'Force sensor 4'])
ax1.set_ylabel('Force (N)')
ax1.set_title('Data from sensors')
ax1.legend()
ax1.grid(True)
force_sum = np.array(force_sum)
force_sum_calc = np.array(force_sum_calc)
ax2.plot(t, force_sum[:, 0], '--r', label='Fx')
ax2.plot(t, force_sum[:, 1], '--g', label='Fy')
ax2.plot(t, force_sum[:, 2], '--b', label='Fz')
ax2.plot(t, force_sum_calc[:, 0], 'r', label='Fx calculated')
ax2.plot(t, force_sum_calc[:, 1], 'g', label='Fy calculated')
ax2.plot(t, force_sum_calc[:, 2], 'b', label='Fz calculated')
ax2.set_xlabel('Time (sec)')
ax2.set_ylabel('Force (N)')
ax2.set_title(f'Force vector at body {side}_foot')
ax2.legend()
ax2.grid(True)
plt.tight_layout()
plt.show()
def get_full_force(data, idx):
return data.sensor(f"{side}_ankle_force_{idx}").data[:3]
def get_force_sum(data):
F_sum = get_full_force(data, 1) + get_full_force(data, 2) + get_full_force(data, 3) + get_full_force(data, 4)
return F_sum[0], F_sum[1], F_sum[2]
t = []
force_sensors_data = []
force_sum = []
force_sum_calc = []
viewer = setup_vis(model, data)
while viewer.is_running():
mujoco.mj_step(model, data)
t.append(data.time)
F_1, F_2, F_3, F_4 = get_sensors_data(data)
force_sensors_data.append([F_1, F_2, F_3, F_4])
Fx, Fy, Fz = get_sensors_force_sum(data)
force_sum.append([Fx, Fy, Fz])
Fx_calc, Fy_calc, Fz_calc = get_force_sum(data)
force_sum_calc.append([Fx_calc, Fy_calc, Fz_calc])
viewer.sync()
get_plots(t, force_sensors_data, force_sum, force_sum_calc, side)
```
### Confirmations
- [x] I searched the [latest documentation](https://mujoco.readthedocs.io/en/latest/overview.html) thoroughly before posting.
- [x] I searched previous [Issues](https://github.com/google-deepmind/mujoco/issues) and [Discussions](https://github.com/google-deepmind/mujoco/discussions), I am certain this has not been raised before.
Contributor guide
Assessment
This issue has not been assessed yet.