Issue with Force/Contact sensing #1962

IntrovertInDisguise · 2024-07-18T11:13:40Z

IntrovertInDisguise
Jul 18, 2024

Hi,
I am a student working on a few simulations involving contact force measurements with the ground plane. The net contact force experienced does not match the weight of the object in a lot of simulations, even in static cases. How do I fix this?

I have coded up a minimal cuboid example seen below :
`import mujoco
import numpy as np
import matplotlib.pyplot as plt
import tempfile
import mediapy as media

MJCF model as a string

mjcf_model = """

<!-- Define the world body -->
<worldbody>
    <!-- Ground plane -->
    <geom name="ground" type="plane" pos="0 0 0" size="10 10 0.1" rgba="0.5 0.5 0.5 1"/>

    <!-- Cuboid -->
    <body name="cuboid" pos="0 0 10" >
        <geom name="cuboid_geom" type="box" size="0.2 0.2 0.2" rgba="1 0 0 1" mass="10"/>
        <joint name="cuboid_joint" type="free"/>
        <site name="contact_site" pos="0 0 -0.2" size="0.05" rgba="0 1 0 1"/>
    </body>

    <!-- Add a directional light -->
    <light name="light1" pos="0 0 2" dir="0 0 -1" diffuse="1.0 1.0 1.0" specular="0.5 0.5 0.5"/>
    <light name="light2" pos="2 2 2" dir="-1 -1 -1" diffuse="0.8 0.8 0.8" specular="0.3 0.3 0.3"/>
</worldbody>

<!-- Sensors -->
<sensor>
    <!-- Touch sensor at the cuboid's contact site -->
    <force site="contact_site" name="contact_touch"/>
</sensor>

<!-- Actuators (optional, for later use) -->
<actuator>
    <!-- Add actuators here if needed -->
</actuator>

""" def compute_cuboid_weight(): cuboid_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cuboid") cuboid_mass = model.body_mass[cuboid_body_id] gravity = np.array([0, 0, -9.81]) weight = cuboid_mass * gravity return weight

Function to run MuJoCo simulation and capture contact forces

def run_simulation_and_capture_contact(model, sim_duration=10, framerate=30):
data = mujoco.MjData(model)
renderer = mujoco.Renderer(model)

# Initialize arrays to store contact forces and weights
contact_forces = []
weights = []

# Initialize list to store frames
frames = []

# Set up camera and options for rendering
options = mujoco.MjvOption()
mujoco.mjv_defaultOption(options)
options.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = True
options.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = True

cam = mujoco.MjvCamera()
cam.azimuth = 90
cam.elevation = -30
cam.distance = 4.0
cam.lookat = np.array([0, 0, 0])

# Step the simulation to let the cuboid settle on the ground
for _ in range(int(sim_duration / model.opt.timestep)):
    mujoco.mj_step(model, data)
    
    # Get the net contact force measurement from the touch sensor
    sensor_touch = data.sensordata
    print(sensor_touch)
    contact_forces.append(sensor_touch)
    

    # Capture frames for the video
    if len(frames) < data.time * framerate:
        renderer.update_scene(data, cam, options)
        pixels = renderer.render()
        frames.append(pixels)

# Convert to numpy arrays for plotting
contact_forces = np.array(contact_forces)

# Return captured data
return contact_forces, frames

Write the MJCF model to a temporary file

with tempfile.NamedTemporaryFile(suffix=".xml", delete=False) as tmp_file:
tmp_file.write(mjcf_model.encode())
tmp_filepath = tmp_file.name

Load the model

model = mujoco.MjModel.from_xml_path(tmp_filepath)

Run simulation and capture contact forces

contact_forces, frames = run_simulation_and_capture_contact(model)
weights = np.array(weights)

Plot the contact force over time

time = np.arange(len(contact_forces)) * model.opt.timestep
plt.figure()
plt.plot(time, contact_forces, label='Measured Contact Force (N)')
plt.plot(time, weights[:, 2], label='Computed Weight (N)', linestyle='dashed')
plt.xlabel('Time (s)')
plt.ylabel('Force (N)')
plt.legend()
plt.title('Contact Force Over Time')
plt.show()

Show the video using mediapy

media.show_video(frames, fps=30)
`
Here are my outputs:

cuboidex.mp4

Am I reading the force sensors right?

yuvaltassa · 2024-07-20T18:19:01Z

yuvaltassa
Jul 20, 2024
Maintainer

Your issue is that the site is too small. Usually the size/shape of a site is meaningless, but not for touch sensors. They only reports contacts that happen inside.

Is the documentation unclear?

https://mujoco.readthedocs.io/en/latest/XMLreference.html#sensor-touch

0 replies

IntrovertInDisguise · 2024-07-22T03:25:03Z

IntrovertInDisguise
Jul 22, 2024
Author

Apologies, though I have commented that I have been using a touch sensor, I have used a force sensor to measure touch(in the XML), as I did note this behaviour from the docs. If I am not wrong, this behaviour should not extend to force sensing.

0 replies

tawjaw · 2024-07-22T04:51:47Z

tawjaw
Jul 22, 2024

@IntrovertInDisguise Force sensor outputs the interaction force between a child and parent body.

As explained in the docs

https://mujoco.readthedocs.io/en/stable/XMLreference.html#sensor-force

You can create a dummy welded (no joints) body as a child to your body with very small mass. The interaction force between the two bodies would be equivalent to the force you are trying to measure on the "parent" body.

The discussion here might help #399

0 replies

IntrovertInDisguise · 2024-07-27T00:42:10Z

IntrovertInDisguise
Jul 27, 2024
Author

