[FEATURE] Add analytical collisions for capsule-capsule and capsule-sphere.#2363
[FEATURE] Add analytical collisions for capsule-capsule and capsule-sphere.#2363hughperkins wants to merge 18 commits intoGenesis-Embodied-AI:mainfrom
Conversation
| (geoms_info.type[i_ga] == gs.GEOM_TYPE.CAPSULE and geoms_info.type[i_gb] == gs.GEOM_TYPE.SPHERE): | ||
| if ti.static(__debug__): | ||
| ti.atomic_add(collider_state.debug_analytical_sphere_capsule_count[i_b], 1) | ||
| # Ensure sphere is always i_ga and capsule is i_gb |
There was a problem hiding this comment.
Maybe I should move this switch inside the function?
There was a problem hiding this comment.
I'm not in favour of adding this debug counters at all. Keep them on your own branch, like I do for timers.
There was a problem hiding this comment.
I think you're adding your comment on the wrong line, and hiding my comment about the var swap.
But anyway... yeah, I mean, I was in two minds about this. And I understood you'd likely not agree with having the counters. But on the other hand, without counters, I couldn't think of a way to ensure that we were executing really gjk and analytical.
In an earlier codebase, Cursor was comparing analytical with analytical, and surprise they matched 😅 . But Cursor believed that it was comparing analytical with gjk, but it was not, because it had misunderstood the logic in narrowphase.py.
There was a problem hiding this comment.
I suppose we could make the test run pytorch profiling perhaps, and run grep (conceptually) on the resulting tracefile.
There was a problem hiding this comment.
oh we cant, because gjk and analytical are funcs, not kernels, so we can't use pytorch prfoiling for this.
There was a problem hiding this comment.
Thoughts on how we can make sure we are really calling gjk and analytical, in each case?
| s = (b * e - c * d) / denom | ||
| t = (a * e - b * d) / denom | ||
|
|
||
| # Clamp s to [0, 1] |
There was a problem hiding this comment.
This avoid this kind of comment.
|
|
||
| denom = a * c - b * b # Denominator (always >= 0) | ||
|
|
||
| # Initialize parameters |
There was a problem hiding this comment.
This avoid this kind of comment.
| a = d1.dot(d1) # Squared length of segment A | ||
| b = d1.dot(d2) # Dot product of directions | ||
| c = d2.dot(d2) # Squared length of segment B |
There was a problem hiding this comment.
This avoid this kind of comment.
| d1 = P2 - P1 # Direction vector of segment A | ||
| d2 = Q2 - Q1 # Direction vector of segment B | ||
| r = P1 - Q1 # Vector between segment origins |
There was a problem hiding this comment.
We don't use inline comments except in very few special cases.
There was a problem hiding this comment.
Could you give a llittle more background on the concerns you have with inline comments?
| # Capsules are aligned along local Z-axis by convention | ||
| local_z = ti.Vector([0.0, 0.0, 1.0], dt=gs.ti_float) | ||
|
|
||
| # Get segment axes in world space | ||
| axis_a = gu.ti_transform_by_quat(local_z, quat_a) | ||
| axis_b = gu.ti_transform_by_quat(local_z, quat_b) |
There was a problem hiding this comment.
I wonder if using the simplified formula for normalised quaternion + local_z == world_z is improve performance:
return ti.Vector(
[2.0 * q_xz + 2.0 * q_wy, -2.0 * q_wx + 2.0 * q_yz, q_ww - q_xx - q_yy + q_zz], dt=gs.ti_float
)I guess it won't.
|
|
||
| # Compute contact normal (from B to A, pointing into geom A) | ||
| if dist > EPS: | ||
| normal = -diff / dist # Negative because func_add_contact expects normal from B to A |
| temp_normal = axis_a.cross(axis_b) | ||
| if temp_normal.dot(temp_normal) < EPS: | ||
| # Axes are parallel, use any perpendicular | ||
| if ti.abs(axis_a[0]) < 0.9: |
There was a problem hiding this comment.
Why this threshold in particular?
There was a problem hiding this comment.
That's a good question 🤔
There was a problem hiding this comment.
Thoughts on what we should do here?
| if temp_normal.dot(temp_normal) < EPS: | ||
| # Axes are parallel, use any perpendicular | ||
| if ti.abs(axis_a[0]) < 0.9: | ||
| temp_normal = ti.Vector([1.0, 0.0, 0.0], dt=gs.ti_float).cross(axis_a) |
There was a problem hiding this comment.
Same. I wonder if it would be worth it to use the reduce formula. At least in this case it is very simple with no math involved, so I would recommend it here, with the original formula in comment:
temp_normal = ti.Vector([0.0, -axis_a[2], axis_a[1]], dt=gs.ti_float)
| if ti.abs(axis_a[0]) < 0.9: | ||
| temp_normal = ti.Vector([1.0, 0.0, 0.0], dt=gs.ti_float).cross(axis_a) | ||
| else: | ||
| temp_normal = ti.Vector([0.0, 1.0, 0.0], dt=gs.ti_float).cross(axis_a) |
There was a problem hiding this comment.
Same:
temp_normal = ti.Vector([axis_a[2], 0.0, -axis_a[0]], dt=gs.ti_float)
| else: | ||
| # Segments are coincident, use arbitrary perpendicular direction | ||
| # Try cross product with axis_a first | ||
| temp_normal = axis_a.cross(axis_b) |
There was a problem hiding this comment.
temp_normal is not a good variable name. Why not just normal ?
| # Segments are coincident, use arbitrary perpendicular direction | ||
| # Try cross product with axis_a first | ||
| temp_normal = axis_a.cross(axis_b) | ||
| if temp_normal.dot(temp_normal) < EPS: |
There was a problem hiding this comment.
It would be more efficient to store temp_normal.dot(temp_normal), and use it to normalise, instead of calling gu.ti_normalize that will do this job once again.
| local_z = ti.Vector([0.0, 0.0, 1.0], dt=gs.ti_float) | ||
| capsule_axis = gu.ti_transform_by_quat(local_z, capsule_quat) |
There was a problem hiding this comment.
Same. Not sure if we should use the reduce formula here.
| if ti.abs(capsule_axis[0]) < 0.9: | ||
| normal = ti.Vector([1.0, 0.0, 0.0], dt=gs.ti_float).cross(capsule_axis) | ||
| else: | ||
| normal = ti.Vector([0.0, 1.0, 0.0], dt=gs.ti_float).cross(capsule_axis) |
There was a problem hiding this comment.
Same. Using the reduce formula here sounds appropriate.
| ), | ||
| ) | ||
|
|
||
| with tempfile.TemporaryDirectory() as tmpdir: |
There was a problem hiding this comment.
We don't use tempfile in pytest. Use the fixture tmp_path instead.
| if ( | ||
| hasattr(ti.lang._template_mapper.__builtins__, "__debug__") | ||
| and ti.lang._template_mapper.__builtins__["__debug__"] | ||
| ): | ||
| analytical_capsule_count = scene_analytical.rigid_solver.collider.collider_state.debug_analytical_capsule_count[ | ||
| 0 | ||
| ] | ||
| analytical_gjk_count = scene_analytical.rigid_solver.collider.collider_state.debug_gjk_count[0] | ||
| gjk_scene_capsule_count = scene_gjk.rigid_solver.collider.collider_state.debug_analytical_capsule_count[0] | ||
| gjk_scene_gjk_count = scene_gjk.rigid_solver.collider.collider_state.debug_gjk_count[0] | ||
|
|
||
| # Scene 1 (analytical) should use analytical path, NOT GJK | ||
| assert analytical_capsule_count > 0, ( | ||
| f"Scene 1 should have used analytical capsule path (count={analytical_capsule_count})" | ||
| ) | ||
| assert analytical_gjk_count == 0, f"Scene 1 should NOT have used GJK path (count={analytical_gjk_count})" | ||
|
|
||
| # Scene 2 (GJK) should use GJK path, NOT analytical | ||
| assert gjk_scene_gjk_count > 0, f"Scene 2 should have used GJK path (count={gjk_scene_gjk_count})" | ||
| assert gjk_scene_capsule_count == 0, ( | ||
| f"Scene 2 should NOT have used analytical capsule path (count={gjk_scene_capsule_count})" | ||
| ) |
There was a problem hiding this comment.
I'm not in favour of instructions just for the sake of unit testing.
There was a problem hiding this comment.
you mean, remove the comments from the asserts?
There was a problem hiding this comment.
when I say 'comments', I mean, the strings after the comma, at the end of each assert line.
| mjcf1 = create_capsule_mjcf("capsule1", (0, 0, 0), (0, 0, 0), 0.1, 0.25) | ||
| mjcf1_path = os.path.join(tmpdir, "capsule1.xml") | ||
| ET.ElementTree(mjcf1).write(mjcf1_path) |
There was a problem hiding this comment.
What about adding write as part of your helper? Sounds more convenient.
| # For perturbed iterations (i_detection > 0), correct contact position and normal | ||
| # This applies to ALL collision methods when multi-contact is enabled |
There was a problem hiding this comment.
No it should not apply to all methods, ie not the ones that are mujoco-compatible as mujoco is not doing this.
There was a problem hiding this comment.
good point. So I'll disable this when we are using mujoco compatible algorithm?
There was a problem hiding this comment.
(or switch out which methods it applies to, when we are using mujoco compatbiel algorithm?)
duburcqa
changed the title
| if geoms_info.type[i_ga] == gs.GEOM_TYPE.PLANE: | ||
| if geoms_info.type[i_ga] == gs.GEOM_TYPE.CAPSULE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE: |
There was a problem hiding this comment.
Beware the MacOS-specific branches for specialisation that are necessary because of taichi compiler bug:
There was a problem hiding this comment.
Note that g1 fall is running just fine on my Mac.
2 participants
Description
Add analytical collisions for capsule-capsule and capsule-sphere
Related Issue
Resolves Genesis-Embodied-AI/Genesis#
Motivation and Context
How Has This Been / Can This Be Tested?
Screenshots (if appropriate):
Checklist:
Submitting Code Changessection of CONTRIBUTING document.