diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index fa712b40..d76011ae 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -11,6 +11,7 @@ from rostok.criterion.simulation_flags import (EventContactBuilder, EventContactTimeOutBuilder, EventFlyingApartBuilder, EventGraspBuilder, EventSlipOutBuilder, EventStopExternalForceBuilder) +from rostok.criterion.simulation_flags import EventContact from rostok.simulation_chrono.simulation_scenario import GraspScenario from rostok.trajectory_optimizer.control_optimizer import BruteForceOptimisation1D, ConstTorqueOptiVar, GlobalOptimisationEachSim, TendonForceOptiVar from rostok.utils.numeric_utils import Offset @@ -92,7 +93,8 @@ def config_independent_torque(grasp_object_blueprint): def config_tendon(grasp_object_blueprint): obj_forces = [] - obj_forces.append(f_ext.RandomForces(1e6, 100, 20)) + obj_forces.append(f_ext.RandomForces(1e6, 100, 0)) + obj_forces[-1].set_activation_by_event(EventContact) 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) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 621b12f6..87b28b20 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -1,10 +1,12 @@ from abc import ABC, abstractmethod from dataclasses import dataclass import numpy as np -from typing import Any, Callable, List +from typing import Any, Callable, List, Optional import pychrono.core as chrono +from rostok.criterion.simulation_flags import SimulationSingleEvent + def random_3d_vector(amp): """Sample random 3d vector with given amplitude (uniform distribution on sphere) @@ -73,10 +75,24 @@ def __init__(self, self.name = name self.pos = pos self.start_time = start_time + self.start_event = None @abstractmethod - def calculate_spatial_force(self, time, data) -> np.ndarray: + def calculate_spatial_force(self, time, data, events) -> np.ndarray: return np.zeros(6) + + def set_activation_by_event(self, event: type[SimulationSingleEvent], use_time: bool = False): + self.start_event = event + self.start_time = self.start_time if use_time else np.inf + + def check_activation_force(self, time, events) -> bool: + time_activation = time >= self.start_time + event_activation = False + for ev in events: + if self.start_event is not None and isinstance(ev, self.start_event): + event_activation = ev.state + + return time_activation or event_activation def enable_data_dump(self, path): self.path = path @@ -92,7 +108,7 @@ class ForceChronoWrapper(): which determines the force and torque at time t. """ - def __init__(self, force: ABCForceCalculator, is_local: bool = False) -> None: + def __init__(self, force: ABCForceCalculator, events: Optional[list[SimulationSingleEvent]] = None, is_local: bool = False) -> None: self.path = None self.force = force @@ -116,11 +132,12 @@ def __init__(self, force: ABCForceCalculator, is_local: bool = False) -> None: self.torque_maker_chrono.SetNameString(name + "_torque") self.is_bound = False self.is_local = is_local + self.events = events self.setup_makers() def get_force_torque(self, time: float, data) -> ForceTorque: impact = ForceTorque() - spatial_force = self.force.calculate_spatial_force(time, data) + spatial_force = self.force.calculate_spatial_force(time, data, self.events) impact.force = tuple(spatial_force[3:]) impact.torque = tuple(spatial_force[:3]) return impact @@ -188,7 +205,7 @@ def __init__(self, super().__init__(name, start_time, pos) self.callback = callback - def calculate_spatial_force(self, time, data) -> np.ndarray: + def calculate_spatial_force(self, time, data, events) -> np.ndarray: return self.callback(time, data) @@ -213,9 +230,9 @@ def __init__(self, self.amp_offset = amp_offset self.freq = freq - def calculate_spatial_force(self, time, data) -> np.ndarray: + def calculate_spatial_force(self, time, data, events) -> np.ndarray: spatial_force = np.zeros(6) - if time >= self.start_time: + if self.check_activation_force(time, events): spatial_force[4] = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset return spatial_force @@ -232,9 +249,9 @@ def __init__(self, start_time: float = 0.0) -> None: """ super().__init__(name="null_gravity_force", start_time=start_time, pos=np.zeros(3)) - def calculate_spatial_force(self, time: float, data) -> np.ndarray: + def calculate_spatial_force(self, time: float, data, events) -> np.ndarray: spatial_force = np.zeros(6) - if time >= self.start_time: + if self.check_activation_force(time, events): mass = data.body_map_ordered[0].body.GetMass() g = data.grav_acc spatial_force[3:] = -mass * g @@ -266,9 +283,9 @@ def __init__(self, self.counter = 0 self.spatial_force = np.zeros(6) - def calculate_spatial_force(self, time: float, data) -> np.ndarray: + def calculate_spatial_force(self, time: float, data, events) -> np.ndarray: - if time >= self.start_time: + if self.check_activation_force(time, events): if self.counter % self.width_step == 0: if self.dim == '2d': self.spatial_force[3:] = random_plane_vector(self.amp, *self.angle) @@ -303,9 +320,9 @@ def __init__(self, self.angle: float = 0.0 self.angle_step: float = angle_step - def calculate_spatial_force(self, time: float, data) -> np.ndarray: + def calculate_spatial_force(self, time: float, data, events) -> np.ndarray: spatial_force = np.zeros(6) - if time >= self.start_time: + if self.check_activation_force(time, events): if self.counter % self.width_step == 0: self.angle += self.angle_step self.counter += 1 @@ -337,12 +354,12 @@ def add_force(self, force: ABCForceCalculator): else: self.force_controller = [self.force_controller, force] - def calculate_spatial_force(self, time: float, data) -> ForceTorque: + def calculate_spatial_force(self, time: float, data, events) -> ForceTorque: if isinstance(self.force_controller, list): v_forces = np.zeros(6) for controller in self.force_controller: - v_forces += np.array(controller.calculate_spatial_force(time, data)) + v_forces += np.array(controller.calculate_spatial_force(time, data, events)) self.data_forces.append(v_forces) return v_forces else: - return self.force_controller.calculate_spatial_force(time, data) \ No newline at end of file + return self.force_controller.calculate_spatial_force(time, data, events) \ No newline at end of file diff --git a/rostok/control_chrono/tendon_controller.py b/rostok/control_chrono/tendon_controller.py index 396c41f5..cb490be2 100644 --- a/rostok/control_chrono/tendon_controller.py +++ b/rostok/control_chrono/tendon_controller.py @@ -24,7 +24,7 @@ class ForceType(Enum): class PulleyForce(ABCForceCalculator): - def calculate_spatial_force(self, time: float, data) -> np.ndarray: + def calculate_spatial_force(self, time: float, data, events) -> np.ndarray: spatial_force = np.zeros(6) pre_point = data[0] point = data[1] @@ -43,7 +43,7 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: class TipForce(ABCForceCalculator): - def calculate_spatial_force(self, time: float, data) -> np.ndarray: + def calculate_spatial_force(self, time: float, data, events) -> np.ndarray: spatial_force = np.zeros(6) pre_point = data[0] point = data[1] diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index b0889bf3..1d2b1af8 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -93,7 +93,7 @@ def run_simulation(self, set_covering_ellipsoid_based_position(grasp_object, reference_point=chrono.ChVectorD(0, 0.1, 0)) if self.obj_external_forces: - chrono_forces = ForceChronoWrapper(deepcopy(self.obj_external_forces)) + chrono_forces = ForceChronoWrapper(deepcopy(self.obj_external_forces), event_list) else: chrono_forces = None simulation.env_creator.add_object(grasp_object,