[control_optimizer]

app/123.py

@@ -0,0 +1,39 @@
+from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint
+from rostok.control_chrono.tendon_controller import TendonControllerParameters, TendonController_2p
+from rostok.criterion.criterion_calculation import SimulationReward, TimeCriterion
+from rostok.library.rule_sets.simple_designs import get_three_link_one_finger, get_two_link_three_finger
+from rostok.simulation_chrono.simulation_scenario import GraspScenario
+from rostok.trajectory_optimizer.control_optimizer import MockOptiVar, BruteForceOptimisation1D, TendonForceOptiVar
+from rostok.library.obj_grasp import objects
+from rostok.utils.numeric_utils import Offset
+
+
+if __name__ == '__main__':
+    data = TendonControllerParameters()
+    data.amount_pulley_in_body = 2
+    data.pulley_parameters_for_body = {
+        0: [Offset(-0.14, True), Offset(0.005, False, True),
+            Offset(0, True)],
+        1: [Offset(-0.14, True), Offset(-0.005, False, True),
+            Offset(0, True)]
+    }
+    data.starting_point_parameters = [Offset(-0.02, False), Offset(0.025, False), Offset(0, True)]
+    data.tip_parameters = [Offset(-0.3, True), Offset(-0.005, False, True), Offset(0, True)]
+
+    gpesk1 = GraspScenario(0.001, 1, TendonController_2p)
+    gpesk1.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 0)
+
+    gpesk2 = GraspScenario(0.001, 1, TendonController_2p)
+    gpesk2.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 1)
+
+
+    simulation_rewarder = SimulationReward(verbosity=0)
+
+
+
+    graph = get_three_link_one_finger()
+    ttt = TendonForceOptiVar(data, simulation_rewarder, -45)
+
+    kukish = BruteForceOptimisation1D([10, 10, 11, 12, 13], [gpesk1, gpesk2], ttt, num_cpu_workers=1)
+    sosik = kukish.parallel_calculate_reward(graph)
+    pass

app/generate_grasper_cfgs.py

@@ -1,9 +1,45 @@
-from rostok.simulation_chrono.simulation_scenario import GraspScenario
+from dataclasses import dataclass, field
+from typing import Type
+from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint
+from rostok.simulation_chrono.simulation_scenario import GraspScenario, ParametrizedSimulation
+import rostok.control_chrono.external_force as f_ext
 
 
-def create_simulator():
-    pass
-    #return simulation_manager
-    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, obj_external_forces=obj_forces)
+@dataclass
+class SimulationConfig:
+    time_step: float = 0.0001
+    time_simulation: float = 10
+    scenario_cls: Type[ParametrizedSimulation] = GraspScenario
+    obj_disturbance_forces: list[f_ext.ABCForceCalculator] = field(default_factory=list)
 
-def 
+
+# From tis stuff generates sim_manager
+
+
+@dataclass
+class GraspObjective:
+    object_list: list[EnvironmentBodyBlueprint] = field(default_factory=list)
+    weight_list: list[float] = field(default_factory=list)
+    event_time_no_contact: float = 2
+    event_flying_apart_time: float = 2
+    event_slipout_time: float = 1
+    event_grasp_time: float = 5
+    event_force_test_time: float = 5
+
+    time_criterion_weight: float = 1
+    instant_contact_link_criterion_weight: float = 1
+    instant_force_criterion_weight: float = 1
+    instant_cog_criterion_weight: float = 1
+    grasp_time_criterion_weight: float = 1
+    final_pos_criterion_weight: float = 1
+
+@dataclass
+class ControlOptimizationParams:
+    optimisation_bound: tuple = field(default_factory=tuple)
+    optimisation_iter: int = field(default_factory=int)
+
+@dataclass
+class ControlBruteForceParams:
+    force_list: list[float] = field(default_factory=list)
+    staring_angle: float = field(default_factory=float)
+

app/mcts_run_setup.py

@@ -2,7 +2,7 @@
 
 import hyperparameters as hp
 
