update simstep

aimclub · Apr 26, 2023 · 22591f1 · 22591f1
1 parent e0686de
commit 22591f1
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 12 deletions.
diff --git a/rostok/virtual_experiment/robot_new.py b/rostok/virtual_experiment/robot_new.py
@@ -170,4 +170,4 @@ def __init__(self,
         self.controller = SinControllerChrono(self.__built_graph.joint_vector, control_parameters)
 
     def get_data(self):
-        return self.sensor.trajectories
+        return self.sensor.body_trajectories
diff --git a/rostok/virtual_experiment/simulation_step.py b/rostok/virtual_experiment/simulation_step.py
@@ -122,10 +122,7 @@ def __init__(self,
         register_chrono_system(self.chrono_system)
 
         self.grab_robot = Robot(self.graph_mechanism, self.chrono_system, start_frame_robot)
-        self.contact_reporter = ContactReporter()
-        self.contact_reporter.set_body_list([(-1, self.grasp_object),
-                                             (1, self.grab_robot.get_base_body())])
-        self.contact_reporter.reset_contact_dict()
+        self.sensor = Sensor([(-1, self.grasp_object)], [])
         # Add grasp object in system and set system without gravity
         self.chrono_system.Set_G_acc(chrono.ChVectorD(0, 0, 0))
 
@@ -270,13 +267,12 @@ def append_arr_in_dict(x, y):
 
             # Get current variables from object
             #SensorFunctions.reset_reporter_for_objects(self.chrono_system, [self.grasp_object.body], self.contact_reporter)
-            self.contact_reporter.reset_contact_dict()
-
-            sensor = Sensor(self.contact_reporter, self.chrono_system)
-            current_data_std_obj_force = sensor.std_contact_forces()
-            current_data_cont_coord = sensor.contact_coord()
-            current_data_abs_coord_COG_obj = sensor.abs_coord_COG(self.grasp_object.body)
-            current_data_amount_obj_contact_surfaces = sensor.amount_contact_forces()
+            self.sensor.contact_reporter.reset_contact_dict()
+            self.sensor.update_current_contact_info(self.chrono_system)
+            current_data_std_obj_force = self.sensor.std_contact_forces()
+            current_data_cont_coord = self.sensor.contact_coord()
+            current_data_abs_coord_COG_obj = self.sensor.abs_coord_COG(self.grasp_object.body)
+            current_data_amount_obj_contact_surfaces = self.sensor.amount_contact_forces()
 
             # TODO: Make it possible to get information from the robot blocks