Add activation by event

app/mcts_run_setup.py

@@ -11,6 +11,7 @@
 from rostok.criterion.simulation_flags import (EventContactBuilder, EventContactTimeOutBuilder,
                                                EventFlyingApartBuilder, EventGraspBuilder,
                                                EventSlipOutBuilder, EventStopExternalForceBuilder)
+from rostok.criterion.simulation_flags import EventContact
 from rostok.simulation_chrono.simulation_scenario import GraspScenario
 from rostok.trajectory_optimizer.control_optimizer import BruteForceOptimisation1D, ConstTorqueOptiVar, GlobalOptimisationEachSim, TendonForceOptiVar
 from rostok.utils.numeric_utils import Offset
@@ -92,7 +93,8 @@ def config_independent_torque(grasp_object_blueprint):
 
 def config_tendon(grasp_object_blueprint):
     obj_forces = []
-    obj_forces.append(f_ext.RandomForces(1e6, 100, 20))
+    obj_forces.append(f_ext.RandomForces(1e6, 100, 0))
+    obj_forces[-1].set_activation_by_event(EventContact)
     obj_forces.append(f_ext.NullGravity(0))
     obj_forces = f_ext.ExternalForces(obj_forces)
     simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p, obj_external_forces=obj_forces)

rostok/control_chrono/external_force.py

@@ -1,10 +1,12 @@
 from abc import ABC, abstractmethod
 from dataclasses import dataclass
 import numpy as np
-from typing import Any, Callable, List
+from typing import Any, Callable, List, Optional
 
 import pychrono.core as chrono
 
+from rostok.criterion.simulation_flags import SimulationSingleEvent
+
 
 def random_3d_vector(amp):
     """Sample random 3d vector with given amplitude (uniform distribution on sphere)
@@ -73,10 +75,24 @@ def __init__(self,
         self.name = name
         self.pos = pos
         self.start_time = start_time
+        self.start_event = None
 
     @abstractmethod
-    def calculate_spatial_force(self, time, data) -> np.ndarray:
+    def calculate_spatial_force(self, time, data, events) -> np.ndarray:
         return np.zeros(6)
+
+    def set_activation_by_event(self, event: type[SimulationSingleEvent], use_time: bool = False):
+        self.start_event = event
+        self.start_time = self.start_time if use_time else np.inf
+
+    def check_activation_force(self, time, events) -> bool:
+        time_activation = time >= self.start_time
+        event_activation = False
+        for ev in events:
+            if self.start_event is not None and isinstance(ev, self.start_event):
+                event_activation = ev.state
+
+        return time_activation or event_activation
 
     def enable_data_dump(self, path):
         self.path = path
@@ -92,7 +108,7 @@ class ForceChronoWrapper():
     which determines the force and torque at time t.
     """
 
-    def __init__(self, force: ABCForceCalculator, is_local: bool = False) -> None:
+    def __init__(self, force: ABCForceCalculator, events: Optional[list[SimulationSingleEvent]] = None, is_local: bool = False) -> None:
         self.path = None
         self.force = force
 
@@ -116,11 +132,12 @@ def __init__(self, force: ABCForceCalculator, is_local: bool = False) -> None:
         self.torque_maker_chrono.SetNameString(name + "_torque")
         self.is_bound = False
         self.is_local = is_local
+        self.events = events
         self.setup_makers()
 
     def get_force_torque(self, time: float, data) -> ForceTorque:
         impact = ForceTorque()
-        spatial_force = self.force.calculate_spatial_force(time, data)
+        spatial_force = self.force.calculate_spatial_force(time, data, self.events)
         impact.force = tuple(spatial_force[3:])
         impact.torque = tuple(spatial_force[:3])
         return impact
