Merge pull request #130 from aimclub/feature/split_config

Refactoring control optimize class

app/control_optimizer_test.py

@@ -0,0 +1,82 @@
+from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint
+from rostok.control_chrono.controller import ConstController
+from rostok.control_chrono.tendon_controller import TendonControllerParameters, TendonController_2p
+from rostok.criterion.criterion_calculation import SimulationReward, TimeCriterion
+from rostok.graph_grammar.graphgrammar_explorer import random_search_mechs_n_branch
+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 ConstTorqueOptiVar, GlobalOptimisationEachSim, BruteForceOptimisation1D, TendonForceOptiVar
+from rostok.library.obj_grasp import objects
+from rostok.utils.numeric_utils import Offset
+from rostok.library.rule_sets.rulset_simple_fingers import create_rules
+
+
+def get_tendon_cfg():
+    tendon_controller_cfg = TendonControllerParameters()
+    tendon_controller_cfg.amount_pulley_in_body = 2
+    tendon_controller_cfg.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)]
+    }
+    tendon_controller_cfg.starting_point_parameters = [
+        Offset(-0.02, False), Offset(0.025, False),
+        Offset(0, True)
+    ]
+    tendon_controller_cfg.tip_parameters = [
+        Offset(-0.3, True), Offset(-0.005, False, True),
+        Offset(0, True)
+    ]
+    return tendon_controller_cfg
+
+
+if __name__ == '__main__':
+    VIS = False
+    tendon_controller_cfg = get_tendon_cfg()
+
+    gpesk_tendon1 = GraspScenario(0.002, 1, TendonController_2p)
+    gpesk_tendon1.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 0)
+
+    gpesk_tendon2 = GraspScenario(0.002, 1, TendonController_2p)
+    gpesk_tendon2.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 1)
+
+    gpesk_const1 = GraspScenario(0.001, 1, ConstController)
+    gpesk_const1.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 0)
+
+    gpesk_const2 = GraspScenario(0.001, 1, ConstController)
+    gpesk_const2.grasp_object_callback = objects.get_object_box(0.1, 0.1, 0.1, 1)
+
+    simulation_rewarder = SimulationReward(verbosity=0)
+
+    graph = get_two_link_three_finger()
+    graph_finger = get_three_link_one_finger()
+
+    tendon_optivar = TendonForceOptiVar(tendon_controller_cfg, simulation_rewarder, -45)
+    tendon_optivar.is_vis = VIS
+
+    const_optivar = ConstTorqueOptiVar(simulation_rewarder, -45)
+    const_optivar.is_vis = VIS
+    direct_args = {"maxiter": 2}
+
+    global_const = GlobalOptimisationEachSim([gpesk_const1, gpesk_const2], const_optivar, (0, 10),
+                                             direct_args)
+    res = global_const.calculate_reward(graph_finger)
+
+    brute_const = BruteForceOptimisation1D([0, 10, 15, 20], [gpesk_const1, gpesk_const2],
+                                           const_optivar,
+                                           #weights = [2, 3],
+                                           num_cpu_workers=4,
+                                           timeout_parallel=7)
+    res = brute_const.calculate_reward(graph_finger)
+
+    brute_tendon = BruteForceOptimisation1D([0, 10], [gpesk_tendon1, gpesk_tendon2],
+                                            tendon_optivar,
+                                            num_cpu_workers=1)
+    res = brute_tendon.calculate_reward(graph)
+
+    global_opti_tendon = GlobalOptimisationEachSim([gpesk_tendon1, gpesk_tendon2], tendon_optivar,
+                                                   (0, 10), direct_args)
+    res = global_opti_tendon.calculate_reward(graph)
+
+    pass

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()
@@ -135,3 +135,72 @@ def config_with_tendon(grasp_object_blueprint):
                                             tendon_forces = hp.TENDON_DISCRETE_FORCES)
 
     return control_optimizer
+
+
+
+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, TendonController_2p)
+    object_callback = grasp_object_blueprint
+    #[
+    #    (lambda obj=obj: creator.create_environment_body(obj)) for obj in grasp_object_blueprint
+    #]
+    simulation_managers = []
+    for k in range(len(grasp_object_blueprint)):
+        simulation_managers.append((deepcopy(simulation_manager), weights[k]))
+        simulation_managers[-1][0].grasp_object_callback = object_callback[k]
+
+    event_contact = EventContact()
+    event_timeout = EventContactTimeOut(hp.FLAG_TIME_NO_CONTACT, event_contact)
+    event_flying_apart = EventFlyingApart(hp.FLAG_FLYING_APART)
+    event_slipout = EventSlipOut(hp.FLAG_TIME_SLIPOUT)
+    event_grasp = EventGrasp(
+        grasp_limit_time=hp.GRASP_TIME,
+        contact_event=event_contact,
+        verbosity=0,
+    )
+    event_stop_external_force = EventStopExternalForce(grasp_event=event_grasp,
+                                                       force_test_time=hp.FORCE_TEST_TIME)
+    for manager in simulation_managers:
+        manager[0].add_event(event_contact)
+        manager[0].add_event(event_timeout)
+        manager[0].add_event(event_flying_apart)
+        manager[0].add_event(event_grasp)
+        manager[0].add_event(event_stop_external_force)
+
+        #create criterion manager
+    simulation_rewarder = SimulationReward(verbosity=0)
+    #create criterions and add them to manager
+    simulation_rewarder.add_criterion(TimeCriterion(hp.GRASP_TIME, event_timeout, event_grasp),
+                                      hp.TIME_CRITERION_WEIGHT)
+    #simulation_rewarder.add_criterion(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp),
+                                      hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp),
+                                      hp.INSTANT_FORCE_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp),
+                                      hp.OBJECT_COG_CRITERION_WEIGHT)
+    n_steps = int(hp.GRASP_TIME / hp.TIME_STEP_SIMULATION)
+    print(n_steps)
+    simulation_rewarder.add_criterion(GraspTimeCriterion(event_grasp, n_steps),
+                                      hp.GRASP_TIME_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(
+        FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp, event_slipout),
+        hp.FINAL_POSITION_CRITERION_WEIGHT)
+
+    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)]
+
+
+    control_optimizer = ParralelOptimizerCombinationForce(simulation_managers,
+                                                       simulation_rewarder, data, hp.TENDON_DISCRETE_FORCES, starting_finger_angles=-25)
+
+    return control_optimizer