@IntrovertInDisguise Force sensor outputs the interaction force between a child and parent body.

As explained in the docs

https://mujoco.readthedocs.io/en/stable/XMLreference.html#sensor-force

You can create a dummy welded (no joints) body as a child to your body with very small mass. The interaction force between the two bodies would be equivalent to the force you are trying to measure on the "parent" body.

The discussion here might help #399

@tawjaw I tried this idea, but there was no apparent change in output. Does this implementation look fine?

`import mujoco
import numpy as np
import matplotlib.pyplot as plt
import tempfile
import mediapy as media

MJCF model as a string

mjcf_model = """

<!-- Define the world body -->
<worldbody>
    <!-- Ground plane -->
    <geom name="ground" type="plane" pos="0 0 0" size="10 10 0.1" rgba="0.2 0.3 0.4 1"/>

    <!-- Cuboid -->
    <body name="cuboid" pos="0 0 1">
        <geom name="cuboid_geom" type="box" size="0.2 0.2 0.2" rgba="1 0 0 1"/>
        <joint name="cuboid_joint" type="free"/>
        <!-- Sensor site similar to quadruped -->
        <site name="contact_site" pos="0 0 -0.2" size="0.05" rgba="0 1 0 1"/>
        <body name="dummy_body" pos="0 0 -0.25">
            <inertial pos="0 0 0" mass="0.000001" diaginertia="0 0 0"/>
            <geom type="box" size="0.01 0.01 0.01" rgba="0 1 0 1"/>
        </body>
    </body>
</worldbody>

<!-- Sensors -->
<sensor>
    <!-- Force sensor at the cuboid's contact site -->
    <force site="contact_site" name="contact_force"/>
</sensor>

"""

Write the MJCF model to a temporary file

with tempfile.NamedTemporaryFile(suffix=".xml", delete=False) as tmp_file:
tmp_file.write(mjcf_model.encode())
tmp_filepath = tmp_file.name

Load the model and create a simulation

model = mujoco.MjModel.from_xml_path(tmp_filepath)
data = mujoco.MjData(model)

Create a renderer

renderer = mujoco.Renderer(model)

Function to compute the weight of the cuboid

def compute_cuboid_weight():
cuboid_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cuboid")
cuboid_mass = model.body_mass[cuboid_body_id]
gravity = np.array([0, 0, -9.81])
weight = cuboid_mass * gravity
return weight

Initialize arrays to store contact forces and weights

contact_forces = []
weights = []

Initialize list to store frames

frames = []

options = mujoco.MjvOption()
mujoco.mjv_defaultOption(options)
options.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = True

Simulation parameters

sim_duration = 10 # duration in seconds
framerate = 30 # frames per second

Step the simulation to let the cuboid settle on the ground

for _ in range(int(sim_duration / model.opt.timestep)):
mujoco.mj_step(model, data)
# Get the contact force measurement from the sensor
sensor_force = data.sensordata[int(model.sensor('contact_force').adr):int(model.sensor('contact_force').adr)+3]
contact_forces.append(sensor_force.copy())

# Compute the weight of the cuboid
cuboid_weight = compute_cuboid_weight()
weights.append(cuboid_weight.copy())

# Capture frames for the video
if len(frames) < data.time * framerate:
    renderer.update_scene(data,-1,)
    pixels = renderer.render()
    frames.append(pixels)

Convert to numpy arrays for plotting

contact_forces = np.array(contact_forces)
weights = np.array(weights)

Plot the contact force and computed weight

time = np.arange(len(contact_forces)) * model.opt.timestep
plt.figure()
plt.plot(time, contact_forces[:, 2], label='Measured Contact Force (N)')
plt.plot(time, weights[:, 2], label='Computed Weight (N)', linestyle='dashed')
plt.xlabel('Time (s)')
plt.ylabel('Force (N)')
plt.legend()
plt.title('Contact Force and Computed Weight Over Time')
plt.show()

Check if the measured contact force matches the computed weight

tolerance = 1e-2 # Tolerance for comparison
if np.allclose(sensor_force, cuboid_weight, atol=tolerance):
print("The measured contact force matches the computed weight.")
else:
print("The measured contact force does not match the computed weight.")

Show the video using mediapy

media.show_video(frames, fps=framerate)
`

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Issue with Force/Contact sensing #1962

Uh oh!

{{title}}

Uh oh!

Replies: 4 comments

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

Issue with Force/Contact sensing #1962

Uh oh!

IntrovertInDisguise Jul 18, 2024

MJCF model as a string

Function to run MuJoCo simulation and capture contact forces

Write the MJCF model to a temporary file

Load the model

Run simulation and capture contact forces

Plot the contact force over time

Show the video using mediapy

Replies: 4 comments

Uh oh!

yuvaltassa Jul 20, 2024 Maintainer

Uh oh!

IntrovertInDisguise Jul 22, 2024 Author

Uh oh!

tawjaw Jul 22, 2024

Uh oh!

IntrovertInDisguise Jul 27, 2024 Author

MJCF model as a string

Write the MJCF model to a temporary file

Load the model and create a simulation

Create a renderer

Function to compute the weight of the cuboid

Initialize arrays to store contact forces and weights

Initialize list to store frames

Simulation parameters

Step the simulation to let the cuboid settle on the ground

Convert to numpy arrays for plotting

Plot the contact force and computed weight

Check if the measured contact force matches the computed weight

Show the video using mediapy

IntrovertInDisguise
Jul 18, 2024

yuvaltassa
Jul 20, 2024
Maintainer

IntrovertInDisguise
Jul 22, 2024
Author

tawjaw
Jul 22, 2024

IntrovertInDisguise
Jul 27, 2024
Author