irsim.lib.algorithm.contact =========================== .. py:module:: irsim.lib.algorithm.contact .. autoapi-nested-parse:: Rigid-body contact resolution for the ``contact`` collision mode. Objects that overlap after a kinematic step are separated along their contact normal. Between two passive bodies, or two driven ones, the separation is shared in inverse proportion to their masses, which is the perfectly inelastic collision of position-based dynamics. A driven object (one with kinematics) pushes a passive body without yielding to it, as a velocity-controlled robot in a physics engine does, provided its traction, ``friction * mass``, is at least the body's ground friction; a body it cannot overcome stalls it. An immovable object (static or infinite mass) stops whatever presses on it, which then slides along the surface: a robot always, a passive body only when the push leaves the friction cone of the two surfaces. Passive bodies keep the velocity a push gave them and coast to a stop under ground friction, which their kinematics model integrates. A push that misses a body's center also turns it: the contact point and normal give a torque, shared through each body's moment of inertia as in position-based rigid-body dynamics, and a spinning passive body slows under friction like a sliding one. Driven objects keep their heading, which their drive holds. The contact normal and depth come from the separating axis theorem (SAT) evaluated between convex pieces of the two shapes: circles, convex polygons, and line segments. Non-convex polygons are triangulated with a constrained Delaunay triangulation, so every shape IR-SIM supports takes part, including compound bodies, linestring walls, and grid maps. The resolution follows position-based dynamics: each overlap is a unilateral distance constraint, projected one pair at a time in Gauss-Seidel sweeps with the correction weighted by inverse mass, and the velocity of a body is recovered from its position change. Pairs are swept outward from immovable objects, and a body that was just pressed against something it cannot move only yields tangentially for the rest of the sweep, a simplified form of shock propagation, so that a chain pushed into a wall settles in one sweep and slides along it. .. rubric:: 引用 Müller, M., Heidelberger, B., Hennix, M., Ratcliff, J. (2007), *Position based dynamics.* J. Vis. Commun. Image Represent. 18(2):109. Mass-weighted constraint projection, Gauss-Seidel iteration over the constraints, and velocities derived from the position update. Macklin, M., Müller, M., Chentanez, N. (2016), *XPBD: position-based simulation of compliant constrained dynamics.* Proc. Motion in Games, p. 49. Müller, M., Macklin, M., Chentanez, N., Jeschke, S., Kim, T.-Y. (2020), *Detailed rigid body simulation with extended position based dynamics.* Comput. Graph. Forum 39(8):101. The rigid-body form of the same projection. Guendelman, E., Bridson, R., Fedkiw, R. (2003), *Nonconvex rigid bodies with stacking.* ACM Trans. Graph. 22(3):871. Shock propagation: contacts are processed outward from immovable objects and settled bodies are treated as immovable, which the anchored sweep order and the blocked directions here simplify to a single pass. Gottschalk, S., Lin, M. C., Manocha, D. (1996), *OBBTree: a hierarchical structure for rapid interference detection.* Proc. SIGGRAPH, p. 171. Ericson, C. (2005), *Real-Time Collision Detection.* Morgan Kaufmann, chapter 5. The separating axis theorem that gives the contact normal and depth between two convex pieces. Catto, E. (2005), *Iterative dynamics with temporal coherence.* Game Developers Conference; and Box2D (https://box2d.org). Sequential per-pair resolution, the linear slop left between separated bodies, and the fattened bounding boxes of the broad phase. Chew, L. P. (1989), *Constrained Delaunay triangulations.* Algorithmica 4:97, as implemented by GEOS and exposed through :func:`shapely.constrained_delaunay_triangles`, for the convex decomposition of non-convex polygons. Author: Ruihua Han assisted by claude Attributes ---------- .. autoapisummary:: irsim.lib.algorithm.contact.CONTACT_SLOP irsim.lib.algorithm.contact.REST_TOLERANCE irsim.lib.algorithm.contact.CONTACT_ITERATIONS irsim.lib.algorithm.contact.Piece irsim.lib.algorithm.contact.Mtv Classes ------- .. autoapisummary:: irsim.lib.algorithm.contact.Contact irsim.lib.algorithm.contact.ContactReport Functions --------- .. autoapisummary:: irsim.lib.algorithm.contact.geometry_pieces irsim.lib.algorithm.contact.object_pieces irsim.lib.algorithm.contact.piece_mtv irsim.lib.algorithm.contact.object_mtv irsim.lib.algorithm.contact.contact_step Module Contents --------------- .. py:data:: CONTACT_SLOP :value: 1e-06 .. py:data:: REST_TOLERANCE :value: 9.999999999999999e-06 .. py:data:: CONTACT_ITERATIONS :value: 32 .. py:data:: Piece .. py:class:: Contact One resolved contact between two objects. :ivar a: First object; ``normal`` points from ``b`` toward it. :ivar b: Second object. :ivar normal: Unit contact normal ``(2,)`` pointing from ``b`` to ``a``. :ivar depth: Penetration depth that was removed, in meters; zero or negative for a resting contact, which moved nothing. :ivar point: World-frame contact point ``(2,)``, midway through the overlap. :ivar force: Contact force along ``normal`` in newtons, the constraint impulse over the squared step time as XPBD estimates it, capped at the traction of a driven object pushing into the contact; ``0`` when the step time was not given. .. py:attribute:: a :type: Any .. py:attribute:: b :type: Any .. py:attribute:: normal :type: numpy.ndarray .. py:attribute:: depth :type: float .. py:attribute:: point :type: numpy.ndarray | None :value: None .. py:attribute:: force :type: float :value: 0.0 .. py:method:: report(obj: Any) -> ContactReport This contact as seen from ``obj``, one of its two objects. .. py:class:: ContactReport One contact of an object, seen from its own side: what a contact sensor mounted on it would report. :ivar other: The object it touched. :ivar point: World-frame contact point ``(2,)``. :ivar normal: Unit normal ``(2,)`` pointing from ``other`` toward the object, the direction it is being pushed in. :ivar depth: Overlap that was pushed apart in meters; zero or negative for a resting contact. :ivar force: Contact force along ``normal`` in newtons. .. py:attribute:: other :type: Any .. py:attribute:: point :type: numpy.ndarray .. py:attribute:: normal :type: numpy.ndarray .. py:attribute:: depth :type: float .. py:attribute:: force :type: float .. py:function:: geometry_pieces(geometry: shapely.geometry.base.BaseGeometry, bounds: tuple | None = None) -> list[Piece] Decompose a shapely geometry into convex pieces. Polygons become themselves when convex, otherwise their constrained Delaunay triangles. Lines become their segments, restricted to those whose bounding box meets ``bounds`` when given. Points contribute nothing. :param geometry: Any shapely geometry or collection. :param bounds: Optional ``(minx, miny, maxx, maxy)`` filter for line segments. :returns: Convex pieces as ``("polygon", vertices (N, 2))`` tuples. :rtype: list .. py:function:: object_pieces(obj: Any, other: Any | None = None) -> list[Piece] Convex pieces of an object at its current pose. A ``circle`` is kept exact. A ``compound`` uses its parts. A ``map`` only contributes the boundary segments near ``other`` (the object it is tested against), found through its own spatial index. :param obj: The object to decompose. :param other: The object it is in contact with, used to restrict map segments. :returns: Convex pieces. :rtype: list .. py:data:: Mtv .. py:function:: piece_mtv(a: Piece, b: Piece, tolerance: float = 0.0) -> Mtv | None Minimum translation that separates convex piece ``a`` from ``b``. :param a: Convex piece to move. :param b: Convex piece it overlaps. :param tolerance: Gap up to which separated pieces still count as a contact, reported with a negative depth. :returns: ``(normal, depth, point)`` where the unit ``normal`` points from ``b`` toward ``a``, moving ``a`` by ``normal * depth`` separates the pieces, and ``point`` is the contact point midway through the overlap; ``None`` when they do not overlap. Exactly touching pieces count as a zero-depth contact, since shapely reports them as intersecting. :rtype: tuple | None .. py:function:: object_mtv(a: Any, b: Any, cache: dict[Any, list[Piece]] | None = None) -> Mtv | None Deepest minimum translation between two objects. Every convex piece of ``a`` is tested against every piece of ``b``, and the deepest overlap wins; shallower overlaps with other pieces are resolved by later sweeps or steps. :param a: Object to move along the returned normal. :param b: Object it overlaps. :param cache: Optional pieces cache keyed by ``id(obj)``, shared across the sweeps of one resolution; map pieces depend on the partner and are never cached. :returns: ``(normal, depth, point)`` with the normal pointing from ``b`` to ``a``, or ``None`` when no pieces overlap. :rtype: tuple | None .. py:function:: contact_step(pairs: list[tuple[Any, Any]], iterations: int = CONTACT_ITERATIONS, slop: float = CONTACT_SLOP, margin: float = 0.0, step_time: float | None = None) -> list[Contact] Separate overlapping object pairs, sharing each correction by mass. One call resolves the contacts of one simulation step, in three stages: 1. Order: pairs that nothing can move are dropped, and the rest are sorted by their contact-graph distance from immovable objects (:func:`_anchored_order`), so a wall contact is handled before the robot contact behind it. 2. Sweep: Gauss-Seidel passes over the pairs. Each overlapping pair is separated along its contact normal: ``a`` moves by ``depth * w_a / (w_a + w_b)`` and ``b`` by the rest, where ``w`` is the inverse mass (zero for static or infinite-mass objects, which never move). Within a sweep, an object that was just pressed against something it cannot move only yields tangentially afterwards and its partner takes the rest, so a chain pushed into a wall settles in one sweep and a box pushed obliquely into a wall slides along it. Sweeps repeat until one moves nothing, or ``iterations`` is reached. A pair resting within :data:`REST_TOLERANCE` of each other needs no correction but still blocks: a box resting on a wall is not pushed into it and back out again within a sweep. 3. Apply: the sweeps work on translated copies of each object's convex pieces and accumulate one displacement per object, applied at the end through :py:meth:`~irsim.world.object_base.ObjectBase.apply_contact_displacement`, which also folds it into the object's velocity. Both objects of every resolved pair are marked as in contact with each other. :param pairs: Candidate ``(a, b)`` object pairs. They should include every pair that a correction could bring into contact, e.g. all pairs within one step's travel of each other, not only the pairs that intersect after the kinematic step; pairs that never overlap are skipped. :param iterations: Maximum sweeps over the pairs. :param slop: Extra gap left between separated bodies. :param margin: How far an object may be moved by the sweeps, used to fetch enough grid-map boundary around each candidate up front. :param step_time: Simulation step, needed to report contact forces and to apply restitution; without it forces stay ``0`` and nothing bounces. :returns: One entry per pair that touched, in the order of the first sweep that found it, carrying its deepest push; a positive ``depth`` means the pair was pushed apart, otherwise the two only rest against each other. :rtype: list[Contact]