Contact Sensors

Contact sensors attach to a rigid or articulation simulation view. The current GPU sensor reports contact count, contact mask, and accumulated normal impulse.

rigids = world.get_rigid_batch(obj_id=0)
contact = rigids.add_contact_sensor(body_ids=[0], name="contact")
force = rigids.add_force_sensor(body_ids=[0], name="force")

world.step()

counts = contact.contact_count
in_contact = contact.in_contact
impulse = contact.net_impulse
average_normal_force = force.force

points = contact.contact_points()
positions_w = points.position_w
normals_w = points.normal_w
normal_impulses_w = points.normal_impulse_w

ForceSensor.force is net_impulse / world.sim_dt. It is not a full six-axis wrench: tangential friction impulse and contact torque are not present.

ContactSensor.contact_points() selects the current packed PhysX contacts for that sensor without copying them off CUDA. Its variable-length outputs include environment, body_slot, the other endpoint’s body reference, position_w, normal_w, normal_impulse_w, and separation. Cartesian fields preserve the PhysX world frame; normals and normal impulses are oriented toward the selected sensor body. Use these raw values in Python/Torch for object-frame conversion, contact filtering, friction-cone approximations, and wrench-space metrics.

The packed impulse contains the solver’s normal component only. Coulomb-cone edges and primitive wrenches are deliberately not generated by the engine, because their discretization, reference frame, and filtering policy belong to the sensor or learning task.

normal_impulse_wrench_about(reference_position_w) requires the moment reference point explicitly and returns [linear impulse, angular impulse] in world axes. Pass each body/link origin if a body-origin wrench is desired, then rotate the result in Python when an object-frame representation is required.

normal_wrenches_w = points.normal_impulse_wrench_about(reference_positions_w)

The high-level GPU example keeps all sensor outputs as Torch CUDA tensors:

python ./python/examples/sim_gpu_contact_sensor.py --num-envs 8
python ./python/examples/sim_gpu_contact_sensor.py --num-envs 8 --viewer

The second command requires Linux/NVIDIA CUDA/OpenGL interop.

