diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index ef0ef074..daa2c4f6 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -3,33 +3,49 @@ import hyperparameters as hp from rostok.control_chrono.tendon_controller import TendonController_2p, TendonControllerParameters -from rostok.criterion.criterion_calculation import ( - FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion, - InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward, - TimeCriterion) -from rostok.criterion.simulation_flags import (EventContactBuilder, - EventContactTimeOutBuilder, +from rostok.criterion.criterion_calculation import (FinalPositionCriterion, GraspTimeCriterion, + InstantContactingLinkCriterion, + InstantForceCriterion, + InstantObjectCOGCriterion, SimulationReward, + TimeCriterion) +from rostok.criterion.simulation_flags import (EventContactBuilder, EventContactTimeOutBuilder, EventFlyingApartBuilder, EventGraspBuilder, - EventSlipOutBuilder, - EventStopExternalForceBuilder) + EventSlipOutBuilder, EventStopExternalForceBuilder) from rostok.simulation_chrono.simulation_scenario import GraspScenario -from rostok.trajectory_optimizer.control_optimizer import ( - CalculatorWithOptimizationDirect, TendonOptimizerCombinationForce) +from rostok.trajectory_optimizer.control_optimizer import BruteForceOptimisation1D, ConstTorqueOptiVar, GlobalOptimisationEachSim, TendonForceOptiVar from rostok.utils.numeric_utils import Offset -from rostok.trajectory_optimizer.control_optimizer import (CalculatorWithGraphOptimization, - CalculatorWithOptimizationDirect) import rostok.control_chrono.external_force as f_ext +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 + + def config_independent_torque(grasp_object_blueprint): - # configurate the simulation manager + 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 event_contact_builder = EventContactBuilder() simulation_manager.add_event_builder(event_contact_builder) - event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT, event_contact_builder) + event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT, + event_contact_builder) simulation_manager.add_event_builder(event_timeout_builder) event_flying_apart_builder = EventFlyingApartBuilder(hp.FLAG_FLYING_APART) simulation_manager.add_event_builder(event_flying_apart_builder) @@ -37,19 +53,20 @@ def config_independent_torque(grasp_object_blueprint): simulation_manager.add_event_builder(event_slipout_builder) event_grasp_builder = EventGraspBuilder( grasp_limit_time=hp.GRASP_TIME, - contact_event_builder=event_contact_builder, + event_contact_builder=event_contact_builder, verbosity=0, ) simulation_manager.add_event_builder(event_grasp_builder) - event_stop_external_force = EventStopExternalForceBuilder(grasp_event_builder=event_grasp_builder, - force_test_time=hp.FORCE_TEST_TIME) + event_stop_external_force = EventStopExternalForceBuilder( + event_grasp_builder=event_grasp_builder, force_test_time=hp.FORCE_TEST_TIME) simulation_manager.add_event_builder(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_builder, event_grasp_builder), - hp.TIME_CRITERION_WEIGHT) + simulation_rewarder.add_criterion( + TimeCriterion(hp.GRASP_TIME, event_timeout_builder, event_grasp_builder), + hp.TIME_CRITERION_WEIGHT) #simulation_rewarder.add_criterion(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT) simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp_builder), hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT) @@ -65,11 +82,68 @@ def config_independent_torque(grasp_object_blueprint): FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp_builder, event_slipout_builder), hp.FINAL_POSITION_CRITERION_WEIGHT) - control_optimizer = CalculatorWithOptimizationDirect(simulation_manager, simulation_rewarder, - hp.CONTROL_OPTIMIZATION_BOUNDS, - hp.CONTROL_OPTIMIZATION_ITERATION) - return control_optimizer + const_optivar = ConstTorqueOptiVar(simulation_rewarder, -45) + direct_args = {"maxiter": 2} + global_const = GlobalOptimisationEachSim([simulation_manager], const_optivar, (0, 10), + direct_args) + + return global_const + + +def config_tendon(grasp_object_blueprint): + + simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p) + + simulation_manager.grasp_object_callback = grasp_object_blueprint + event_contact_builder = EventContactBuilder() + simulation_manager.add_event_builder(event_contact_builder) + event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT, + event_contact_builder) + simulation_manager.add_event_builder(event_timeout_builder) + event_flying_apart_builder = EventFlyingApartBuilder(hp.FLAG_FLYING_APART) + simulation_manager.add_event_builder(event_flying_apart_builder) + event_slipout_builder = EventSlipOutBuilder(hp.FLAG_TIME_SLIPOUT) + simulation_manager.add_event_builder(event_slipout_builder) + event_grasp_builder = EventGraspBuilder( + grasp_limit_time=hp.GRASP_TIME, + event_contact_builder=event_contact_builder, + verbosity=0, + ) + simulation_manager.add_event_builder(event_grasp_builder) + event_stop_external_force = EventStopExternalForceBuilder( + event_grasp_builder=event_grasp_builder, force_test_time=hp.FORCE_TEST_TIME) + simulation_manager.add_event_builder(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_builder, event_grasp_builder), + hp.TIME_CRITERION_WEIGHT) + #simulation_rewarder.add_criterion(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT) + simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp_builder), + hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT) + simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp_builder), + hp.INSTANT_FORCE_CRITERION_WEIGHT) + simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp_builder), + 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_builder, n_steps), + hp.GRASP_TIME_CRITERION_WEIGHT) + simulation_rewarder.add_criterion( + FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp_builder, event_slipout_builder), + hp.FINAL_POSITION_CRITERION_WEIGHT) + tendon_controller_cfg = get_tendon_cfg() + tendon_optivar = TendonForceOptiVar(tendon_controller_cfg, simulation_rewarder, -45) + tendon_optivar.is_vis = True + brute_tendon = BruteForceOptimisation1D(hp.TENDON_DISCRETE_FORCES, [simulation_manager], + tendon_optivar, + num_cpu_workers=1) + return brute_tendon + +""" def config_with_tendon(grasp_object_blueprint): # configurate the simulation manager @@ -203,4 +277,6 @@ def config_combination_force_tendon_multiobject_parallel(grasp_object_blueprint, control_optimizer = ParralelOptimizerCombinationForce(simulation_managers, simulation_rewarder, data, hp.TENDON_DISCRETE_FORCES, starting_finger_angles=-25) - return control_optimizer \ No newline at end of file + return control_optimizer + +""" \ No newline at end of file diff --git a/app/one_graph_simulation.py b/app/one_graph_simulation.py index ef51a5bf..be8e2635 100644 --- a/app/one_graph_simulation.py +++ b/app/one_graph_simulation.py @@ -1,31 +1,18 @@ -from mcts_run_setup import config_independent_torque, config_with_tendon +from mcts_run_setup import config_tendon from rostok.library.obj_grasp.objects import get_object_sphere from rostok.library.rule_sets.simple_designs import ( get_three_link_one_finger, get_three_link_one_finger_independent) -from rostok.simulation_chrono.simulation_utils import SimulationResult # create blueprint for object to grasp grasp_object_blueprint = get_object_sphere(0.05) # create reward counter using run setup function # control_optimizer = config_with_const_troques(grasp_object_blueprint) -control_optimizer = config_independent_torque(grasp_object_blueprint) -control_optimizer = config_with_tendon(grasp_object_blueprint) - -simulation_rewarder = control_optimizer.rewarder -simulation_manager = control_optimizer.simulation_scenario + +control_optimizer = config_tendon(grasp_object_blueprint) graph = get_three_link_one_finger_independent() graph = get_three_link_one_finger() -control = [10] - -data = control_optimizer.optim_parameters2data_control(control, graph) - -vis = True - -#simulation_output: SimulationResult = simulation_manager.run_simulation(graph, data, [[-45.0, 0.0],[-45,0],[-45,0]], vis, True) -simulation_output: SimulationResult = simulation_manager.run_simulation(graph, data, [[0.0, 0, 0]], vis, True) -res = simulation_rewarder.calculate_reward(simulation_output) -print('reward', res) +control_optimizer.calculate_reward(graph) \ No newline at end of file diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index 807a74e5..96b07c94 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -465,7 +465,7 @@ def event_check(self, current_time: float, step_n: int, robot_data, env_data): return EventCommands.CONTINUE -class EventStopExternalForceBuilder(): +class EventStopExternalForceBuilder(EventBuilder): def __init__(self, force_test_time: float, event_grasp_builder: EventGraspBuilder): super().__init__(event_class=EventStopExternalForce) self.event_grasp_builder = event_grasp_builder