@@ -188,7 +205,7 @@ def __init__(self,
         super().__init__(name, start_time, pos)
         self.callback = callback
 
-    def calculate_spatial_force(self, time, data) -> np.ndarray:
+    def calculate_spatial_force(self, time, data, events) -> np.ndarray:
         return self.callback(time, data)
 
 
@@ -213,9 +230,9 @@ def __init__(self,
         self.amp_offset = amp_offset
         self.freq = freq
 
-    def calculate_spatial_force(self, time, data) -> np.ndarray:
+    def calculate_spatial_force(self, time, data, events) -> np.ndarray:
         spatial_force = np.zeros(6)
-        if time >= self.start_time:
+        if self.check_activation_force(time, events):
             spatial_force[4] = self.amp * np.sin(self.freq *
                                                  (time - self.start_time)) + self.amp_offset
         return spatial_force
@@ -232,9 +249,9 @@ def __init__(self, start_time: float = 0.0) -> None:
         """
         super().__init__(name="null_gravity_force", start_time=start_time, pos=np.zeros(3))
 
-    def calculate_spatial_force(self, time: float, data) -> np.ndarray:
+    def calculate_spatial_force(self, time: float, data, events) -> np.ndarray:
         spatial_force = np.zeros(6)
-        if time >= self.start_time:
+        if self.check_activation_force(time, events):
             mass = data.body_map_ordered[0].body.GetMass()
             g = data.grav_acc
             spatial_force[3:] = -mass * g
@@ -266,9 +283,9 @@ def __init__(self,
         self.counter = 0
         self.spatial_force = np.zeros(6)
 
-    def calculate_spatial_force(self, time: float, data) -> np.ndarray:
+    def calculate_spatial_force(self, time: float, data, events) -> np.ndarray:
 
-        if time >= self.start_time:
+        if self.check_activation_force(time, events):
             if self.counter % self.width_step == 0:
                 if self.dim == '2d':
                     self.spatial_force[3:] = random_plane_vector(self.amp, *self.angle)
@@ -303,9 +320,9 @@ def __init__(self,
         self.angle: float = 0.0
         self.angle_step: float = angle_step
 
-    def calculate_spatial_force(self, time: float, data) -> np.ndarray:
+    def calculate_spatial_force(self, time: float, data, events) -> np.ndarray:
         spatial_force = np.zeros(6)
-        if time >= self.start_time:
+        if self.check_activation_force(time, events):
             if self.counter % self.width_step == 0:
                 self.angle += self.angle_step
             self.counter += 1
@@ -337,12 +354,12 @@ def add_force(self, force: ABCForceCalculator):
         else:
             self.force_controller = [self.force_controller, force]
 
-    def calculate_spatial_force(self, time: float, data) -> ForceTorque:
+    def calculate_spatial_force(self, time: float, data, events) -> ForceTorque:
         if isinstance(self.force_controller, list):
             v_forces = np.zeros(6)
             for controller in self.force_controller:
-                v_forces += np.array(controller.calculate_spatial_force(time, data))
+                v_forces += np.array(controller.calculate_spatial_force(time, data, events))
                 self.data_forces.append(v_forces)
             return v_forces
         else:
-            return self.force_controller.calculate_spatial_force(time, data)
+            return self.force_controller.calculate_spatial_force(time, data, events)

rostok/control_chrono/tendon_controller.py

@@ -24,7 +24,7 @@ class ForceType(Enum):
 
 class PulleyForce(ABCForceCalculator):
 
-    def calculate_spatial_force(self, time: float, data) -> np.ndarray:
+    def calculate_spatial_force(self, time: float, data, events) -> np.ndarray:
         spatial_force = np.zeros(6)
         pre_point = data[0]
         point = data[1]
@@ -43,7 +43,7 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray:
 
 class TipForce(ABCForceCalculator):
 
-    def calculate_spatial_force(self, time: float, data) -> np.ndarray:
+    def calculate_spatial_force(self, time: float, data, events) -> np.ndarray:
         spatial_force = np.zeros(6)
         pre_point = data[0]
         point = data[1]

rostok/simulation_chrono/simulation_scenario.py

@@ -93,7 +93,7 @@ def run_simulation(self,
         set_covering_ellipsoid_based_position(grasp_object,
                                               reference_point=chrono.ChVectorD(0, 0.1, 0))
         if self.obj_external_forces:
-            chrono_forces = ForceChronoWrapper(deepcopy(self.obj_external_forces))
+            chrono_forces = ForceChronoWrapper(deepcopy(self.obj_external_forces), event_list)
         else:
             chrono_forces = None
         simulation.env_creator.add_object(grasp_object,