tests: add few quick check for forcing modules

Author: skim0119

tests/virtual_field/behavior/runtime/test_noel_c4_behavior.py

@@ -199,6 +199,13 @@ def test_server_accepts_noel_c4_mode() -> None:
     assert len(hello_ack["payload"]["arm_ids"]) == 2
 
 
+def test_backend_step_noel_c4_does_not_raise() -> None:
+    backend = MultiArmPassThroughBackend()
+    backend.register_user("user_noel_step", character_mode="noel-c4")
+    scene_state = backend.step(1.0 / 120.0, command=None)
+    assert len(scene_state.arms) == 2
+
+
 def test_server_accepts_cathy_foraging_mode() -> None:
     server = VRWebSocketServer(
         ssl_context=None,  # type: ignore[arg-type]

tests/virtual_field/equations/runtime/test_cathy_throw_forces_equations.py

@@ -9,6 +9,7 @@
     _pull_sphere_to_point_force,
     _PullSphereToPoint,
     _SphereBoxed,
+    _TravelingContractingWave,
 )
 
 pytestmark = pytest.mark.equations
@@ -207,6 +208,40 @@ def test_sucker_actuation_force_decays_with_distance() -> None:
     )
 
 
+def test_sucker_actuation_respects_sucker_index() -> None:
+    rod = DummyRod()
+    sphere = DummySphere(
+        position=[0.03, 0.0, -0.05],
+        velocity=[0.0, 0.0, 0.0],
+        radius=0.01,
+        mass=1.0,
+    )
+    kwargs = dict(k=10.0, nu=0.0, trigger=True, capture_distance=0.2)
+    contact_all = SuckerActuationToSphere(**kwargs)
+    contact_subset = SuckerActuationToSphere(**kwargs, sucker_index=[0])
+    contact_none = SuckerActuationToSphere(**kwargs, sucker_index=[])
+
+    def clear_forces() -> None:
+        rod.external_forces[:] = 0.0
+        rod.external_torques[:] = 0.0
+        sphere.external_forces[:] = 0.0
+        sphere.external_torques[:] = 0.0
+
+    contact_all.apply_contact(rod, sphere)
+    f_all = sphere.external_forces.copy()
+    clear_forces()
+
+    contact_subset.apply_contact(rod, sphere)
+    f_subset = sphere.external_forces.copy()
+    clear_forces()
+
+    contact_none.apply_contact(rod, sphere)
+    f_empty = sphere.external_forces.copy()
+
+    assert np.allclose(f_all, f_subset)
+    assert np.allclose(f_empty, 0.0)
+
+
 def test_sucker_actuation_adds_alignment_torque() -> None:
     rod = DummyRod()
     sphere = DummySphere(
@@ -228,3 +263,51 @@ def test_sucker_actuation_adds_alignment_torque() -> None:
     assert np.linalg.norm(sphere.external_forces[:, 0]) > 0.0
     assert np.linalg.norm(sphere.external_torques[:, 0]) > 0.0
     assert np.linalg.norm(rod.external_torques[:, 0]) > 0.0
+
+
+def test_traveling_contracting_wave_also_modulates_stiffness() -> None:
+    class DummyWaveRod:
+        def __init__(self) -> None:
+            self.total_elements = 6
+            self.current_elements = 4
+            self.rest_sigma = np.zeros((3, 6), dtype=np.float64)
+            self.shear_matrix = np.zeros((3, 3, 6), dtype=np.float64)
+            self.bend_matrix = np.zeros((3, 3, 6), dtype=np.float64)
+            self.shear_matrix[0, 0, :] = 2.0
+            self.shear_matrix[1, 1, :] = 3.0
+            self.bend_matrix[0, 0, :] = 5.0
+            self.bend_matrix[1, 1, :] = 7.0
+
+    rod = DummyWaveRod()
+    event = {"count": 0}
+    wave = _TravelingContractingWave(
+        event_id=lambda: event["count"],
+        original_rest_sigma=rod.rest_sigma,
+        original_shear_matrix=rod.shear_matrix,
+        original_bend_matrix=rod.bend_matrix,
+        amplitude=-0.2,
+        stiffness_amplitude=0.5,
+        width=1.5,
+        duration=0.4,
+    )
+
+    wave.apply_forces(rod, time=0.0)
+    assert np.allclose(rod.rest_sigma[2], 0.0)
+    assert np.allclose(rod.shear_matrix[0, 0], 2.0)
+    assert np.allclose(rod.bend_matrix[0, 0], 5.0)
+
+    event["count"] = 1
+    wave.apply_forces(rod, time=0.1)
+
+    assert np.min(rod.rest_sigma[2, 2:]) < 0.0
+    assert np.max(rod.shear_matrix[0, 0, 2:]) > 2.0
+    assert np.max(rod.shear_matrix[1, 1, 2:]) > 3.0
+    assert np.max(rod.bend_matrix[0, 0, 2:]) > 5.0
+    assert np.max(rod.bend_matrix[1, 1, 2:]) > 7.0
+
+    wave.apply_forces(rod, time=1.0)
+    assert np.allclose(rod.rest_sigma[2], 0.0)
+    assert np.allclose(rod.shear_matrix[0, 0], 2.0)
+    assert np.allclose(rod.shear_matrix[1, 1], 3.0)
+    assert np.allclose(rod.bend_matrix[0, 0], 5.0)
+    assert np.allclose(rod.bend_matrix[1, 1], 7.0)

tests/virtual_field/equations/runtime/test_noel_c4_sdf_hash_equations.py

@@ -0,0 +1,193 @@
+import numpy as np
+import pytest
+
+from virtual_field.runtime.spirob_elastica.sdf_objects import (
+    SDFObstacleCylinders,
+    obstacle_cylinder_sdf_and_normal,
+)
+from virtual_field.runtime.spirob_elastica.sdf_objects_hash import (
+    CylinderSpatialHash,
+    SDFObstacleCylindersHash,
+    obstacle_cylinder_sdf_and_normal_hash,
+)
+
+pytestmark = pytest.mark.equations
+
+
+def _make_obstacles() -> tuple[np.ndarray, ...]:
+    starts = np.array(
+        [
+            [0.0, 0.0, 0.0],
+            [1.5, 0.0, 0.0],
+            [0.0, 0.0, 1.5],
+        ],
+        dtype=np.float64,
+    )
+    directions = np.array(
+        [
+            [0.0, 1.0, 0.0],
+            [0.0, 1.0, 0.0],
+            [1.0, 0.0, 0.0],
+        ],
+        dtype=np.float64,
+    )
+    lengths = np.array([1.0, 1.0, 0.75], dtype=np.float64)
+    radii = np.array([0.2, 0.2, 0.15], dtype=np.float64)
+    normals = np.array(
+        [
+            [1.0, 0.0, 0.0],
+            [1.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0],
+        ],
+        dtype=np.float64,
+    )
+    return starts, directions, lengths, radii, normals
+
+
+def test_cylinder_spatial_hash_query_returns_local_candidates() -> None:
+    starts, directions, lengths, radii, normals = _make_obstacles()
+    spatial_hash = CylinderSpatialHash(
+        starts=starts,
+        directions=directions,
+        lengths=lengths,
+        radii=radii,
+        normals=normals,
+        cell_size=0.75,
+    )
+
+    candidates = spatial_hash.query_candidates(np.array([0.1, 0.5, 0.0]))
+
+    assert 0 in candidates
+    assert 1 not in candidates
+    assert 2 not in candidates
+
+
+@pytest.mark.parametrize(
+    "point",
+    [
+        np.array([0.1, 0.5, 0.0], dtype=np.float64),
+        np.array([1.65, 0.25, 0.0], dtype=np.float64),
+        np.array([0.4, 0.1, 1.55], dtype=np.float64),
+        np.array([0.9, 0.9, 0.9], dtype=np.float64),
+    ],
+)
+def test_hash_sdf_matches_bruteforce_query(point: np.ndarray) -> None:
+    starts, directions, lengths, radii, normals = _make_obstacles()
+    spatial_hash = CylinderSpatialHash(
+        starts=starts,
+        directions=directions,
+        lengths=lengths,
+        radii=radii,
+        normals=normals,
+        cell_size=0.75,
+    )
+
+    sdf_ref, normal_ref, idx_ref = obstacle_cylinder_sdf_and_normal(
+        point, starts, directions, lengths, radii, normals
+    )
+    sdf_hash, normal_hash, idx_hash = obstacle_cylinder_sdf_and_normal_hash(
+        point,
+        starts,
+        directions,
+        lengths,
+        radii,
+        normals,
+        spatial_hash,
+    )
+
+    assert idx_hash == idx_ref
+    assert np.isclose(sdf_hash, sdf_ref)
+    assert np.allclose(normal_hash, normal_ref)
+
+
+def test_hash_force_matches_bruteforce_force_and_tip_penetration() -> None:
+    starts, directions, lengths, radii, normals = _make_obstacles()
+    positions = np.array(
+        [
+            [0.15, 0.15, 1.65],
+            [0.25, 0.50, 0.50],
+            [0.00, 0.00, 0.00],
+        ],
+        dtype=np.float64,
+    )
+    velocities = np.array(
+        [
+            [0.0, -0.1, 0.0],
+            [0.0, 0.0, 0.0],
+            [0.0, 0.0, 0.0],
+        ],
+        dtype=np.float64,
+    )
+    radius = np.array([0.05, 0.05, 0.05], dtype=np.float64)
+
+    class DummyRod:
+        def __init__(self) -> None:
+            self.position_collection = positions.copy()
+            self.velocity_collection = velocities.copy()
+            self.external_forces = np.zeros((3, positions.shape[1]), dtype=np.float64)
+            self.radius = radius.copy()
+
+    tip_state_ref = {"arm": 0.0}
+    tip_state_hash = {"arm": 0.0}
+
+    rod_ref = DummyRod()
+    rod_hash = DummyRod()
+
+    brute_force = SDFObstacleCylinders(
+        starts=starts,
+        directions=directions,
+        lengths=lengths,
+        radii=radii,
+        normals=normals,
+        tip_penetration_state=tip_state_ref,
+        tip_penetration_key="arm",
+        stiffness=1000.0,
+        damping=0.5,
+    )
+    hash_force = SDFObstacleCylindersHash(
+        starts=starts,
+        directions=directions,
+        lengths=lengths,
+        radii=radii,
+        normals=normals,
+        tip_penetration_state=tip_state_hash,
+        tip_penetration_key="arm",
+        stiffness=1000.0,
+        damping=0.5,
+        cell_size=0.75,
+    )
+
+    brute_force.apply_forces(rod_ref)
+    hash_force.apply_forces(rod_hash)
+
+    assert np.allclose(rod_hash.external_forces, rod_ref.external_forces)
+    assert tip_state_hash["arm"] == pytest.approx(tip_state_ref["arm"])
+
+
+def test_hash_force_auto_expands_query_padding_for_larger_rod_radius() -> None:
+    starts, directions, lengths, radii, normals = _make_obstacles()
+
+    class DummyRod:
+        def __init__(self) -> None:
+            self.position_collection = np.array([[0.15], [0.25], [0.0]])
+            self.velocity_collection = np.zeros((3, 1), dtype=np.float64)
+            self.external_forces = np.zeros((3, 1), dtype=np.float64)
+            self.radius = np.array([0.3], dtype=np.float64)
+
+    rod = DummyRod()
+    force = SDFObstacleCylindersHash(
+        starts=starts,
+        directions=directions,
+        lengths=lengths,
+        radii=radii,
+        normals=normals,
+        stiffness=1000.0,
+        damping=0.0,
+    )
+
+    assert force.query_padding == pytest.approx(np.max(radii))
+
+    force.apply_forces(rod)
+
+    assert force.query_padding == pytest.approx(0.3)
+    assert rod.external_forces[0, 0] > 0.0

tests/virtual_field/modules/runtime/test_mode_base_modules.py

@@ -2,6 +2,7 @@
 import pytest
 
 from virtual_field.runtime.mode_base import OctoArmSimulationBase
+from virtual_field.runtime.two_gcr_simulation import TwoGCRSimulation
 
 pytestmark = pytest.mark.modules
 
@@ -47,6 +48,41 @@ def handle_commands(self, command) -> None:  # noqa: ANN001
         _ = command
 
 
+def _make_dummy_rod(base: list[float]) -> object:
+    return type(
+        "DummyRod",
+        (),
+        {
+            "position_collection": np.array(
+                [
+                    [base[0], base[0], base[0]],
+                    [base[1], base[1], base[1]],
+                    [base[2], base[2] - 0.2, base[2] - 0.4],
+                ],
+                dtype=np.float64,
+            ),
+            "radius": np.array([0.03, 0.02], dtype=np.float64),
+            "lengths": np.array([0.2, 0.2], dtype=np.float64),
+            "director_collection": np.repeat(
+                np.eye(3, dtype=np.float64)[..., None], 2, axis=2
+            ),
+        },
+    )()
+
+
+class _TwoGCRInitRegression(TwoGCRSimulation):
+    def build_simulation(self) -> None:
+        self.wave_event_before_post_setup = self._grip_wave_event.get(
+            self.arm_ids[0], 0
+        )
+        self.simulator = object()
+        self.timestepper = _DummyStepper()
+        self.rods = {
+            arm_id: _make_dummy_rod(base)
+            for arm_id, base in self.arm_bases.items()
+        }
+
+
 def _base_position() -> tuple[float, float, float]:
     return (0.0, 1.0, -0.15)
 
@@ -117,3 +153,16 @@ def test_arm_states_use_active_rod_slices_when_available() -> None:
     assert state.radii == [0.02]
     assert state.element_lengths == [0.2]
     assert len(state.directors) == 1
+
+
+def test_two_gcr_control_state_exists_during_build_simulation() -> None:
+    simulation = _TwoGCRInitRegression(
+        user_id="user_dummy",
+        arm_ids=("left_arm", "right_arm"),
+        base_left=[-0.15, 0.2, 0.15],
+        base_right=[0.15, 0.2, 0.15],
+        dt_internal=0.1,
+    )
+
+    assert simulation.wave_event_before_post_setup == 0
+    assert simulation._grip_wave_event == {"left_arm": 0, "right_arm": 0}