Derive a robot's collision model once and reuse it - #885
Open
iory wants to merge 1 commit into
Open
Conversation
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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_linkper link, thensetup_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 withrobot.build_collision_model(...)) derives both from the robot the way MoveIt's setup assistant fills in an SRDF: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_poseas 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.checkeris aRobotCollisionCheckerover 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, sochecker.is_collision_free()says False at both robots' rest poses whilein_self_collision()correctly says False. Nothing is silently dropped to make the proxy checker agree.build_gridsdf_self_datatakes 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 optionalpython-fclpackage the default-pose test falls back to the proxies, sampling is skipped, and a warning says so;model.methodrecords 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 passedruff check,typos(CI's v1.29.10), and the Sphinx Returns-indent check over the 7 new public callables