add contact checker

rostok/block_builder_chrono/blocks_utils.py

@@ -73,8 +73,7 @@ def evaluate(self, time, rest_angle, angle, vel, link):
         """
         torque = 0
 
-        if self.spring_coef > 10**-5:
-            torque = -self.spring_coef * (angle - rest_angle) - self.damping_coef * vel
+        torque = -self.spring_coef * (angle - rest_angle) - self.damping_coef * vel
             # if angle <= 0:
             #     torque = -self.spring_coef * (angle - rest_angle) - self.damping_coef * vel
             # else:

rostok/criterion/simulation_flags.py

@@ -115,10 +115,12 @@ def find_event(self, event_list: list[SimulationSingleEvent]):
             if isinstance(event, self.even_class):
                 return event
         return None
+
     @abstractmethod
     def build_event(self, event_list):
         pass
 
+
 class EventContactBuilder(EventBuilder):
     def __init__(self, take_from_body: bool = False) -> None:
         super().__init__(event_class=EventContact)
@@ -482,31 +484,57 @@ def build_event(self, event_list) -> EventStopExternalForce:
             raise Exception(
                 'Event requires another event prebuilt: EventStopExternalForce <- EventGrasp')
         return event_list.append(EventStopExternalForce(force_test_time=self.force_test_time, grasp_event=grasp_event))
-    
+
 
 class EventBodyTooLow(SimulationSingleEvent):
     def __init__(self, ref_height: float):
         super().__init__()
         self.ref_height = ref_height
-    
+
     def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor):
         robot_position = robot_data.get_body_trajectory_point()
         # it works only with current rule set, where the palm/flat always has the smallest index among the bodies
         main_body_pos = list(robot_position.items())[0][1]
 
-        if main_body_pos[1]<self.ref_height:
+        if main_body_pos[1] < self.ref_height:
             return EventCommands.STOP
 
         return EventCommands.CONTINUE
-
+
+
+class EventContactInInitialPosition(SimulationSingleEvent):
+    def __init__(self):
+        super().__init__()
+
+    def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor):
+        if step_n == 0:
+            n_contacts = env_data.get_amount_contacts()
+            if n_contacts[0] > 0:
+                return EventCommands.STOP
+
+        return EventCommands.CONTINUE
+
+
+class EventContactInInitialPositionBuilder(EventBuilder):
+    def __init__(self):
+        super().__init__(event_class=EventStopExternalForce)
+
+    def build_event(self, event_list) -> EventContactInInitialPosition:
+        event = self.find_event(event_list=event_list)
+        if event:
+            raise Exception(
+                'Attempt to create two same events for a simulation: EventContactInInitialPosition')
+        return event_list.append(EventContactInInitialPosition())
+
 
 class EventBodyTooLowBuilder(EventBuilder):
     def __init__(self, ref_height: float):
         super().__init__(event_class=EventStopExternalForce)
         self.ref_height = ref_height
+
     def build_event(self, event_list) -> EventBodyTooLow:
         event = self.find_event(event_list=event_list)
         if event:
             raise Exception(
                 'Attempt to create two same events for a simulation: EventBodyTooLow')
-        return event_list.append(EventBodyTooLow(ref_height=self.ref_height))
+        return event_list.append(EventBodyTooLow(ref_height=self.ref_height))

wheels_ruleset.ipynb

@@ -370,19 +370,7 @@
    "cell_type": "code",
    "execution_count": 17,
    "metadata": {},
-   "outputs": [
-    {
-     "ename": "",
-     "evalue": "",
-     "output_type": "error",
-     "traceback": [
-      "\u001b[1;31mThe Kernel crashed while executing code in the current cell or a previous cell. \n",
-      "\u001b[1;31mPlease review the code in the cell(s) to identify a possible cause of the failure. \n",
-      "\u001b[1;31mClick <a href='https://aka.ms/vscodeJupyterKernelCrash'>here</a> for more info. \n",
-      "\u001b[1;31mView Jupyter <a href='command:jupyter.viewOutput'>log</a> for further details."
-     ]
-    }
-   ],
+   "outputs": [],
    "source": [
     "#list(result.robot_final_ds.get_data(\"COG\").items())[0][1][-5:]"
    ]
@@ -404,6 +392,7 @@
     "    ChronoBlockCreatorInterface\n",
     "from rostok.block_builder_api.easy_body_shapes import Box\n",
     "\n",
+    "\n",
     "def create_bump_track():\n",
     "    def_mat = DefaultChronoMaterialNSC()\n",
     "    floor = ChronoBlockCreatorInterface.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0]))\n",