Open
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
- Load the model below.
- Run the code below.
- Interact with model by applying external force to the model
- 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:
<mujoco model="left_ankle_roll_link">
<compiler angle="radian" meshdir="meshes/"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="-130" elevation="-20"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"/>
</asset>
<worldbody>
<geom name="floor" size="0 0 0.05" pos="0 0 -0.1" type="plane" material="groundplane"/>
<body name="left_ankle_roll_link">
<joint type='free'/>
<geom name="left_ankle_roll_collision" size="0.109 0.0325 0.0025" pos="0.044356 0 -0.02951" type="box" group="1" rgba="0 0.7 0 1"/>
<body name="left_force_platform_link" pos="0.044356 0 -0.03201">
<body name="left_force_sensor_1" pos="0 0 0">
<site name="left_foot_fsr_1" pos="0.098 0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.006" pos="0.098 0.022 -0.0242" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_2" pos="0 0 0">
<site name="left_foot_fsr_2" pos="0.098 -0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.006" pos="0.098 -0.022 -0.0242" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_3" pos="0 0 0">
<site name="left_foot_fsr_3" pos="-0.098 -0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.006" pos="-0.098 -0.022 -0.0242" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_4" pos="0 0 0">
<site name="left_foot_fsr_4" pos="-0.098 0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.006" pos="-0.098 0.022 -0.0242" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<geom name="left_force_platform_collision" size="0.12 0.053 0.006" pos="0 0 -0.0242" type="box" group="1" rgba="0 0 0.7 1"/>
<site name="left_foot_fsr_sum" pos="0 0 -0.0302" rgba="1 0.65 0 1" type="cylinder"/>
<body name="left_foot" pos="0 0 -0.0302">
<inertial pos="0 0 0" mass="0.001" diaginertia="1e-07 1e-07 1e-07"/>
</body>
</body>
</body>
</worldbody>
<sensor>
<force site="left_foot_fsr_1" name="left_ankle_force_1"/>
<force site="left_foot_fsr_2" name="left_ankle_force_2"/>
<force site="left_foot_fsr_3" name="left_ankle_force_3"/>
<force site="left_foot_fsr_4" name="left_ankle_force_4"/>
<force site="left_foot_fsr_sum" name="left_ankle_force_sum"/>
</sensor>
</mujoco>
The second one without interfere:
<mujoco model="left_ankle_roll_link">
<compiler angle="radian" meshdir="meshes/"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="-130" elevation="-20"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"/>
</asset>
<worldbody>
<geom name="floor" size="0 0 0.05" pos="0 0 -0.1" type="plane" material="groundplane"/>
<body name="left_ankle_roll_link">
<joint type='free'/>
<geom name="left_ankle_roll_collision" size="0.109 0.0325 0.0025" pos="0.044356 0 -0.02951" type="box" group="1" rgba="0 0.7 0 1"/>
<body name="left_force_platform_link" pos="0.044356 0 -0.03201">
<body name="left_force_sensor_1" pos="0 0 0">
<site name="left_foot_fsr_1" pos="0.098 0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.003" pos="0.098 0.022 -0.0212" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_2" pos="0 0 0">
<site name="left_foot_fsr_2" pos="0.098 -0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.003" pos="0.098 -0.022 -0.0212" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_3" pos="0 0 0">
<site name="left_foot_fsr_3" pos="-0.098 -0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.003" pos="-0.098 -0.022 -0.0212" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<body name="left_force_sensor_4" pos="0 0 0">
<site name="left_foot_fsr_4" pos="-0.098 0.022 -0.0091" size="0.0101 0.0091" rgba="1 0.65 0 1" type="cylinder"/>
<geom size="0.0101 0.0101 0.003" pos="-0.098 0.022 -0.0212" type="box" group="1" rgba="0.7 0 0 1"/>
</body>
<geom name="left_force_platform_collision" size="0.12 0.053 0.003" pos="0 0 -0.0272" type="box" group="1" rgba="0 0 0.7 1"/>
<site name="left_foot_fsr_sum" pos="0 0 -0.0302" rgba="1 0.65 0 1" type="cylinder"/>
<body name="left_foot" pos="0 0 -0.0302">
<inertial pos="0 0 0" mass="0.001" diaginertia="1e-07 1e-07 1e-07"/>
</body>
</body>
</body>
</worldbody>
<sensor>
<force site="left_foot_fsr_1" name="left_ankle_force_1"/>
<force site="left_foot_fsr_2" name="left_ankle_force_2"/>
<force site="left_foot_fsr_3" name="left_ankle_force_3"/>
<force site="left_foot_fsr_4" name="left_ankle_force_4"/>
<force site="left_foot_fsr_sum" name="left_ankle_force_sum"/>
</sensor>
</mujoco>
Code required for reproduction
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
- I searched the latest documentation thoroughly before posting.
- I searched previous Issues and Discussions, I am certain this has not been raised before.