rostok/control_chrono/control_utils.py

@@ -1,6 +1,8 @@
 from dataclasses import dataclass, field
 
 from rostok.control_chrono.external_force import ForceChronoWrapper
+from rostok.graph_grammar.node import GraphGrammar
+from rostok.graph_grammar.node_block_typing import get_joint_vector_from_graph
 
 
 @dataclass
@@ -16,3 +18,21 @@ def add(self, controller: ForceChronoWrapper):
             self.controller_list.append(controller)
         else:
             raise Exception("Force controller should be bound to body, before use")
+
+
+def build_control_graph_from_joint(graph: GraphGrammar, joint_dict: dict):
+    """Build control parametrs based on joint_dict and graph structure
+
+    Args:
+        graph (GraphGrammar): _description_
+        joint_dict (dict): maps joint to value
+
+    Returns:
+        _type_: _description_
+    """
+    joints = get_joint_vector_from_graph(graph)
+    control_sequence = []
+    for idx in joints:
+        node = graph.get_node_by_id(idx)
+        control_sequence.append(joint_dict[node])
+    return control_sequence

rostok/graph_grammar/graph_comprehension.py

@@ -2,7 +2,7 @@
 
 from rostok.graph_grammar.node import GraphGrammar
 from typing import Any, NamedTuple, Optional, TypedDict, Union
-from rostok.graph_grammar.node_block_typing import NodeFeatures
+from rostok.graph_grammar.node_block_typing import NodeFeatures, get_joint_vector_from_graph
 
 
 def is_star_topology(graph: nx.DiGraph):
@@ -57,3 +57,8 @@ def get_tip_ids(graph: GraphGrammar) -> list[int]:
         if not tip:
             raise Exception('Attempt to find a tip on a path without bodies')
     return tip_bodies
+
+
+def is_valid_graph(graph: GraphGrammar):
+    n_joints = len(get_joint_vector_from_graph(graph))
+    return n_joints > 0

rostok/library/rule_sets/simple_designs.py

@@ -53,19 +53,21 @@ def get_two_link_three_finger():
     graph = GraphGrammar()
     rules = ["Init",
         "AddFinger", "Terminal_Radial_Translate1", "Phalanx", "Phalanx", "Remove_FG",
-        "Terminal_Link3", "Terminal_Joint1", "Terminal_Joint6", "Terminal_Link1", 
-        "RemoveFinger_N",
-        "RemoveFinger_R", 
-        "AddFinger_RNT", "Terminal_Radial_Translate1", "Phalanx", "Phalanx",
-        "Remove_FG", "Terminal_Joint2", "Terminal_Link1", "Terminal_Joint6", "Terminal_Link1",
-        "RemoveFinger_P", 
-        "AddFinger_RPT", "Terminal_Radial_Translate1", "Phalanx", "Phalanx",
-        "Remove_FG", "Terminal_Joint2", "Terminal_Link1", "Terminal_Joint6", "Terminal_Link1"
+
+        "AddFinger_P", "Terminal_Radial_Translate1", "Phalanx", "Phalanx", "Remove_FG", 
+
+        "AddFinger_N", "Terminal_Radial_Translate1", "Phalanx", "Phalanx", "Remove_FG", 
+
+        "RemoveFinger_RP",
+        "RemoveFinger_RN",
+        "RemoveFinger_R",
+
+
     ]
-    rule_vocabul, _ = create_rules()
+    rule_vocabul = create_rules()
     for rule in rules:
         graph.apply_rule(rule_vocabul.get_rule(rule))
-
+    rule_vocabul.make_graph_terminal(graph)
     return graph
 
 

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,20 +40,23 @@ def __repr__(self) -> str:
     def __str__(self) -> str:
         json_data = json.dumps(self, indent=4, cls=RostokJSONEncoder)
         return json_data
+
+    @abstractmethod
+    def get_scenario_name(self):
+        return self.__str__
 
 
 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
 
@@ -61,7 +69,7 @@ def reset_events(self):
 
     def run_simulation(self,
                        graph: GraphGrammar,
-                       data,
+                       controller_data,
                        starting_positions=None,
                        vis=False,
                        delay=False):
@@ -91,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)
@@ -117,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)