example

aimclub · Oct 11, 2023 · a57ae3d · a57ae3d
1 parent 9295aff
commit a57ae3d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py
@@ -75,9 +75,10 @@ def config_with_tendon(grasp_object_blueprint):
     # configurate the simulation manager
 
     obj_forces = []
-    obj_forces.append(f_ext.NullGravity(1))
+    obj_forces.append(f_ext.NullGravity(0))
+    obj_forces.append(f_ext.RandomForces(1e6, 100, 0))
     obj_forces = f_ext.ExternalForces(obj_forces)
-    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, obj_external_forces=f_ext.NullGravity(0))
+    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, obj_external_forces=obj_forces)
     simulation_manager.grasp_object_callback = grasp_object_blueprint #lambda: creator.create_environment_body(
         #grasp_object_blueprint)
     event_contact = EventContact()

diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py
@@ -249,7 +249,7 @@ def __init__(self,
                  start_time: float = 0.0,
                  pos: np.ndarray = np.zeros(3),
                  dimension="3d",
-                 angle=0.0) -> None:
+                 angle=(0.0, 0.0)) -> None:
         """Apply force with random direction and given amplitude
 
         Args:
@@ -271,9 +271,11 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray:
         if time >= self.start_time:
             if self.counter % self.width_step == 0:
                 if self.dim == '2d':
-                    self.spatial_force[3:] = random_2d_vector(self.amp, self.angle)
-                else:
+                    self.spatial_force[3:] = random_plane_vector(self.amp, *self.angle)
+                elif self.dim == '3d':
                     self.spatial_force[3:] = random_3d_vector(self.amp)
+                else:
+                    raise Exception("Wrong name of dimension. Should be 2d or 3d")
             self.counter += 1
         return self.spatial_force
 
@@ -337,12 +339,9 @@ def add_force(self, force: ForceTemplate):
 
     def calculate_spatial_force(self, time: float, data) -> ForceTorque:
         if isinstance(self.force_controller, list):
-            v_forces = np.zeros(3)
+            v_forces = np.zeros(6)
             for controller in self.force_controller:
                 v_forces += np.array(controller.calculate_spatial_force(time, data))
-            impact = ForceTorque()
-            impact.force = tuple(v_forces[3:])
-            impact.torque = tuple(v_forces[:3])
-            return impact
+            return v_forces
         else:
             return self.force_controller.calculate_spatial_force(time, data)