[Bug Report] Unable to get correct force data by contact sensor. #1238
Description
I write a simple example, but can not get correct force data by contact sensor.
Describe the bug
I set a ground plane and two cube. The first cube has size as (1.0, 1.0, 1.0) and init position as (0,0,1.5). The second cube has size as (0.8, 0.8, 0.8) and init position as (0,0,3.0). Then I let the simulation run.
Finally, when they all landed, the force to first cube is wrong.
Steps to
# %%
from omni.isaac.lab.app import AppLauncher
app_launcher = AppLauncher()
simulation = app_launcher.app
# %%
import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.assets import AssetBaseCfg, RigidObjectCfg
from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg
from omni.isaac.lab.utils import configclass
from omni.isaac.lab.sensors import ContactSensorCfg
# %%
@configclass
class SceneConfig(InteractiveSceneCfg):
light = AssetBaseCfg(prim_path="/World/Light",
spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)))
ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane",
spawn=sim_utils.GroundPlaneCfg())
cube1 = AssetBaseCfg(prim_path="/World/cube1",
init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 1.5)),
# init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)),
spawn=sim_utils.MeshCuboidCfg(size=(1.0, 1.0, 1.0),
physics_material=sim_utils.materials.RigidBodyMaterialCfg(),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False,
max_depenetration_velocity=5.0),
collision_props=sim_utils.CollisionPropertiesCfg(),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 0.0))))
cube2 = AssetBaseCfg(prim_path="/World/cube2",
init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 3.0)),
# init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 1.25)),
spawn=sim_utils.MeshCuboidCfg(size=(0.8, 0.8, 0.8),
physics_material=sim_utils.materials.RigidBodyMaterialCfg(),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(disable_gravity=False,
max_depenetration_velocity=5.0),
collision_props=sim_utils.CollisionPropertiesCfg(),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.75, 0.75, 0.75))))
sensor1 = ContactSensorCfg(prim_path="/World/cube1",
update_period=0.0,
history_length=6,
track_pose=True,
debug_vis=True,
filter_prim_paths_expr=["/World/cube2"])
sensor11 = ContactSensorCfg(prim_path="/World/cube1",
update_period=0.0,
history_length=6,
track_pose=True,
debug_vis=True,
filter_prim_paths_expr=["/World/defaultGroundPlane/GroundPlane/CollisionPlane"])
sensor2 = ContactSensorCfg(prim_path="/World/cube2",
update_period=0.0,
history_length=6,
track_pose=True,
debug_vis=True,
filter_prim_paths_expr=["/World/cube1"])
# %%
sim_cfg = sim_utils.SimulationCfg()
sim = sim_utils.SimulationContext(sim_cfg)
sim.set_simulation_dt(1.0/120.0)
scene_cfg = SceneConfig(num_envs=1, env_spacing=2.0)
scene = InteractiveScene(scene_cfg)
sim.reset()
# %%
def output_data(data, name):
data = [round(f, 3) for f in data.tolist()]
print(f'{name}: {data}')
# %%
sim_dt = sim.get_physics_dt()
sensor1 = scene["sensor1"]
sensor11 = scene["sensor11"]
sensor2 = scene["sensor2"]
mass1 = sensor1.body_physx_view.get_masses()
mass2 = sensor2.body_physx_view.get_masses()
print(f'mass1: {mass1}')
print(f'mass2: {mass2}')
for i in range(100):
sim.step()
scene.update(sim_dt)
force1 = sensor1.data.net_forces_w[0][0]
force2 = sensor2.data.net_forces_w[0][0]
filted_force1 = sensor1.data.force_matrix_w[0][0][0]
filted_force11 = sensor11.data.force_matrix_w[0][0][0]
pos1 = sensor1.data.pos_w[0][0]
pos2 = sensor2.data.pos_w[0][0]
quat1 = sensor1.data.quat_w[0][0]
quat2 = sensor2.data.quat_w[0][0]
print('-'*64)
print(f'step {i+1}')
print(' -- '*16)
output_data(force1, 'force1')
output_data(force2, 'force2')
output_data(pos1, 'pos1')
output_data(pos2, 'pos2')
output_data(quat1, 'quat1')
output_data(quat2, 'quat2')
output_data(filted_force1, 'cube2->cube1')
output_data(filted_force11, 'ground->cube1')
print('-'*64)
When the first cube landed, the force is corrct as:
force1: [0.001, 0.0, 9804.056]
force2: [0.0, 0.0, 0.0]
pos1: [-0.0, -0.0, 0.5]
pos2: [0.0, 0.0, 1.794]
quat1: [1.0, 0.0, 0.0, 0.0]
quat2: [1.0, 0.0, 0.0, 0.0]
cube2->cube1: [0.0, 0.0, 0.0]
ground->cube1: [0.001, 0.0, 9804.056]
When the first cube landed, the force is wrong as:
force1: [-0.004, -0.002, 1227.905]
force2: [0.005, 0.002, 5027.711]
pos1: [-0.0, -0.0, 0.5]
pos2: [-0.0, -0.0, 1.4]
quat1: [1.0, -0.0, 0.0, 0.0]
quat2: [1.0, -0.0, 0.0, -0.0]
cube2->cube1: [-0.005, -0.002, -5027.711]
ground->cube1: [0.001, 0.0, 6255.616]
To cube1, the gravity is 10000*9.8=-9800, force from cube2 is -5027.711, force from ground is 6255.616, the force of cube1 is serious imbalance, right?
System Info
Describe the characteristic of your environment:
- Commit: 2804ff3
- Isaac Sim Version: 4.2.9
- OS: Ubuntu 22.04
- GPU: RTX 2080Ti
- CUDA: 12.1
- GPU Driver: 535.183.01
Additional context
Add any other context about the problem here.
Checklist
- I have checked that there is no similar issue in the repo (required)
- I have checked that the issue is not in running Isaac Sim itself and is related to the repo
Acceptance Criteria
Add the criteria for which this task is considered done. If not known at issue creation time, you can add this once the issue is assigned.
- Criteria 1
- Criteria 2