Complete source: sim_gpu_contact_sensor.py 
  1"""High-level PhysX GPU rigid batch with CUDA contact/force sensors.
  2
  3This example keeps the public path small:
  4
  5- create a batched ``KangSimWorld`` on CUDA,
  6- spawn two rigid views across many environments,
  7- attach ``ContactSensor`` and ``ForceSensor`` to the views,
  8- read the sensor outputs as Torch CUDA tensors,
  9- optionally draw both rigid batches through GPU ExternalBuffer visuals.
 10"""
 11
 12from __future__ import annotations
 13
 14import argparse
 15from pathlib import Path
 16
 17import kangengine as ke
 18import numpy as np
 19import torch
 20from kangengine import imgui, keys
 21
 22
 23LEFT_OBJ_ID = 0
 24RIGHT_OBJ_ID = 1
 25
 26
 27def asset_path(*parts: str) -> str:
 28    return str(Path(ke.__file__).resolve().parent / "assets" / Path(*parts))
 29
 30
 31def make_env_origins(num_envs: int, device) -> torch.Tensor:
 32    env_ids = torch.arange(num_envs, dtype=torch.float32, device=device)
 33    columns = min(4, max(1, num_envs))
 34    x = torch.remainder(env_ids, float(columns)) * 1.6
 35    y = torch.div(env_ids, columns, rounding_mode="floor") * 1.8
 36    return torch.stack((x, y, torch.zeros_like(x)), dim=1)
 37
 38
 39def make_identity_quaternions(count: int, device) -> torch.Tensor:
 40    rotations = torch.zeros((count, 4), dtype=torch.float32, device=device)
 41    rotations[:, 3] = 1.0
 42    return rotations
 43
 44
 45def env_group_colors(num_envs: int):
 46    palette = (
 47        (0.18, 0.52, 0.92, 1.0),
 48        (0.92, 0.42, 0.18, 1.0),
 49        (0.42, 0.72, 0.28, 1.0),
 50        (0.74, 0.36, 0.82, 1.0),
 51        (0.88, 0.72, 0.22, 1.0),
 52        (0.25, 0.68, 0.68, 1.0),
 53        (0.86, 0.38, 0.58, 1.0),
 54        (0.52, 0.52, 0.58, 1.0),
 55    )
 56    colors = []
 57    for env_id in range(num_envs):
 58        colors.append(list(palette[env_id % len(palette)]))
 59    return colors
 60
 61
 62def contact_asset(args) -> str:
 63    filename = "ball.xml" if args.shape == "sphere" else "box.xml"
 64    return asset_path("objects", filename)
 65
 66
 67def initial_x_offset(args) -> float:
 68    # ball.xml radius is 0.12, box.xml x half-extent is 0.18.
 69    return 0.135 if args.shape == "sphere" else 0.205
 70
 71
 72class GpuContactSensorDemo:
 73    def __init__(self, args):
 74        self.args = args
 75        self.device = torch.device(f"cuda:{args.cuda_device}")
 76        self.world = None
 77        self.peak_impulse = 0.0
 78        self.peak_force = 0.0
 79
 80    def setup(self):
 81        args = self.args
 82        physics_config = ke.physics.PhysicsConfig.z_up()
 83        physics_config.enable_contact_reports = (
 84            False  # Turn off CPU contact report callback
 85        )
 86        physics_config.restitution = 0.3
 87        self.world = ke.sim.KangSimWorld(
 88            num_envs=args.num_envs,
 89            physics_config=physics_config,
 90            sim_device=self.device,
 91            sim_dt=1.0 / 120.0,
 92            add_ground=True,
 93        )
 94        rigid_xml = contact_asset(args)
 95        rigid_data = self.world.load_mjcf(rigid_xml)
 96
 97        for env_id in range(args.num_envs):
 98            self.world.add_rigid(
 99                rigid_data,
100                env_id=env_id,
101                obj_id=LEFT_OBJ_ID,
102                name=f"left_{args.shape}",
103                density=200.0,
104            )
105            self.world.add_rigid(
106                rigid_data,
107                env_id=env_id,
108                obj_id=RIGHT_OBJ_ID,
109                name=f"right_{args.shape}",
110                density=200.0,
111            )
112
113        self.left = self.world.get_rigid_batch(obj_id=LEFT_OBJ_ID)
114        self.right = self.world.get_rigid_batch(obj_id=RIGHT_OBJ_ID)
115        self.left_contact = self.left.add_contact_sensor(
116            body_ids=[0], name="left_contact"
117        )
118        self.left_force = self.left.add_force_sensor(body_ids=[0], name="left_force")
119        self.right_contact = self.right.add_contact_sensor(
120            body_ids=[0], name="right_contact"
121        )
122
123        self._build_reset_tensors()
124        self.world.init_gpu_system(cuda_device_id=args.cuda_device)
125        self.reset()
126        return self
127
128    def _build_reset_tensors(self):
129        args = self.args
130        origins = make_env_origins(args.num_envs, self.device)
131        self.rotations = make_identity_quaternions(args.num_envs, self.device)
132        self.zeros3 = torch.zeros(
133            (args.num_envs, 3), dtype=torch.float32, device=self.device
134        )
135
136        offset = initial_x_offset(self.args)
137        self.left_pos = origins + torch.tensor(
138            [-offset, 0.0, 1.0], dtype=torch.float32, device=self.device
139        )
140        self.right_pos = origins + torch.tensor(
141            [offset, 0.0, 1.0], dtype=torch.float32, device=self.device
142        )
143        self.left_vel = self.zeros3.clone()
144        self.right_vel = self.zeros3.clone()
145        self.left_vel[:, 0] = float(self.args.speed)
146        self.right_vel[:, 0] = -float(self.args.speed)
147
148    def reset(self):
149        self.left.set_root_state(
150            None,
151            self.left_pos,
152            self.rotations,
153            linear_velocity=self.left_vel,
154            angular_velocity=self.zeros3,
155        )
156        self.right.set_root_state(
157            None,
158            self.right_pos,
159            self.rotations,
160            linear_velocity=self.right_vel,
161            angular_velocity=self.zeros3,
162        )
163        self.world.step(substeps=0, refresh=False, apply_commands=False)
164        self.peak_impulse = 0.0
165        self.peak_force = 0.0
166
167    def step(self, substeps: int):
168        self.world.step(substeps=substeps, refresh=False)
169        self._update_peak_metrics()
170        return self.left_contact.data
171
172    def _current_metrics(self):
173        impulse = self.left_contact.net_impulse[:, 0]
174        force = self.left_force.force[:, 0]
175        max_impulse = float(torch.linalg.vector_norm(impulse, dim=1).max().item())
176        max_force = float(torch.linalg.vector_norm(force, dim=1).max().item())
177        return max_impulse, max_force
178
179    def _update_peak_metrics(self):
180        max_impulse, max_force = self._current_metrics()
181        self.peak_impulse = max(self.peak_impulse, max_impulse)
182        self.peak_force = max(self.peak_force, max_force)
183        return max_impulse, max_force
184
185    def report(self, step: int):
186        torch.cuda.synchronize(self.device)
187        counts = self.left_contact.contact_count[:, 0]
188        hit_count = int(torch.count_nonzero(counts).item())
189        max_impulse, max_force = self._current_metrics()
190        print(
191            f"step {step:04d} | contacts={hit_count}/{self.args.num_envs} "
192            f"impulse={max_impulse:.6f} force={max_force:.2f} "
193            f"peak_impulse={self.peak_impulse:.6f} peak_force={self.peak_force:.2f}"
194        )
195
196    def release(self):
197        if self.world is not None:
198            self.world.release()
199            self.world = None
200
201
202def run_headless(args):
203    demo = GpuContactSensorDemo(args).setup()
204    try:
205        print("KangSimWorld high-level GPU contact sensor example")
206        print(f"  envs       : {args.num_envs}")
207        print(f"  device     : cuda:{args.cuda_device}")
208        print("  tensors    : ContactSensor/ForceSensor outputs stay on CUDA")
209        for step in range(args.steps):
210            if args.reset_every and step and step % args.reset_every == 0:
211                demo.reset()
212                demo.report(step)
213                continue
214            demo.step(args.substeps)
215            if step % args.report_every == 0 or step == args.steps - 1:
216                demo.report(step)
217    finally:
218        demo.release()
219
220
221class GpuContactSensorViewer(ke.App):
222    def __init__(self, args):
223        super().__init__()
224        self.args = args
225
226    def setup(self):
227        self._skip_fixed_updates_this_frame = False
228        self.show_contact_debug = True
229        self.contact_marker_view = None
230        self.force_arrow_view = None
231        self.left_sensor_marker_view = None
232        self.right_sensor_marker_view = None
233        self.left_sensor_normal_view = None
234        self.right_sensor_normal_view = None
235        self.left_sensor_point_count = 0
236        self.right_sensor_point_count = 0
237        self.demo = GpuContactSensorDemo(self.args).setup()
238        self.timing = self.configure_timing(
239            ke.SimulationTimingConfig.from_dt(
240                physics_dt=self.demo.world.sim_dt,
241                fixed_dt=self.demo.world.sim_dt * self.args.substeps,
242                render_hz=60.0,
243            )
244        )
245        self.set_simulation_hotkeys_enabled(True)
246        self.standard_materials = self.create_standard_materials()
247        self.add_ground(scale=16.0, material=self.standard_materials.ground)
248        self.set_camera_view([3.5, -5.5, 3.4], [2.0, 1.2, 0.8])
249
250        self.visual = ke.visual.sim.SimWorldVisualizer(self, self.demo.world)
251        rigid_xml = contact_asset(self.args)
252        group_colors = env_group_colors(self.args.num_envs)
253        self.visual.add(
254            self.demo.left,
255            rigid_xml,
256            path="/gpu_contact/left",
257            material=self.standard_materials.common,
258            color=group_colors,
259        )
260        self.visual.add(
261            self.demo.right,
262            rigid_xml,
263            path="/gpu_contact/right",
264            material=self.standard_materials.common,
265            color=group_colors,
266        )
267        self.check_error()
268
269    def pre_update(self):
270        if self.was_key_pressed(keys.C):
271            self.show_contact_debug = not self.show_contact_debug
272        if self.was_key_pressed(keys.R):
273            self.demo.reset()
274            self._skip_fixed_updates_this_frame = True
275
276    def fixed_update(self, fixed_dt):
277        if not self._skip_fixed_updates_this_frame:
278            self.demo.step(self.args.substeps)
279
280    def pre_render(self):
281        self.visual.sync()
282        self._update_contact_debug()
283        self._skip_fixed_updates_this_frame = False
284        self.check_error()
285
286    def _clear_contact_debug(self):
287        empty3 = np.zeros((0, 3), dtype=np.float32)
288        empty4 = np.zeros((0, 4), dtype=np.float32)
289        if self.contact_marker_view is not None:
290            self.contact_marker_view.update_lines(empty3, empty3, empty4)
291        if self.force_arrow_view is not None:
292            self.force_arrow_view.update_arrows(empty3, empty3, empty4)
293        for view in (self.left_sensor_marker_view, self.right_sensor_marker_view):
294            if view is not None:
295                view.update_lines(empty3, empty3, empty4)
296        for view in (self.left_sensor_normal_view, self.right_sensor_normal_view):
297            if view is not None:
298                view.update_arrows(empty3, empty3, empty4)
299        self.left_sensor_point_count = 0
300        self.right_sensor_point_count = 0
301
302    def _update_contact_debug(self):
303        if not self.show_contact_debug:
304            self._clear_contact_debug()
305            return
306
307        points = self._contact_points_cpu()
308        if points.size == 0:
309            self._clear_contact_debug()
310            return
311        # 0:3  = contact position xyz
312        # 3:6  = contact normal xyz
313        # 6:9  = normal impulse vector xyz
314        # 9    = separation
315        positions = points[:, 0:3]
316        impulses = points[:, 6:9]
317        self._update_contact_markers(positions)
318        self._update_force_arrows(positions, impulses)
319
320        left_contacts = self.demo.left_contact.contact_points()
321        right_contacts = self.demo.right_contact.contact_points()
322        left_positions, left_normals = self._sensor_contacts_cpu(left_contacts)
323        right_positions, right_normals = self._sensor_contacts_cpu(right_contacts)
324        self.left_sensor_point_count = int(left_positions.shape[0])
325        self.right_sensor_point_count = int(right_positions.shape[0])
326        self._update_sensor_contacts(
327            "left", left_positions, left_normals, (0.15, 0.45, 1.0, 1.0)
328        )
329        self._update_sensor_contacts(
330            "right", right_positions, right_normals, (1.0, 0.25, 0.12, 1.0)
331        )
332
333    def _contact_points_cpu(self):
334        gpu_system = self.demo.world.gpu_system
335        count_tensor = torch.as_tensor(
336            gpu_system.contact_point_count(), device=self.demo.device
337        )
338        count = int(count_tensor[0].item())
339        if count <= 0:
340            return np.zeros((0, 10), dtype=np.float32)
341        points = torch.as_tensor(gpu_system.contact_points(), device=self.demo.device)
342        count = min(count, int(points.shape[0]), int(self.args.max_debug_contacts))
343        return points[:count].cpu().numpy().astype(np.float32, copy=False)
344
345    def _sensor_contacts_cpu(self, contacts):
346        count = min(int(contacts.count), int(self.args.max_debug_contacts))
347        if count <= 0:
348            empty = np.zeros((0, 3), dtype=np.float32)
349            return empty, empty
350        positions = (
351            contacts.position_w[:count].detach().cpu().numpy().astype(np.float32)
352        )
353        normals = contacts.normal_w[:count].detach().cpu().numpy().astype(np.float32)
354        return positions, normals
355
356    def _update_sensor_contacts(self, side, positions, normals, color):
357        marker_view_name = f"{side}_sensor_marker_view"
358        normal_view_name = f"{side}_sensor_normal_view"
359        marker_view = getattr(self, marker_view_name)
360        normal_view = getattr(self, normal_view_name)
361        colors = np.repeat(
362            np.asarray([color], dtype=np.float32), positions.shape[0], axis=0
363        )
364
365        # Use a side-specific short axis so coincident left/right positions are
366        # still distinguishable: left is X-shaped, right is Y-shaped.
367        half = float(self.args.contact_marker_size) * 0.7
368        marker_axis = 0 if side == "left" else 1
369        offset = np.zeros((1, 3), dtype=np.float32)
370        offset[0, marker_axis] = half
371        starts = positions - offset
372        ends = positions + offset
373        if marker_view is None:
374            marker_view = self.scene.log_lines(
375                f"/debug/{side}_sensor_contact_points",
376                self.standard_materials.common,
377                starts,
378                ends,
379                colors,
380                0.012,
381                8,
382            )
383            setattr(self, marker_view_name, marker_view)
384        else:
385            marker_view.update_lines(starts, ends, colors)
386
387        normal_ends = positions + normals * float(self.args.contact_normal_scale)
388        if normal_view is None:
389            normal_view = self.scene.log_arrows(
390                f"/debug/{side}_sensor_contact_normals",
391                self.standard_materials.common,
392                positions,
393                normal_ends,
394                colors,
395                0.012,
396                10,
397            )
398            setattr(self, normal_view_name, normal_view)
399        else:
400            normal_view.update_arrows(positions, normal_ends, colors)
401
402    def _update_contact_markers(self, positions):
403        half = float(self.args.contact_marker_size) * 0.5
404        offsets = np.array(
405            (
406                (half, 0.0, 0.0),
407                (0.0, half, 0.0),
408                (0.0, 0.0, half),
409            ),
410            dtype=np.float32,
411        )
412        starts = []
413        ends = []
414        for pos in positions:
415            for offset in offsets:
416                starts.append(pos - offset)
417                ends.append(pos + offset)
418        starts = np.asarray(starts, dtype=np.float32)
419        ends = np.asarray(ends, dtype=np.float32)
420        colors = np.repeat(
421            np.array([[0.05, 0.95, 1.0, 1.0]], dtype=np.float32),
422            starts.shape[0],
423            axis=0,
424        )
425        if self.contact_marker_view is None:
426            self.contact_marker_view = self.scene.log_lines(
427                "/debug/gpu_contact_points",
428                self.standard_materials.common,
429                starts,
430                ends,
431                colors,
432                0.006,
433                8,
434            )
435        else:
436            self.contact_marker_view.update_lines(starts, ends, colors)
437
438    def _update_force_arrows(self, positions, impulses):
439        dt = max(float(self.demo.world.sim_dt), 1e-8)
440        forces = impulses / dt
441        norms = np.linalg.norm(forces, axis=1)
442        active = norms > float(self.args.force_threshold)
443        if not np.any(active):
444            empty3 = np.zeros((0, 3), dtype=np.float32)
445            empty4 = np.zeros((0, 4), dtype=np.float32)
446            if self.force_arrow_view is not None:
447                self.force_arrow_view.update_arrows(empty3, empty3, empty4)
448            return
449
450        starts = positions[active].astype(np.float32, copy=False)
451        ends = (starts + forces[active] * float(self.args.force_arrow_scale)).astype(
452            np.float32, copy=False
453        )
454        colors = np.repeat(
455            np.array([[1.0, 0.86, 0.05, 1.0]], dtype=np.float32),
456            starts.shape[0],
457            axis=0,
458        )
459        if self.force_arrow_view is None:
460            self.force_arrow_view = self.scene.log_arrows(
461                "/debug/gpu_contact_forces",
462                self.standard_materials.common,
463                starts,
464                ends,
465                colors,
466                0.018,
467                12,
468            )
469        else:
470            self.force_arrow_view.update_arrows(starts, ends, colors)
471
472    def render(self):
473        counts = self.demo.left_contact.contact_count[:, 0]
474        force = self.demo.left_force.force[:, 0]
475        hit_count = int(torch.count_nonzero(counts).item())
476        max_force = float(torch.linalg.vector_norm(force, dim=1).max().item())
477
478        imgui.begin("GPU Contact Sensor")
479        state = "paused" if self.is_simulation_paused() else "running"
480        imgui.text(f"State: {state}")
481        imgui.text(f"Envs: {self.args.num_envs}")
482        imgui.text(f"Contacts: {hit_count}/{self.args.num_envs}")
483        imgui.text(f"Max normal force: {max_force:.2f}")
484        imgui.text(f"Peak normal force: {self.demo.peak_force:.2f}")
485        imgui.text(f"Contact debug: {'on' if self.show_contact_debug else 'off'}")
486        imgui.text("Raw contacts: cyan crosses / yellow force")
487        imgui.text(f"Left sensor: blue ({self.left_sensor_point_count} points)")
488        imgui.text(f"Right sensor: red ({self.right_sensor_point_count} points)")
489        imgui.separator()
490        imgui.text(
491            "Enter: play/pause    Space: pause/step    R: reset    C: contact debug"
492        )
493        imgui.end()
494
495    def cleanup(self):
496        if hasattr(self, "visual"):
497            self.visual.release()
498        if hasattr(self, "demo"):
499            self.demo.release()
500
501
502def parse_args():
503    parser = argparse.ArgumentParser(description=__doc__)
504    parser.add_argument("--num-envs", type=int, default=8)
505    parser.add_argument("--steps", type=int, default=180)
506    parser.add_argument("--substeps", type=int, default=1)
507    parser.add_argument("--report-every", type=int, default=30)
508    parser.add_argument("--reset-every", type=int, default=0)
509    parser.add_argument("--shape", choices=("box", "sphere"), default="box")
510    parser.add_argument("--speed", type=float, default=1.5)
511    parser.add_argument("--max-debug-contacts", type=int, default=128)
512    parser.add_argument("--contact-marker-size", type=float, default=0.08)
513    parser.add_argument("--contact-normal-scale", type=float, default=0.18)
514    parser.add_argument("--force-arrow-scale", type=float, default=0.001)
515    parser.add_argument("--force-threshold", type=float, default=1e-4)
516    parser.add_argument("--cuda-device", type=int, default=0)
517    parser.add_argument("--viewer", action="store_true")
518    parser.add_argument("--width", type=int, default=1280)
519    parser.add_argument("--height", type=int, default=720)
520    args = parser.parse_args()
521
522    if args.num_envs < 1:
523        parser.error("--num-envs must be positive")
524    if args.steps < 1:
525        parser.error("--steps must be positive")
526    if args.substeps < 1:
527        parser.error("--substeps must be positive")
528    if args.report_every < 1:
529        parser.error("--report-every must be positive")
530    if args.reset_every < 0:
531        parser.error("--reset-every must be non-negative")
532    if args.speed <= 0.0:
533        parser.error("--speed must be positive")
534    if args.max_debug_contacts < 1:
535        parser.error("--max-debug-contacts must be positive")
536    if args.contact_marker_size <= 0.0:
537        parser.error("--contact-marker-size must be positive")
538    if args.contact_normal_scale <= 0.0:
539        parser.error("--contact-normal-scale must be positive")
540    if args.force_arrow_scale < 0.0:
541        parser.error("--force-arrow-scale must be non-negative")
542    if args.force_threshold < 0.0:
543        parser.error("--force-threshold must be non-negative")
544    return args
545
546
547def main():
548    args = parse_args()
549    if args.viewer:
550        app = GpuContactSensorViewer(args)
551        app.initialize(args.width, args.height, False, ke.UpAxis.Z)
552        app.start()
553    else:
554        run_headless(args)
555
556
557if __name__ == "__main__":
558    main()

GPU contact points and force arrows