diff --git a/app/app_new_mcts.py b/app/app_new_mcts.py new file mode 100644 index 00000000..0333fdf1 --- /dev/null +++ b/app/app_new_mcts.py @@ -0,0 +1,27 @@ +import hyperparameters as hp +from rostok.graph_generators.environments.design_environment import (SubStringDesignEnvironment) +from rostok.graph_generators.mcts_manager import MCTSManager +from rostok.graph_generators.search_algorithms.mcts import MCTS + +from rostok.library.rule_sets.rulset_simple_fingers import create_rules +from rostok.graph_grammar.node import GraphGrammar +import hyperparameters as hp +from tendon_graph_evaluators import evaluator_tendon_standart, evaluator_tendon_standart_parallel +import tendon_driven_cfg + +if __name__ == "__main__": + rule_vocabulary = create_rules() + + init_graph = GraphGrammar() + env = SubStringDesignEnvironment(rule_vocabulary, evaluator_tendon_standart_parallel, + hp.MAX_NUMBER_RULES, init_graph, 0) + + mcts = MCTS(env, hp.MCTS_C) + name_directory = input("enter directory name: ") + mcts_manager = MCTSManager(mcts, name_directory, verbosity=2, use_date=True) + #mcts_manager.save_information_about_search( + # hp, tendon_driven_cfg.default_grasp_objective.object_list) + + for i in range(hp.FULL_LOOP_MCTS): + mcts_manager.run_search(hp.BASE_ITERATION_LIMIT_TENDON, 1, 1, 2) + mcts_manager.save_results() \ No newline at end of file diff --git a/app/generate_grasper_cfgs.py b/app/generate_grasper_cfgs.py deleted file mode 100644 index 40187a5b..00000000 --- a/app/generate_grasper_cfgs.py +++ /dev/null @@ -1,129 +0,0 @@ -from copy import deepcopy -from dataclasses import dataclass, field -from typing import Type -from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint -from rostok.control_chrono.controller import ConstController, RobotControllerChrono -from rostok.criterion.criterion_calculation import FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion, InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward, TimeCriterion -from rostok.criterion.simulation_flags import EventContact, EventContactTimeOut, EventFlyingApart, EventSlipOut, \ -EventGrasp, EventStopExternalForce -from rostok.simulation_chrono.simulation_scenario import GraspScenario, ParametrizedSimulation -import rostok.control_chrono.external_force as f_ext -from rostok.trajectory_optimizer.control_optimizer import BasePrepareOptiVar, ConstTorqueOptiVar - - -@dataclass -class SimulationConfig: - time_step: float = 0.0001 - time_simulation: float = 10 - scenario_cls: Type[GraspScenario] = GraspScenario - obj_disturbance_forces: list[f_ext.ABCForceCalculator] = field(default_factory=list) - - -# 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 BaseCalculateRewardCfg: - sim_config: SimulationConfig = field(default_factory=SimulationConfig) - grasp_objective: GraspObjective = field(default_factory=GraspObjective) - prepare_reward: BasePrepareOptiVar = ConstTorqueOptiVar(SimulationReward(), None) - - -@dataclass -class BruteForceRewardCfg(BaseCalculateRewardCfg): - pass - - -def create_base_sim(contoller_cls, sim_cfg: SimulationConfig): - """Base sim without object - - Args: - contoller_cls (_type_): _description_ - sim_cfg (SimulationConfig): _description_ - - Returns: - _type_: _description_ - """ - return sim_cfg.scenario_cls(sim_cfg.time_step, - sim_cfg.time_simulation, - contoller_cls, - obj_external_forces=sim_cfg.obj_disturbance_forces) - - -def add_grasp_events_from_cfg(sim: GraspScenario, cfg: GraspObjective): - event_contact = EventContact() - sim.add_event(event_contact) - event_timeout = EventContactTimeOut(cfg.event_time_no_contact, event_contact) - sim.add_event(event_timeout) - event_flying_apart = EventFlyingApart(cfg.event_flying_apart_time) - sim.add_event(event_flying_apart) - event_slipout = EventSlipOut(cfg.event_slipout_time) - sim.add_event(event_slipout) - event_grasp = EventGrasp(grasp_limit_time=cfg.event_grasp_time, - contact_event=event_contact, - verbosity=0, - simulation_stop=1) - sim.add_event(event_grasp) - event_stop_external_force = EventStopExternalForce(grasp_event=event_grasp, - force_test_time=cfg.event_force_test_time) - sim.add_event(event_stop_external_force) - return event_contact, event_timeout, event_flying_apart, event_slipout, event_stop_external_force - - -def create_sim_list(base_sim: GraspScenario, object_list: list[EnvironmentBodyBlueprint]): - sim_list = [] - for obj_i in object_list: - sim_i = deepcopy(base_sim) - sim_i.grasp_object_callback = obj_i - sim_list.append(sim_i) - return sim_list - - -def prepare_simulation_scenario_list(contoller_cls, sim_cfg: SimulationConfig, - grasp_objective_cfg: GraspObjective): - - base_simulation = create_base_sim(contoller_cls, sim_cfg) - event_contact, event_timeout, event_flying_apart, event_slipout, event_stop_external_force = add_grasp_events_from_cfg( - base_simulation, grasp_objective_cfg) - sim_list = create_sim_list(base_simulation, grasp_objective_cfg.object_list) - - return sim_list - - -def create_rewarder(grasp_objective_cfg: GraspObjective): - simulation_rewarder = SimulationReward(verbosity=0) - - simulation_rewarder.add_criterion(TimeCriterion(hp.GRASP_TIME, event_timeout, event_grasp), - hp.TIME_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) diff --git a/app/hyperparameters.py b/app/hyperparameters.py index 8193b570..e711b51b 100644 --- a/app/hyperparameters.py +++ b/app/hyperparameters.py @@ -1,32 +1,33 @@ -MAX_NUMBER_RULES = 13 +MAX_NUMBER_RULES = 13 + +BASE_ITERATION_LIMIT_TENDON = 10 +FULL_LOOP_MCTS = 10 +MCTS_C = 5 -BASE_ITERATION_LIMIT = 30 -BASE_ITERATION_LIMIT_GRAPH = 200 -BASE_ITERATION_LIMIT_TENDON = 100 ITERATION_REDUCTION_TIME = 0.7 TIME_CRITERION_WEIGHT = 3 -FORCE_CRITERION_WEIGHT = 1 +FORCE_CRITERION_WEIGHT = 0.5 OBJECT_COG_CRITERION_WEIGHT = 1 INSTANT_FORCE_CRITERION_WEIGHT = 1 -INSTANT_CONTACTING_LINK_CRITERION_WEIGHT = 1 +INSTANT_CONTACTING_LINK_CRITERION_WEIGHT = 2 GRASP_TIME_CRITERION_WEIGHT = 1 FINAL_POSITION_CRITERION_WEIGHT = 5 -REFERENCE_DISTANCE = 0.3 +REFERENCE_DISTANCE = 0.1 -CONTROL_OPTIMIZATION_BOUNDS = (3, 15) +CONTROL_OPTIMIZATION_BOUNDS = (5, 15) CONTROL_OPTIMIZATION_BOUNDS_TENDON = (10, 20) CONTROL_OPTIMIZATION_ITERATION = 8 CONTROL_OPTIMIZATION_ITERATION_TENDON = 20 TENDON_CONST = -5 -TENDON_DISCRETE_FORCES = [10, 15, 20] +TENDON_DISCRETE_FORCES = [10, 15, 20] -TIME_STEP_SIMULATION = 0.00001 -GRASP_TIME = 5 -FORCE_TEST_TIME = 5 -TIME_SIMULATION = 10 +TIME_STEP_SIMULATION = 0.0005 +GRASP_TIME = 1.5 +FORCE_TEST_TIME = 3 +TIME_SIMULATION = 4.5 -FLAG_TIME_NO_CONTACT = 2.5 -FLAG_TIME_SLIPOUT = 1 +FLAG_TIME_NO_CONTACT = 0.5 +FLAG_TIME_SLIPOUT = 0.4 FLAG_FLYING_APART = 10 \ No newline at end of file diff --git a/app/tendon_driven_cfg.py b/app/tendon_driven_cfg.py new file mode 100644 index 00000000..5e14bc4e --- /dev/null +++ b/app/tendon_driven_cfg.py @@ -0,0 +1,59 @@ +from rostok.control_chrono.tendon_controller import TendonControllerParameters +from rostok.library.obj_grasp.objects import get_object_parametrized_cuboctahedron, get_object_parametrized_dipyramid_3 +from rostok.pipeline.generate_grasper_cfgs import MCTSCfg, SimulationConfig, GraspObjective, BruteForceRewardCfg +from rostok.simulation_chrono.simulation_scenario import GraspScenario +import rostok.control_chrono.external_force as f_ext +from rostok.trajectory_optimizer.control_optimizer import TendonForceOptiVar +from rostok.utils.numeric_utils import Offset + + +HARD_OBJECT_SET = [ + get_object_parametrized_cuboctahedron(0.05, mass=0.100), + get_object_parametrized_dipyramid_3(0.05, mass=0.200) +] +HARD_OBJECT_SET_W = [1.0, 1.0] + + +def get_random_force_with_null_grav(amp): + obj_forces = [] + obj_forces.append(f_ext.NullGravity(0)) + obj_forces.append(f_ext.RandomForces(amp, 100, 0)) + obj_forces = f_ext.ExternalForces(obj_forces) + return obj_forces + + +def get_default_tendon_params(): + 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)] + return data + + +rand_null_force = get_random_force_with_null_grav(10) +default_simulation_config = SimulationConfig(0.0005, 4.5, GraspScenario, rand_null_force) +default_grasp_objective = GraspObjective( + # Objects setup + HARD_OBJECT_SET, + HARD_OBJECT_SET_W, + # Event setup + event_time_no_contact_param=0.5, + event_flying_apart_time_param=10, + event_slipout_time_param=0.4, + event_grasp_time_param=1.5, + event_force_test_time_param=3, + # Weight setup + time_criterion_weight=3, + final_pos_criterion_weight=5, + # Object pos setup + refernece_distance=0.3) + +brute_force_opti_default_cfg = BruteForceRewardCfg([10, 15, 20]) + +hyperparams_mcts_default = MCTSCfg() diff --git a/app/tendon_graph_evaluators.py b/app/tendon_graph_evaluators.py new file mode 100644 index 00000000..e50eba98 --- /dev/null +++ b/app/tendon_graph_evaluators.py @@ -0,0 +1,33 @@ +from copy import deepcopy +from rostok.trajectory_optimizer.control_optimizer import TendonForceOptiVar +import tendon_driven_cfg +import rostok.pipeline.generate_grasper_cfgs as pipeline + +tendon_optivar_standart = TendonForceOptiVar(tendon_driven_cfg.get_default_tendon_params(), + params_start_pos=-45) + +evaluator_tendon_standart = pipeline.create_reward_calulator( + tendon_driven_cfg.default_simulation_config, tendon_driven_cfg.default_grasp_objective, + tendon_optivar_standart, tendon_driven_cfg.brute_force_opti_default_cfg) + +brute_force_opti_default_cfg_parallel = deepcopy(tendon_driven_cfg.brute_force_opti_default_cfg) +brute_force_opti_default_cfg_parallel.num_cpu_workers = 6 + +evaluator_tendon_standart_parallel = pipeline.create_reward_calulator( + tendon_driven_cfg.default_simulation_config, tendon_driven_cfg.default_grasp_objective, + tendon_optivar_standart, brute_force_opti_default_cfg_parallel) + + + +tendon_optivar_strong = TendonForceOptiVar(tendon_driven_cfg.get_default_tendon_params(), + params_start_pos=-60) +brute_force_opti_strong = deepcopy(tendon_driven_cfg.brute_force_opti_default_cfg) +brute_force_opti_strong.variants = [30, 40, 50] + +simulation_config_strong = deepcopy(tendon_driven_cfg.default_simulation_config) +simulation_config_strong.obj_disturbance_forces = tendon_driven_cfg.get_random_force_with_null_grav( + 150) + +evaluator_tendon_strong = pipeline.create_reward_calulator( + simulation_config_strong, tendon_driven_cfg.default_grasp_objective, tendon_optivar_strong, + brute_force_opti_strong) diff --git a/rostok/pipeline/generate_grasper_cfgs.py b/rostok/pipeline/generate_grasper_cfgs.py new file mode 100644 index 00000000..2dbf3e5a --- /dev/null +++ b/rostok/pipeline/generate_grasper_cfgs.py @@ -0,0 +1,193 @@ +from copy import deepcopy +from dataclasses import dataclass, field +from typing import Type + +from pyparsing import Any +from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint +from rostok.control_chrono.controller import ConstController, RobotControllerChrono +from rostok.criterion.criterion_calculation import FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion, InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward, TimeCriterion +from rostok.criterion.simulation_flags import EventContact, EventContactTimeOut, EventFlyingApart, EventSlipOut, \ +EventGrasp, EventStopExternalForce +from rostok.simulation_chrono.simulation_scenario import GraspScenario, ParametrizedSimulation +import rostok.control_chrono.external_force as f_ext +from rostok.trajectory_optimizer.control_optimizer import BasePrepareOptiVar, BruteForceOptimisation1D, ConstTorqueOptiVar, GlobalOptimisationEachSim +from scipy.optimize import direct + + +@dataclass +class SimulationConfig: + time_step: float = 0.0001 + time_simulation: float = 10 + scenario_cls: Type[GraspScenario] = GraspScenario + obj_disturbance_forces: list[f_ext.ABCForceCalculator] = field(default_factory=list) + + +@dataclass +class GraspObjective: + object_list: list[EnvironmentBodyBlueprint] = field(default_factory=list) + weight_list: list[float] = field(default_factory=list) + event_time_no_contact_param: float = 2 + event_flying_apart_time_param: float = 2 + event_slipout_time_param: float = 1 + event_grasp_time_param: float = 5 + event_force_test_time_param: 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 + + refernece_distance: float = 0.3 + + +@dataclass +class BruteForceRewardCfg(): + variants: list[float] = field(default_factory=list) + num_cpu_workers = 1 + timeout_parallel: float = 20 + chunksize = 'auto' + + +@dataclass +class GlobalOptimisationRewardCfg(): + optimisation_tool: Any = direct + args_for_optimiser = None + bound: tuple[float, float] = (0, 1) + + +@dataclass +class MCTSCfg(): + C: int = 5 + full_loop: int = 10 + base_iteration: int = 10 + max_number_rules: int = 13 + + +def create_base_sim(contoller_cls, sim_cfg: SimulationConfig): + """Base sim without object + + Args: + contoller_cls (_type_): _description_ + sim_cfg (SimulationConfig): _description_ + + Returns: + _type_: _description_ + """ + return sim_cfg.scenario_cls(sim_cfg.time_step, + sim_cfg.time_simulation, + contoller_cls, + obj_external_forces=sim_cfg.obj_disturbance_forces) + + +def add_grasp_events_from_cfg(sim: GraspScenario, cfg: GraspObjective): + event_contact = EventContact() + sim.add_event(event_contact) + event_timeout = EventContactTimeOut(cfg.event_time_no_contact_param, event_contact) + sim.add_event(event_timeout) + event_flying_apart = EventFlyingApart(cfg.event_flying_apart_time_param) + sim.add_event(event_flying_apart) + event_slipout = EventSlipOut(cfg.event_slipout_time_param) + sim.add_event(event_slipout) + event_grasp = EventGrasp(grasp_limit_time=cfg.event_grasp_time_param, + contact_event=event_contact, + verbosity=0, + simulation_stop=True) + sim.add_event(event_grasp) + event_stop_external_force = EventStopExternalForce( + grasp_event=event_grasp, force_test_time=cfg.event_force_test_time_param) + sim.add_event(event_stop_external_force) + return event_contact, event_timeout, event_flying_apart, event_slipout, event_stop_external_force, event_grasp + + +def create_sim_list(base_sim: GraspScenario, object_list: list[EnvironmentBodyBlueprint]): + sim_list = [] + for obj_i in object_list: + sim_i = deepcopy(base_sim) + sim_i.grasp_object_callback = obj_i + sim_list.append(sim_i) + return sim_list + + +def prepare_simulation_scenario_list(contoller_cls, sim_cfg: SimulationConfig, + grasp_objective_cfg: GraspObjective): + + base_simulation = create_base_sim(contoller_cls, sim_cfg) + # event_contact, event_timeout, event_flying_apart, event_slipout, event_stop_external_force, event_grasp = add_grasp_events_from_cfg( + # base_simulation, grasp_objective_cfg) + sim_list = create_sim_list(base_simulation, grasp_objective_cfg.object_list) + + return sim_list + + +def create_rewarder(grasp_objective_cfg: GraspObjective, sim_cfg: SimulationConfig, event_timeout, + event_grasp, event_slipout): + simulation_rewarder = SimulationReward(verbosity=0) + + simulation_rewarder.add_criterion( + TimeCriterion(grasp_objective_cfg.event_grasp_time_param, event_timeout, event_grasp), + grasp_objective_cfg.time_criterion_weight) + + simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp), + grasp_objective_cfg.instant_contact_link_criterion_weight) + simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp), + grasp_objective_cfg.instant_contact_link_criterion_weight) + simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp), + grasp_objective_cfg.instant_cog_criterion_weight) + n_steps = int(grasp_objective_cfg.event_grasp_time_param / sim_cfg.time_step) + print(n_steps) + simulation_rewarder.add_criterion(GraspTimeCriterion(event_grasp, n_steps), + grasp_objective_cfg.grasp_time_criterion_weight) + simulation_rewarder.add_criterion( + FinalPositionCriterion(grasp_objective_cfg.refernece_distance, event_grasp, event_slipout), + grasp_objective_cfg.final_pos_criterion_weight) + + return simulation_rewarder + + +def create_global_optimisation(sim_list: list[GraspScenario], preapare_reward: BasePrepareOptiVar, + glop_cfg: GlobalOptimisationRewardCfg): + rew = GlobalOptimisationEachSim(sim_list, preapare_reward, glop_cfg.bound, + glop_cfg.args_for_optimiser, glop_cfg.optimisation_tool) + + return rew + + +def create_bruteforce_optimisation(sim_list: list[GraspScenario], + preapare_reward: BasePrepareOptiVar, + grasp_objective: GraspObjective, brute_cfg: BruteForceRewardCfg): + rew = BruteForceOptimisation1D(brute_cfg.variants, sim_list, preapare_reward, + grasp_objective.weight_list, brute_cfg.num_cpu_workers, + brute_cfg.chunksize, brute_cfg.timeout_parallel) + + return rew + + +def create_reward_calulator(sim_config: SimulationConfig, grasp_objective: GraspObjective, + prepare_reward: BasePrepareOptiVar, + optimisation_control_cgf: GlobalOptimisationRewardCfg | + BruteForceRewardCfg): + + simlist = prepare_simulation_scenario_list(prepare_reward.control_class, sim_config, + grasp_objective) + + event_timeout = event_grasp = event_slipout = None + + for sim_i in simlist: + _, event_timeout, _, event_slipout, _, event_grasp = add_grasp_events_from_cfg( + sim_i, grasp_objective) + + rewarder = create_rewarder(grasp_objective, sim_config, event_timeout, event_grasp, + event_slipout) + + prepare_reward.set_reward_fun(rewarder) + + if isinstance(optimisation_control_cgf, GlobalOptimisationRewardCfg): + return create_global_optimisation(simlist, prepare_reward, optimisation_control_cgf) + elif isinstance(optimisation_control_cgf, BruteForceRewardCfg): + return create_bruteforce_optimisation(simlist, prepare_reward, grasp_objective, + optimisation_control_cgf) + else: + raise Exception("Wrong type of optimisation_control_cgf") + diff --git a/rostok/trajectory_optimizer/control_optimizer.py b/rostok/trajectory_optimizer/control_optimizer.py index e3b5a02f..8f29368d 100644 --- a/rostok/trajectory_optimizer/control_optimizer.py +++ b/rostok/trajectory_optimizer/control_optimizer.py @@ -54,7 +54,7 @@ class BasePrepareOptiVar(): def __init__(self, each_control_params: Any, control_class: Type[RobotControllerChrono], - rewarder: SimulationReward, + rewarder: SimulationReward | None, params_start_pos=None): self.each_control_params = each_control_params self.control_class = control_class @@ -127,15 +127,23 @@ def reward_one_sim_scenario(self, x: list, graph: GraphGrammar, sim: Parametrize start_pos = self.build_starting_positions(graph) # pylint: disable=assignment-from-none is_vis = self.is_vis_decision(graph) and self.is_vis simout = sim.run_simulation(graph, control_data, start_pos, is_vis) - rew = self.rewarder.calculate_reward(simout) + rew = self.rewarder.calculate_reward(simout) return rew, x, sim + + def set_reward_fun(self, rewarder: SimulationReward): + """Set reward function. + + Args: + rewarder (SimulationReward): _description_ + """ + self.rewarder = rewarder class TendonForceOptiVar(BasePrepareOptiVar): def __init__(self, each_control_params: TendonControllerParameters, - rewarder: SimulationReward, + rewarder: SimulationReward | None = None, params_start_pos=None): super().__init__(each_control_params, TendonController_2p, rewarder, params_start_pos)