-from rostok.control_chrono.tendon_controller import TendonControllerParameters
+from rostok.control_chrono.tendon_controller import TendonController_2p, TendonControllerParameters
 from rostok.criterion.criterion_calculation import (
     FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion,
     InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward,
@@ -23,7 +23,7 @@
 
 def config_independent_torque(grasp_object_blueprint):
     # configurate the simulation manager
-    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, tendon = False)
+    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION)
     # simulation_manager.grasp_object_callback = lambda: creator.create_environment_body(
     #     grasp_object_blueprint)
     simulation_manager.grasp_object_callback = grasp_object_blueprint
@@ -78,7 +78,7 @@ def config_with_tendon(grasp_object_blueprint):
     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=obj_forces)
+    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p, obj_external_forces=obj_forces)
     simulation_manager.grasp_object_callback = grasp_object_blueprint #lambda: creator.create_environment_body(
         #grasp_object_blueprint)
     event_contact = EventContact()
@@ -140,7 +140,7 @@ def config_with_tendon(grasp_object_blueprint):
 
 def config_combination_force_tendon_multiobject_parallel(grasp_object_blueprint, weights):
     # configurate the simulation manager
-    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, True)
+    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p)
     object_callback = grasp_object_blueprint
     #[
     #    (lambda obj=obj: creator.create_environment_body(obj)) for obj in grasp_object_blueprint

rostok/simulation_chrono/simulation_scenario.py

@@ -17,15 +17,20 @@
 from rostok.block_builder_chrono.block_builder_chrono_api import \
     ChronoBlockCreatorInterface as creator
 from rostok.control_chrono.tendon_controller import TendonController_2p
-
+from abc import abstractmethod
 
 class ParametrizedSimulation:
 
     def __init__(self, step_length, simulation_length):
         self.step_length = step_length
         self.simulation_length = simulation_length
 
-    def run_simulation(self, graph: GraphGrammar, data):
+    def run_simulation(self,
+                       graph: GraphGrammar,
+                       controller_data,
+                       starting_positions=None,
+                       vis=False,
+                       delay=False):
         pass
 
     def __repr__(self) -> str:
@@ -35,30 +40,23 @@ def __repr__(self) -> str:
     def __str__(self) -> str:
         json_data = json.dumps(self, indent=4, cls=RostokJSONEncoder)
         return json_data
-
-
-class GraspScenario(ParametrizedSimulation):
-    """
 
+    @abstractmethod
+    def get_scenario_name(self):
+        return self.__str__
 
 
-
-
-    PROKINUT CONTROLL CLS
-
-    Args:
-        ParametrizedSimulation (_type_): _description_
-    """
+class GraspScenario(ParametrizedSimulation):
     def __init__(self,
                  step_length,
                  simulation_length,
-                 tendon=True,
+                 controller_cls = ConstController,
                  smc=False,
                  obj_external_forces: Optional[ABCForceCalculator] = None) -> None:
         super().__init__(step_length, simulation_length)
         self.grasp_object_callback = None
         self.event_container: List[SimulationSingleEvent] = []
-        self.tendon = tendon
+        self.controller_cls = controller_cls
         self.smc = smc
         self.obj_external_forces = obj_external_forces
 
@@ -71,7 +69,7 @@ def reset_events(self):
 
     def run_simulation(self,
                        graph: GraphGrammar,
-                       data,
+                       controller_data,
                        starting_positions=None,
                        vis=False,
                        delay=False):
@@ -101,13 +99,12 @@ def run_simulation(self,
                                           force_torque_controller=chrono_forces)
 
         # add design and determine the outer force
-        if self.tendon:
-            simulation.add_design(graph,
-                                  data,
-                                  TendonController_2p,
-                                  starting_positions=starting_positions)
-        else:
-            simulation.add_design(graph, data, starting_positions=starting_positions)
+
+        simulation.add_design(graph,
+                                controller_data,
+                                self.controller_cls,
+                                starting_positions=starting_positions)
+
         # setup parameters for the data store
 
         n_steps = int(self.simulation_length / self.step_length)
@@ -127,4 +124,7 @@ def run_simulation(self,
             "n_contacts": (SensorCalls.AMOUNT_FORCE, SensorObjectClassification.BODY)
         }
         simulation.add_robot_data_type_dict(robot_data_dict)
-        return simulation.simulate(n_steps, self.step_length, 10000, self.event_container, vis)
+        return simulation.simulate(n_steps, self.step_length, 10000, self.event_container, vis)
+
+    def get_scenario_name(self):
+        return str(self.grasp_object_callback)