Skip to content

Derive a robot's collision model once and reuse it - #885

Open
iory wants to merge 1 commit into
mainfrom
collision-model
Open

Derive a robot's collision model once and reuse it#885
iory wants to merge 1 commit into
mainfrom
collision-model

Conversation

@iory

@iory iory commented Sep 7, 2026

Copy link
Copy Markdown
Owner

Every routine that avoids collisions needs the same two things from a robot: a cheap proxy geometry for each link, and the list of link pairs worth checking against each other. Until now each caller rebuilt both by hand (RobotCollisionChecker.add_link per link, then setup_self_collision_pairs), and none of them excluded the pairs that touch by design -- so a legitimate rest pose read as "in collision" on robots like the PR2 whose links overlap at rest, and the trajectory optimizer's self-collision cost was built on list-adjacency, which ignores the kinematic tree.

robot.collision_model (built on first access, persisted, rebuilt with robot.build_collision_model(...)) derives both from the robot the way MoveIt's setup assistant fills in an SRDF:

  • adjacent: parent/child pairs are dropped
  • default_pose: pairs already in contact at the default pose (all joints at zero, clipped into their limits) are dropped -- parts that touch by design
  • always: pairs colliding in >= 95% of 200 uniformly random configurations are dropped -- their geometry overlaps whatever the robot does

Contacts are tested with FCL on the exact meshes, using each mesh's convex hull only as the broadphase. That distinction matters: hulls alone reported Fetch's tucked reset_pose as self-colliding (gripper_link/torso_lift_link, shoulder_lift_link/torso_lift_link) and excluded 32 PR2 pairs at rest that the real geometry never touches; exact verification clears the false positives and keeps 18 more PR2 pairs under guard, for no measurable cost.

Two query surfaces come out of it. model.checker is a RobotCollisionChecker over sphere/capsule proxies with the pairs already set -- cheap and usable from a differentiable cost, but conservative. model.in_self_collision() / model.self_colliding_pairs() answer on the meshes. The model records how conservative the proxies are for this robot (proxy_overlap_pairs) rather than leaving it to be discovered: on Fetch the proxies already overlap for 9 of 167 checked pairs at the default pose, on PR2 for 61 of 1162, so checker.is_collision_free() says False at both robots' rest poses while in_self_collision() correctly says False. Nothing is silently dropped to make the proxy checker agree.

build_gridsdf_self_data takes the derived pairs (link_pairs=), so the grid-SDF self-collision cost can use them; the trajectory problem is not rewired in this PR. Without the optional python-fcl package the default-pose test falls back to the proxies, sampling is skipped, and a warning says so; model.method records which path ran.

Measured on this machine: Fetch derives in 0.5 s, PR2 in 0.3 s, a cache hit is ~1 ms; in_self_collision() is ~0.3 ms after the first call.

Test plan

  • pytest tests/skrobot_tests/collision_tests/ tests/skrobot_tests/planner_tests/ -- 118 passed, 26 new: a hand-built URDF where a non-adjacent pair touches at rest (must be excluded) and another only collides when pitched (must be kept), kinematic distance, pose restoration, cache round-trip and invalidation, the no-fcl warning path, gridsdf pairs, and Fetch (rest pose collision-free on the meshes, folding the forearm into the torso detected, proxies reported as conservative)
  • pytest tests/skrobot_tests/model_tests/ -- 149 passed
  • ruff check, typos (CI's v1.29.10), and the Sphinx Returns-indent check over the 7 new public callables
  • The docs snippet's numbers are the actual Fetch output

Every collision-aware routine rebuilt the same two things by hand -- a
proxy geometry per link and the link pairs worth checking -- and none of
them excluded the pairs that touch by design, so a legitimate rest pose
read as "in collision" on robots like the PR2. RobotModel.collision_model
now derives both the way MoveIt fills in an SRDF (drop parent/child pairs,
pairs in contact at the default pose, and pairs colliding in nearly every
random configuration), verifies contacts on the exact meshes rather than
convex hulls, and persists the result keyed by mesh content and joint
limits. It also says how conservative the sphere/capsule proxies are for
a given robot instead of leaving that to be discovered.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant