From 808f14dd2891ac7c111e6050d83eb67e9597a963 Mon Sep 17 00:00:00 2001 From: Mikhail Chaikovskii Date: Wed, 1 Nov 2023 12:33:27 +0300 Subject: [PATCH 1/6] add event builders --- app/mcts_run_setup.py | 48 +++---- rostok/criterion/criterion_calculation.py | 78 ++++++----- rostok/criterion/simulation_flags.py | 121 ++++++++++++++++++ rostok/simulation_chrono/simulation.py | 3 +- .../simulation_chrono/simulation_scenario.py | 19 +-- rostok/simulation_chrono/simulation_utils.py | 2 + 6 files changed, 202 insertions(+), 69 deletions(-) diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index 5f6482e8..ef0ef074 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -7,11 +7,11 @@ FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion, InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward, TimeCriterion) -from rostok.criterion.simulation_flags import (EventContact, - EventContactTimeOut, - EventFlyingApart, EventGrasp, - EventSlipOut, - EventStopExternalForce) +from rostok.criterion.simulation_flags import (EventContactBuilder, + EventContactTimeOutBuilder, + EventFlyingApartBuilder, EventGraspBuilder, + EventSlipOutBuilder, + EventStopExternalForceBuilder) from rostok.simulation_chrono.simulation_scenario import GraspScenario from rostok.trajectory_optimizer.control_optimizer import ( CalculatorWithOptimizationDirect, TendonOptimizerCombinationForce) @@ -27,42 +27,42 @@ def config_independent_torque(grasp_object_blueprint): # simulation_manager.grasp_object_callback = lambda: creator.create_environment_body( # grasp_object_blueprint) simulation_manager.grasp_object_callback = grasp_object_blueprint - event_contact = EventContact() - simulation_manager.add_event(event_contact) - event_timeout = EventContactTimeOut(hp.FLAG_TIME_NO_CONTACT, event_contact) - simulation_manager.add_event(event_timeout) - event_flying_apart = EventFlyingApart(hp.FLAG_FLYING_APART) - simulation_manager.add_event(event_flying_apart) - event_slipout = EventSlipOut(hp.FLAG_TIME_SLIPOUT) - simulation_manager.add_event(event_slipout) - event_grasp = EventGrasp( + 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, - contact_event=event_contact, + contact_event_builder=event_contact_builder, verbosity=0, ) - simulation_manager.add_event(event_grasp) - event_stop_external_force = EventStopExternalForce(grasp_event=event_grasp, + 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) - simulation_manager.add_event(event_stop_external_force) + 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, event_grasp), + 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), + simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp_builder), hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT) - simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp), + simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp_builder), hp.INSTANT_FORCE_CRITERION_WEIGHT) - simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp), + 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, 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, event_slipout), + FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp_builder, event_slipout_builder), hp.FINAL_POSITION_CRITERION_WEIGHT) control_optimizer = CalculatorWithOptimizationDirect(simulation_manager, simulation_rewarder, diff --git a/rostok/criterion/criterion_calculation.py b/rostok/criterion/criterion_calculation.py index d07c1dc5..be8d4e86 100644 --- a/rostok/criterion/criterion_calculation.py +++ b/rostok/criterion/criterion_calculation.py @@ -5,8 +5,8 @@ import numpy as np from scipy.spatial import distance -from rostok.criterion.simulation_flags import (EventContactTimeOut, EventGrasp, - EventSlipOut) +from rostok.criterion.simulation_flags import (EventContactTimeOutBuilder, EventGraspBuilder, + EventSlipOutBuilder) from rostok.simulation_chrono.simulation_utils import SimulationResult from rostok.utils.json_encoder import RostokJSONEncoder @@ -40,10 +40,10 @@ class TimeCriterion(Criterion): event_grasp (EventGrasp): event of object grasping """ - def __init__(self, time: float, event_timeout: EventContactTimeOut, event_grasp: EventGrasp): + def __init__(self, time: float, event_timeout_builder: EventContactTimeOutBuilder, event_grasp_builder: EventGraspBuilder): self.max_simulation_time = time - self.event_timeout = event_timeout - self.event_grasp = event_grasp + self.event_timeout_builder = event_timeout_builder + self.event_grasp_builder = event_grasp_builder def calculate_reward(self, simulation_output: SimulationResult): """Return 1 for all robots that can grasp the object. @@ -53,10 +53,12 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.event_grasp.state: + event_container = simulation_output.event_container + event_grasp = self.event_grasp_builder.find_event(event_list=event_container) + if event_grasp.state: return 1 - - if self.event_timeout.state: + event_timeout=self.event_timeout_builder.find_event(event_list=event_container) + if event_timeout.state: return 0 else: time = simulation_output.time @@ -73,8 +75,8 @@ class ForceCriterion(Criterion): event_timeout (EventContactTimeOut): event of contact time out """ - def __init__(self, event_timeout: EventContactTimeOut): - self.event_timeout = event_timeout + def __init__(self, event_timeout_builder: EventContactTimeOutBuilder): + self.event_timeout_builder = event_timeout_builder def calculate_reward(self, simulation_output: SimulationResult) -> float: """Return 0 if there is no contact. For every step where object is in contact with robot @@ -84,8 +86,8 @@ def calculate_reward(self, simulation_output: SimulationResult) -> float: Returns: float: reward of the criterion """ - - if self.event_timeout.state: + event_timeout=self.event_timeout_builder.find_event(event_list=simulation_output.event_container) + if event_timeout.state: return 0 else: env_data = simulation_output.environment_final_ds @@ -115,8 +117,8 @@ class InstantObjectCOGCriterion(Criterion): event_grasp (EventGrasp): event of object grasping """ - def __init__(self, grasp_event: EventGrasp): - self.grasp_event = grasp_event + def __init__(self, grasp_event_builder: EventGraspBuilder): + self.grasp_event_builder = grasp_event_builder def calculate_reward(self, simulation_output: SimulationResult): """Calculate the reward based on distance between object COG and force centroid in the moment of grasp. @@ -124,10 +126,11 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.grasp_event.state: + grasp_event = self.grasp_event_builder.find_event(event_list=simulation_output.event_container) + if grasp_event.state: env_data = simulation_output.environment_final_ds - body_COG = env_data.get_data("COG")[0][self.grasp_event.step_n + 1] - body_outer_force_center = env_data.get_data("force_center")[0][self.grasp_event.step_n + + body_COG = env_data.get_data("COG")[0][grasp_event.step_n + 1] + body_outer_force_center = env_data.get_data("force_center")[0][grasp_event.step_n + 1] if body_outer_force_center is np.nan: return 0 @@ -144,8 +147,8 @@ class InstantForceCriterion(Criterion): event_grasp (EventGrasp): event of object grasping """ - def __init__(self, grasp_event: EventGrasp): - self.grasp_event = grasp_event + def __init__(self, grasp_event_builder: EventGraspBuilder): + self.grasp_event_builder = grasp_event_builder def calculate_reward(self, simulation_output: SimulationResult): """Calculate std of force modules in the grasp moment and calculate reward using it. @@ -153,9 +156,10 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.grasp_event.state: + grasp_event = self.grasp_event_builder.find_event(event_list=simulation_output.event_container) + if grasp_event.state: env_data = simulation_output.environment_final_ds - body_contacts: np.ndarray = env_data.get_data("forces")[0][self.grasp_event.step_n + 1] + body_contacts: np.ndarray = env_data.get_data("forces")[0][grasp_event.step_n + 1] if len(body_contacts) > 0: forces = [] for force in body_contacts: @@ -175,8 +179,8 @@ class InstantContactingLinkCriterion(Criterion): event_grasp (EventGrasp): event of object grasping """ - def __init__(self, grasp_event: EventGrasp): - self.grasp_event = grasp_event + def __init__(self, grasp_event_builder: EventGraspBuilder): + self.grasp_event_builder = grasp_event_builder def calculate_reward(self, simulation_output: SimulationResult): """The reward is the fraction of the links that contacts with object in the grasp moment. @@ -184,13 +188,14 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.grasp_event.state: + grasp_event = self.grasp_event_builder.find_event(event_list=simulation_output.event_container) + if grasp_event.state: robot_data = simulation_output.robot_final_ds robot_contacts = robot_data.get_data("n_contacts") n_bodies = len(robot_contacts.keys()) contacting_bodies = 0 for _, contacts in robot_contacts.items(): - if contacts[self.grasp_event.step_n + 1] > 0: + if contacts[grasp_event.step_n + 1] > 0: contacting_bodies += 1 return contacting_bodies / n_bodies @@ -206,8 +211,8 @@ class GraspTimeCriterion(Criterion): total_steps (total_steps): the amount of the possible steps """ - def __init__(self, grasp_event: EventGrasp, total_steps: int): - self.grasp_event = grasp_event + def __init__(self, grasp_event_builder: EventGraspBuilder, total_steps: int): + self.grasp_event_builder = grasp_event_builder self.total_steps = total_steps def calculate_reward(self, simulation_output: SimulationResult): @@ -216,8 +221,9 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.grasp_event.state: - return (self.total_steps - self.grasp_event.step_n) / self.total_steps + grasp_event = self.grasp_event_builder.find_event(event_list=simulation_output.event_container) + if grasp_event.state: + return (self.total_steps - grasp_event.step_n) / self.total_steps else: return 0 @@ -231,11 +237,11 @@ class FinalPositionCriterion(Criterion): slipout_event (EventSlipOut): event of object slip out """ - def __init__(self, reference_distance: float, grasp_event: EventGrasp, - slipout_event: EventSlipOut): + def __init__(self, reference_distance: float, grasp_event_builder: EventGraspBuilder, + slipout_event_builder: EventSlipOutBuilder): self.reference_distance = reference_distance - self.grasp_event = grasp_event - self.slipout_event = slipout_event + self.grasp_event_builder = grasp_event_builder + self.slipout_event_builder = slipout_event_builder def calculate_reward(self, simulation_output: SimulationResult): """The reward is 1 - dist(position in the grasp moment, position in the final moment)/(reference distance) @@ -243,9 +249,11 @@ def calculate_reward(self, simulation_output: SimulationResult): Returns: float: reward of the criterion """ - if self.grasp_event.state and not self.slipout_event.state: + grasp_event = self.grasp_event_builder.find_event(event_list=simulation_output.event_container) + slipout_event = self.slipout_event_builder.find_event(event_list=simulation_output.event_container) + if grasp_event.state and not slipout_event.state: env_data = simulation_output.environment_final_ds - grasp_pos = env_data.get_data("COG")[0][self.grasp_event.step_n + 1] + grasp_pos = env_data.get_data("COG")[0][grasp_event.step_n + 1] final_pos = env_data.get_data("COG")[0][-1] dist = distance.euclidean(grasp_pos, final_pos) if dist <= self.reference_distance: diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index 2a8c80d3..807a74e5 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -106,6 +106,33 @@ def event_check( return EventCommands.CONTINUE +class EventBuilder(): + def __init__(self, event_class): + self.even_class = event_class + + def find_event(self, event_list: list[SimulationSingleEvent]): + for event in event_list: + if isinstance(event, self.even_class): + return event + return None + @abstractmethod + def build_event(self, event_list): + pass + +class EventContactBuilder(EventBuilder): + def __init__(self, take_from_body: bool = False) -> None: + super().__init__(event_class=EventContact) + self.from_body = take_from_body + + def build_event(self, event_list) -> EventContact: + event = self.find_event(event_list=event_list) + if event is None: + return EventContact(take_from_body=self.from_body) + else: + raise Exception( + 'Attempt to create two same events for a simulation') + + class EventContactTimeOut(SimulationSingleEvent): """ Event that occurs if the robot doesn't contact with body during the reference time from the start of the simulation. @@ -149,6 +176,25 @@ def event_check( return EventCommands.CONTINUE +class EventContactTimeOutBuilder(EventBuilder): + def __init__(self, reference_time, event_contact_builder: EventContactBuilder) -> None: + super().__init__(event_class=EventContactTimeOut) + self.reference_time = reference_time + self.event_contact_builder = event_contact_builder + + def build_event(self, event_list) -> EventContactTimeOut: + event = self.find_event(event_list=event_list) + if event: + raise Exception( + 'Attempt to create two same events for a simulation: EventContactTimeOut') + contact_event = self.event_contact_builder.find_event( + event_list=event_list) + if contact_event is None: + raise Exception( + 'Event requires another event prebuilt: EventContactTimeOut <- EventContact') + return EventContactTimeOut(ref_time=self.reference_time, contact_event=contact_event) + + class EventFlyingApart(SimulationSingleEvent): """ The event that stops simulation if the robot parts have flown apart. @@ -191,6 +237,20 @@ def event_check( return EventCommands.CONTINUE +class EventFlyingApartBuilder(EventBuilder): + def __init__(self, max_distance) -> None: + super().__init__(event_class=EventFlyingApart) + self.max_distance = max_distance + + def build_event(self, event_list) -> EventFlyingApart: + event = self.find_event(event_list=event_list) + if event is None: + return EventFlyingApart(max_distance=self.max_distance) + else: + raise Exception( + 'Attempt to create two same events for a simulation: EventFlyingApart') + + class EventSlipOut(SimulationSingleEvent): """ The event that stops simulation if the body slips out from the grasp after the contact. @@ -248,6 +308,21 @@ def event_check( return EventCommands.CONTINUE +class EventSlipOutBuilder(EventBuilder): + + def __init__(self, ref_time): + super().__init__(event_class=EventSlipOut) + self.reference_time = ref_time + + def build_event(self, event_list) -> EventSlipOut: + event = self.find_event(event_list=event_list) + if event is None: + return EventSlipOut(ref_time=self.reference_time) + else: + raise Exception( + 'Attempt to create two same events for a simulation: EventSlipOut') + + class EventGrasp(SimulationSingleEvent): """ Event that activates the force if @@ -339,6 +414,33 @@ def event_check( return EventCommands.CONTINUE +class EventGraspBuilder(EventBuilder): + def __init__( + self, + event_contact_builder: EventContactBuilder, + grasp_limit_time: float, + verbosity: int = 0, + simulation_stop: bool = False, + ): + super().__init__(event_class=EventGrasp) + self.event_contact_builder = event_contact_builder + self.verbosity = verbosity + self.grasp_limit_time = grasp_limit_time + self.simulation_stop = simulation_stop + + def build_event(self, event_list) -> EventGrasp: + event = self.find_event(event_list=event_list) + if event: + raise Exception( + 'Attempt to create two same events for a simulation: EventGrasp') + contact_event = self.event_contact_builder.find_event( + event_list=event_list) + if contact_event is None: + raise Exception( + 'Event requires another event prebuilt: EventGrasp <- EventContact') + return EventGrasp(verbosity=self.verbosity, grasp_limit_time=self.grasp_limit_time, simulation_stop=self.simulation_stop, contact_event=contact_event) + + class EventStopExternalForce(SimulationSingleEvent): """ Event that controls external force and stops simulation after reference time has passed. @@ -361,3 +463,22 @@ def event_check(self, current_time: float, step_n: int, robot_data, env_data): return EventCommands.STOP return EventCommands.CONTINUE + + +class EventStopExternalForceBuilder(): + def __init__(self, force_test_time: float, event_grasp_builder: EventGraspBuilder): + super().__init__(event_class=EventStopExternalForce) + self.event_grasp_builder = event_grasp_builder + self.force_test_time = force_test_time + + def build_event(self, event_list) -> EventStopExternalForce: + event = self.find_event(event_list=event_list) + if event: + raise Exception( + 'Attempt to create two same events for a simulation: EventStopExternalForce') + grasp_event = self.event_grasp_builder.find_event( + event_list=event_list) + if grasp_event is None: + raise Exception( + 'Event requires another event prebuilt: EventStopExternalForce <- EventGrasp') + return EventStopExternalForce(force_test_time=self.force_test_time, grasp_event=grasp_event) diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index b320a456..052652cc 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -9,7 +9,7 @@ from rostok.control_chrono.control_utils import ForceTorqueContainer from rostok.control_chrono.controller import ConstController from rostok.control_chrono.external_force import ABCForceCalculator, ForceChronoWrapper -from rostok.criterion.simulation_flags import (EventCommands, SimulationSingleEvent) +from rostok.criterion.simulation_flags import (EventCommands) from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation_utils import SimulationResult from rostok.virtual_experiment.robot_new import BuiltGraphChrono, RobotChrono @@ -305,6 +305,7 @@ def simulate( self.result.environment_final_ds = self.env_creator.data_storage self.result.robot_final_ds = self.robot.data_storage self.result.time = self.chrono_system.GetChTime() + self.result.event_container = event_container self.n_steps = number_of_steps self.result.reduce_ending(i) return self.result diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index 9a66de2c..ca5afaf1 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -6,7 +6,7 @@ from rostok.control_chrono.controller import (ConstController, SinControllerChrono) from rostok.control_chrono.external_force import ForceChronoWrapper, ABCForceCalculator -from rostok.criterion.simulation_flags import SimulationSingleEvent +from rostok.criterion.simulation_flags import EventBuilder from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation import (ChronoSystems, EnvCreator, SingleRobotSimulation, ChronoVisManager) @@ -55,17 +55,18 @@ def __init__(self, obj_external_forces: Optional[ABCForceCalculator] = None) -> None: super().__init__(step_length, simulation_length) self.grasp_object_callback = None - self.event_container: List[SimulationSingleEvent] = [] + self.event_builder_container: List[EventBuilder] = [] self.controller_cls = controller_cls self.smc = smc self.obj_external_forces = obj_external_forces - def add_event(self, event): - self.event_container.append(event) + def add_event_builder(self, event_builder): + self.event_builder_container.append(event_builder) - def reset_events(self): - for event in self.event_container: - event.reset() + def build_events(self): + event_list=[] + for event_builder in self.event_builder_container: + event_builder.build_event(event_list) def run_simulation(self, graph: GraphGrammar, @@ -74,7 +75,7 @@ def run_simulation(self, vis=False, delay=False): # events should be reset before every simulation - self.reset_events() + event_list = self.build_events() # build simulation from the subclasses if self.smc: @@ -124,7 +125,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, event_list, vis) def get_scenario_name(self): return str(self.grasp_object_callback) \ No newline at end of file diff --git a/rostok/simulation_chrono/simulation_utils.py b/rostok/simulation_chrono/simulation_utils.py index 89d14b79..3fad5192 100644 --- a/rostok/simulation_chrono/simulation_utils.py +++ b/rostok/simulation_chrono/simulation_utils.py @@ -6,6 +6,7 @@ from rostok.block_builder_chrono.block_classes import ChronoEasyShapeObject from rostok.virtual_experiment.sensors import DataStorage +from rostok.criterion.simulation_flags import SimulationSingleEvent def calculate_covering_sphere(obj: ChronoEasyShapeObject): @@ -83,6 +84,7 @@ class SimulationResult: n_steps = 0 robot_final_ds: Optional[DataStorage] = None environment_final_ds: Optional[DataStorage] = None + event_container: List[SimulationSingleEvent] = field(default_factory=list) def reduce_ending(self, step_n): if self.robot_final_ds: From e2832334e9b77fbf391a20e5065195466536e98d Mon Sep 17 00:00:00 2001 From: KirillZharkov Date: Wed, 1 Nov 2023 14:29:38 +0300 Subject: [PATCH 2/6] 123 --- app/mcts_run_setup.py | 128 +++++++++++++++++++++------ app/one_graph_simulation.py | 21 +---- rostok/criterion/simulation_flags.py | 2 +- 3 files changed, 107 insertions(+), 44 deletions(-) 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 From 64c4bb451b98116a2b69330af12ca7742f3df82b Mon Sep 17 00:00:00 2001 From: Mikhail Chaikovskii Date: Wed, 1 Nov 2023 14:53:57 +0300 Subject: [PATCH 3/6] fix return --- app/hyperparameters.py | 2 +- rostok/criterion/simulation_flags.py | 14 +++++++------- rostok/simulation_chrono/simulation_scenario.py | 1 + 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/app/hyperparameters.py b/app/hyperparameters.py index 8193b570..7d59a99b 100644 --- a/app/hyperparameters.py +++ b/app/hyperparameters.py @@ -22,7 +22,7 @@ TENDON_CONST = -5 TENDON_DISCRETE_FORCES = [10, 15, 20] -TIME_STEP_SIMULATION = 0.00001 +TIME_STEP_SIMULATION = 0.0001 GRASP_TIME = 5 FORCE_TEST_TIME = 5 TIME_SIMULATION = 10 diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index 96b07c94..639e6128 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -127,10 +127,10 @@ def __init__(self, take_from_body: bool = False) -> None: def build_event(self, event_list) -> EventContact: event = self.find_event(event_list=event_list) if event is None: - return EventContact(take_from_body=self.from_body) + return event_list.append(EventContact(take_from_body=self.from_body)) else: raise Exception( - 'Attempt to create two same events for a simulation') + 'Attempt to create two same events for a simulation: EventContact') class EventContactTimeOut(SimulationSingleEvent): @@ -192,7 +192,7 @@ def build_event(self, event_list) -> EventContactTimeOut: if contact_event is None: raise Exception( 'Event requires another event prebuilt: EventContactTimeOut <- EventContact') - return EventContactTimeOut(ref_time=self.reference_time, contact_event=contact_event) + return event_list.append(EventContactTimeOut(ref_time=self.reference_time, contact_event=contact_event)) class EventFlyingApart(SimulationSingleEvent): @@ -245,7 +245,7 @@ def __init__(self, max_distance) -> None: def build_event(self, event_list) -> EventFlyingApart: event = self.find_event(event_list=event_list) if event is None: - return EventFlyingApart(max_distance=self.max_distance) + return event_list.append(EventFlyingApart(max_distance=self.max_distance)) else: raise Exception( 'Attempt to create two same events for a simulation: EventFlyingApart') @@ -317,7 +317,7 @@ def __init__(self, ref_time): def build_event(self, event_list) -> EventSlipOut: event = self.find_event(event_list=event_list) if event is None: - return EventSlipOut(ref_time=self.reference_time) + return event_list.append(EventSlipOut(ref_time=self.reference_time)) else: raise Exception( 'Attempt to create two same events for a simulation: EventSlipOut') @@ -438,7 +438,7 @@ def build_event(self, event_list) -> EventGrasp: if contact_event is None: raise Exception( 'Event requires another event prebuilt: EventGrasp <- EventContact') - return EventGrasp(verbosity=self.verbosity, grasp_limit_time=self.grasp_limit_time, simulation_stop=self.simulation_stop, contact_event=contact_event) + return event_list.append(EventGrasp(verbosity=self.verbosity, grasp_limit_time=self.grasp_limit_time, simulation_stop=self.simulation_stop, contact_event=contact_event)) class EventStopExternalForce(SimulationSingleEvent): @@ -481,4 +481,4 @@ def build_event(self, event_list) -> EventStopExternalForce: if grasp_event is None: raise Exception( 'Event requires another event prebuilt: EventStopExternalForce <- EventGrasp') - return EventStopExternalForce(force_test_time=self.force_test_time, grasp_event=grasp_event) + return event_list.append(EventStopExternalForce(force_test_time=self.force_test_time, grasp_event=grasp_event)) diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index ca5afaf1..b0889bf3 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -67,6 +67,7 @@ def build_events(self): event_list=[] for event_builder in self.event_builder_container: event_builder.build_event(event_list) + return event_list def run_simulation(self, graph: GraphGrammar, From 0382ba306d239e70fe1b63aeaa73d3708af4463a Mon Sep 17 00:00:00 2001 From: KirillZharkov Date: Wed, 1 Nov 2023 15:22:27 +0300 Subject: [PATCH 4/6] piymav na bug --- app/hyperparameters.py | 2 +- app/mcts_run_setup.py | 2 +- app/one_graph_simulation.py | 7 ++++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/app/hyperparameters.py b/app/hyperparameters.py index 7d59a99b..f85e70aa 100644 --- a/app/hyperparameters.py +++ b/app/hyperparameters.py @@ -22,7 +22,7 @@ TENDON_CONST = -5 TENDON_DISCRETE_FORCES = [10, 15, 20] -TIME_STEP_SIMULATION = 0.0001 +TIME_STEP_SIMULATION = 0.001 GRASP_TIME = 5 FORCE_TEST_TIME = 5 TIME_SIMULATION = 10 diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index daa2c4f6..8c24870d 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -136,7 +136,7 @@ def config_tendon(grasp_object_blueprint): 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 + tendon_optivar.is_vis = False brute_tendon = BruteForceOptimisation1D(hp.TENDON_DISCRETE_FORCES, [simulation_manager], tendon_optivar, num_cpu_workers=1) diff --git a/app/one_graph_simulation.py b/app/one_graph_simulation.py index be8e2635..ff21e0e7 100644 --- a/app/one_graph_simulation.py +++ b/app/one_graph_simulation.py @@ -2,7 +2,7 @@ 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) + get_three_link_one_finger, get_three_link_one_finger_independent, get_two_link_three_finger, ) # create blueprint for object to grasp grasp_object_blueprint = get_object_sphere(0.05) @@ -13,6 +13,7 @@ control_optimizer = config_tendon(grasp_object_blueprint) graph = get_three_link_one_finger_independent() -graph = get_three_link_one_finger() +graph = graph = get_two_link_three_finger() -control_optimizer.calculate_reward(graph) \ No newline at end of file +rewsss = control_optimizer.calculate_reward(graph) +pass \ No newline at end of file From 86b10a2a7171e4ecd97bb4c67587fc8cd5af8ad7 Mon Sep 17 00:00:00 2001 From: Mikhail Chaikovskii Date: Wed, 1 Nov 2023 15:42:02 +0300 Subject: [PATCH 5/6] fix activate --- rostok/simulation_chrono/simulation.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index 052652cc..bd713832 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -249,7 +249,8 @@ def simulate_step(self, step_length: float, current_time: float, step_n: int): self.env_creator.data_storage.sensor) def activate(self, current_time): - self.env_creator.force_torque_container.controller_list[0].start_time = current_time + if self.env_creator.force_torque_container.controller_list: + self.env_creator.force_torque_container.controller_list[0].start_time = current_time def handle_single_events(self, event_container, current_time, step_n): if event_container is None: From 110c8955867522e6b67411b254eb75eb7a3acc83 Mon Sep 17 00:00:00 2001 From: Mikhail Chaikovskii Date: Wed, 1 Nov 2023 19:54:37 +0300 Subject: [PATCH 6/6] simulation with external force --- app/mcts_run_setup.py | 9 ++++++--- app/one_graph_simulation.py | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index 8c24870d..fa712b40 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -91,8 +91,11 @@ def config_independent_torque(grasp_object_blueprint): def config_tendon(grasp_object_blueprint): - - simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p) + obj_forces = [] + obj_forces.append(f_ext.RandomForces(1e6, 100, 20)) + obj_forces.append(f_ext.NullGravity(0)) + obj_forces = f_ext.ExternalForces(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 event_contact_builder = EventContactBuilder() @@ -136,7 +139,7 @@ def config_tendon(grasp_object_blueprint): hp.FINAL_POSITION_CRITERION_WEIGHT) tendon_controller_cfg = get_tendon_cfg() tendon_optivar = TendonForceOptiVar(tendon_controller_cfg, simulation_rewarder, -45) - tendon_optivar.is_vis = False + tendon_optivar.is_vis = True brute_tendon = BruteForceOptimisation1D(hp.TENDON_DISCRETE_FORCES, [simulation_manager], tendon_optivar, num_cpu_workers=1) diff --git a/app/one_graph_simulation.py b/app/one_graph_simulation.py index ff21e0e7..110dfb2a 100644 --- a/app/one_graph_simulation.py +++ b/app/one_graph_simulation.py @@ -5,7 +5,7 @@ get_three_link_one_finger, get_three_link_one_finger_independent, get_two_link_three_finger, ) # create blueprint for object to grasp -grasp_object_blueprint = get_object_sphere(0.05) +grasp_object_blueprint = get_object_sphere(0.05, mass=0.2) # create reward counter using run setup function # control_optimizer = config_with_const_troques(grasp_object_blueprint)