From 0c14b29662c026cc6a8d02152694a47e51cb90f0 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Mon, 2 Oct 2023 11:27:14 +0300 Subject: [PATCH 01/16] external forces --- rostok/virtual_experiment/external_forces.py | 119 +++++++++++++++++++ 1 file changed, 119 insertions(+) create mode 100644 rostok/virtual_experiment/external_forces.py diff --git a/rostok/virtual_experiment/external_forces.py b/rostok/virtual_experiment/external_forces.py new file mode 100644 index 00000000..30e25de6 --- /dev/null +++ b/rostok/virtual_experiment/external_forces.py @@ -0,0 +1,119 @@ +from typing import Any, Dict, List, Tuple +from abc import abstractmethod +from dataclasses import dataclass, field + +import pychrono as chrono +import numpy as np +from matplotlib.pyplot import cla + +from rostok.control_chrono.controller import ForceTorque, ForceControllerTemplate + +def random_3d_vector(amp): + phi = np.random.uniform(0, 2*np.pi) + cos_theta = np.random.uniform(-1, 1) + sin_theta = np.sqrt(1 - cos_theta**2) + z_force = amp*cos_theta + y_force = amp*sin_theta*np.sin(phi) + x_force = amp*sin_theta*np.cos(phi) + return x_force, y_force, z_force + +def random_2d_vector(amp, angle: float = 0): + angle = np.random.uniform(0, 2*np.pi) + + el1 = np.cos(angle)*amp + el2 = np.sin(angle)*amp + + v1 = chrono.ChVectorD(el1, el2, 0) + + q1 = chrono.Q_from_AngZ(angle) + v1 = q1.Rotate(v1) + + return v1.x, v1.y, v1.z + + +class YaxisShaker(ForceControllerTemplate): + + def __init__(self, + amp: float = 5, + amp_offset: float = 1, + freq: float = 5, + start_time: float = 0.0) -> None: + super().__init__() + self.amp = amp + self.amp_offset = amp_offset + self.freq = freq + self.start_time = start_time + + def get_force_torque(self, time: float, data) -> ForceTorque: + impact = ForceTorque() + y_force = 0 + if time >= self.start_time: + y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset + impact.force = (0, y_force, 0) + return impact + + +class NullGravity(ForceControllerTemplate): + def __init__(self, gravitry_force, start_time: float = 0.0) -> None: + super().__init__() + self.gravity = gravitry_force + self.start_time = start_time + + def get_force_torque(self, time: float, data) -> ForceTorque: + impact = ForceTorque() + y_force = 0 + x_force = 0 + z_force = 0 + if time >= self.start_time: + y_force = -self.gravity + impact.force = (x_force, y_force, z_force) + return impact + +class RandomShaker(ForceControllerTemplate): + def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: + super().__init__() + self.start_time = start_time + self.width_step = width_step + self.amp = amp + self.counter = 0 + self.y_force = 0 + self.x_force = 0 + self.z_force = 0 + self.args = args + + def get_force_torque(self, time: float, data) -> ForceTorque: + impact = ForceTorque() + if time >= self.start_time: + if self.counter % self.width_step == 0: + if self.args and self.args[0] == '2d': + self.x_force, self.y_force, self.z_force = random_2d_vector(self.amp, self.args[1]) + else: + self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) + self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) + self.counter += 1 + impact.force = (self.x_force, self.y_force, self.z_force) + return impact + +class ClockXZShaker(ForceControllerTemplate): + def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: + super().__init__() + self.amp = amp + self.start_time = start_time + self.width_step = width_step + self.counter = 0 + self.angle = 0 + self.angle_step = angle_step + + def get_force_torque(self, time: float, data) -> ForceTorque: + impact = ForceTorque() + y_force = 0 + x_force = 0 + z_force = 0 + if time >= self.start_time: + if self.counter % self.width_step == 0: + self.angle += self.angle_step + self.counter += 1 + x_force = np.cos(self.angle_step)*self.amp + z_force = np.sin(self.angle_step)*self.amp + impact.force = (x_force, y_force, z_force) + return impact \ No newline at end of file From 7ee5b144339e94aaa3f368fa611d2b2485f00765 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Mon, 2 Oct 2023 11:48:18 +0300 Subject: [PATCH 02/16] Add API for external forces --- rostok/simulation_chrono/simulation_scenario.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index d0542bf7..1f283b4f 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -17,6 +17,7 @@ SensorObjectClassification) from rostok.block_builder_chrono.block_builder_chrono_api import \ ChronoBlockCreatorInterface as creator +from rostok.control_chrono.controller import ForceControllerTemplate #from rostok.control_chrono.tendon_controller import TendonController_2p class ParametrizedSimulation: @@ -39,12 +40,13 @@ def __str__(self) -> str: class GraspScenario(ParametrizedSimulation): - def __init__(self, step_length, simulation_length, tendon = True, smc = False) -> None: + def __init__(self, step_length, simulation_length, tendon = True, smc = False, obj_external_forces: Optional[ForceControllerTemplate] = None) -> None: super().__init__(step_length, simulation_length) self.grasp_object_callback = None self.event_container: List[SimulationSingleEvent] = [] self.tendon = tendon self.smc = smc + self.obj_external_forces = obj_external_forces def add_event(self, event): self.event_container.append(event) @@ -73,7 +75,7 @@ def run_simulation(self, graph: GraphGrammar, data, starting_positions = None, v reference_point=chrono.ChVectorD(0, 0.1, 0)) simulation.env_creator.add_object(grasp_object, read_data=True, - force_torque_controller=None) + force_torque_controller=self.obj_external_forces) # add design and determine the outer force if self.tendon: From d69dc0b98d9cb3ddb8f7f2d67f7b213186f2eed6 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Mon, 2 Oct 2023 12:21:21 +0300 Subject: [PATCH 03/16] Add external forces class --- rostok/control_chrono/controller.py | 8 +++- rostok/virtual_experiment/external_forces.py | 50 ++++++++++++-------- 2 files changed, 38 insertions(+), 20 deletions(-) diff --git a/rostok/control_chrono/controller.py b/rostok/control_chrono/controller.py index a7a9411c..b438462e 100644 --- a/rostok/control_chrono/controller.py +++ b/rostok/control_chrono/controller.py @@ -120,8 +120,14 @@ def __init__(self) -> None: self.setup_makers() @abstractmethod - def get_force_torque(self, time: float, data) -> ForceTorque: + def calculate_impact(self, time, *args) -> tuple[float, float, float]: pass + + + def get_force_torque(self, time: float, data, *args) -> ForceTorque: + impact = ForceTorque() + impact.force = self.calculate_impact(time, *args) + return impact def update(self, time: float, data=None): force_torque = self.get_force_torque(time, data) diff --git a/rostok/virtual_experiment/external_forces.py b/rostok/virtual_experiment/external_forces.py index 30e25de6..85d060ca 100644 --- a/rostok/virtual_experiment/external_forces.py +++ b/rostok/virtual_experiment/external_forces.py @@ -43,15 +43,12 @@ def __init__(self, self.amp_offset = amp_offset self.freq = freq self.start_time = start_time - - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + + def calculate_impact(self, time, *args) -> tuple[float, float, float]: y_force = 0 if time >= self.start_time: - y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset - impact.force = (0, y_force, 0) - return impact - + y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset + return 0, y_force, 0 class NullGravity(ForceControllerTemplate): def __init__(self, gravitry_force, start_time: float = 0.0) -> None: @@ -59,15 +56,13 @@ def __init__(self, gravitry_force, start_time: float = 0.0) -> None: self.gravity = gravitry_force self.start_time = start_time - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + def calculate_impact(self, time: float, data) -> ForceTorque: y_force = 0 x_force = 0 z_force = 0 if time >= self.start_time: y_force = -self.gravity - impact.force = (x_force, y_force, z_force) - return impact + return x_force, y_force, z_force class RandomShaker(ForceControllerTemplate): def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: @@ -81,8 +76,7 @@ def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *a self.z_force = 0 self.args = args - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + def get_force_torque(self, time: float, data) -> tuple[float, float, float]: if time >= self.start_time: if self.counter % self.width_step == 0: if self.args and self.args[0] == '2d': @@ -91,8 +85,7 @@ def get_force_torque(self, time: float, data) -> ForceTorque: self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) self.counter += 1 - impact.force = (self.x_force, self.y_force, self.z_force) - return impact + return self.x_force, self.y_force, self.z_force class ClockXZShaker(ForceControllerTemplate): def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: @@ -104,8 +97,7 @@ def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = self.angle = 0 self.angle_step = angle_step - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + def get_force_torque(self, time: float, data) -> tuple[float, float, float]: y_force = 0 x_force = 0 z_force = 0 @@ -115,5 +107,25 @@ def get_force_torque(self, time: float, data) -> ForceTorque: self.counter += 1 x_force = np.cos(self.angle_step)*self.amp z_force = np.sin(self.angle_step)*self.amp - impact.force = (x_force, y_force, z_force) - return impact \ No newline at end of file + return x_force, y_force, z_force + +class ExternalForces(ForceControllerTemplate): + def __init__(self, force_controller: ForceControllerTemplate | List[ForceControllerTemplate]) -> None: + self.force_controller = force_controller + + def add_force(self, force: ForceControllerTemplate): + if isinstance(self.force_controller, list): + self.force_controller.append(force) + else: + self.force_controller = [self.force_controller, force] + + def get_force_torque(self, time: float, data) -> ForceTorque: + if isinstance(self.force_controller, list): + v_forces = np.zeros(3) + for controller in self.force_controller: + v_forces += np.array(controller.calculate_impact(time, data)) + impact = ForceTorque() + impact.force = v_forces + return impact + else: + return self.force_controller.get_force_torque(time, data) \ No newline at end of file From 47c03a59d61bfc199270c77a0ca213d407bbd675 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Mon, 2 Oct 2023 16:32:35 +0300 Subject: [PATCH 04/16] Add comments --- rostok/virtual_experiment/external_forces.py | 57 ++++++++++++++++++-- 1 file changed, 54 insertions(+), 3 deletions(-) diff --git a/rostok/virtual_experiment/external_forces.py b/rostok/virtual_experiment/external_forces.py index 85d060ca..a298fcbe 100644 --- a/rostok/virtual_experiment/external_forces.py +++ b/rostok/virtual_experiment/external_forces.py @@ -9,6 +9,14 @@ from rostok.control_chrono.controller import ForceTorque, ForceControllerTemplate def random_3d_vector(amp): + """Calculate random 3d vector with given amplitude (uniform distribution on sphere) + + Args: + amp (float): amplitude of vector + + Returns: + tuple: x, y, z components of vector + """ phi = np.random.uniform(0, 2*np.pi) cos_theta = np.random.uniform(-1, 1) sin_theta = np.sqrt(1 - cos_theta**2) @@ -18,6 +26,15 @@ def random_3d_vector(amp): return x_force, y_force, z_force def random_2d_vector(amp, angle: float = 0): + """Calculate random 2d vector with given amplitude (uniform distribution on circle) + + Args: + amp (float): amplitude of vector + angle (float, optional): angle along axis z of vector. Defaults to 0. + + Returns: + tuple: x, y, z components of vector + """ angle = np.random.uniform(0, 2*np.pi) el1 = np.cos(angle)*amp @@ -31,13 +48,21 @@ def random_2d_vector(amp, angle: float = 0): return v1.x, v1.y, v1.z -class YaxisShaker(ForceControllerTemplate): +class YaxisSin(ForceControllerTemplate): def __init__(self, amp: float = 5, amp_offset: float = 1, freq: float = 5, start_time: float = 0.0) -> None: + """Shake by sin along y axis + + Args: + amp (float, optional): Amplitude of sin. Defaults to 5. + amp_offset (float, optional): Amplitude offset of force. Defaults to 1. + freq (float, optional): Frequency of sin. Defaults to 5. + start_time (float, optional): Start time of force application. Defaults to 0.0. + """ super().__init__() self.amp = amp self.amp_offset = amp_offset @@ -52,6 +77,12 @@ def calculate_impact(self, time, *args) -> tuple[float, float, float]: class NullGravity(ForceControllerTemplate): def __init__(self, gravitry_force, start_time: float = 0.0) -> None: + """Apply force to compensate gravity + + Args: + gravitry_force (float): gravity force of object + start_time (float, optional): start time of force application. Defaults to 0.0. + """ super().__init__() self.gravity = gravitry_force self.start_time = start_time @@ -64,8 +95,15 @@ def calculate_impact(self, time: float, data) -> ForceTorque: y_force = -self.gravity return x_force, y_force, z_force -class RandomShaker(ForceControllerTemplate): +class RandomForces(ForceControllerTemplate): def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: + """Apply force with random direction and given amplitude + + Args: + amp (float): amplitude of force + start_time (float, optional): Start time of force application. Defaults to 0.0. + width_step (int, optional): Number of steps between changes of force direction. Defaults to 20. + """ super().__init__() self.start_time = start_time self.width_step = width_step @@ -87,8 +125,16 @@ def get_force_torque(self, time: float, data) -> tuple[float, float, float]: self.counter += 1 return self.x_force, self.y_force, self.z_force -class ClockXZShaker(ForceControllerTemplate): +class ClockXZForces(ForceControllerTemplate): def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: + """Apply force with given amplitude in xz plane and rotate it with given angle step + + Args: + amp (float): amplitude of force + angle_step (float, optional): Size of angle for changing force direction. Defaults to np.pi/6. + start_time (float, optional): Start time of force application. Defaults to 0.0. + width_step (int, optional): _description_. Defaults to 20. + """ super().__init__() self.amp = amp self.start_time = start_time @@ -111,6 +157,11 @@ def get_force_torque(self, time: float, data) -> tuple[float, float, float]: class ExternalForces(ForceControllerTemplate): def __init__(self, force_controller: ForceControllerTemplate | List[ForceControllerTemplate]) -> None: + """Class for combining several external forces + + Args: + force_controller (ForceControllerTemplate | List[ForceControllerTemplate]): Forces or list of forces + """ self.force_controller = force_controller def add_force(self, force: ForceControllerTemplate): From aeb8cc862478fb383f375e3874fc38057e86b669 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Tue, 3 Oct 2023 11:25:44 +0300 Subject: [PATCH 05/16] format --- .../simulation_chrono/simulation_scenario.py | 40 ++++++++++++------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index 1f283b4f..12db690e 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -4,22 +4,21 @@ import numpy as np import pychrono as chrono -from rostok.control_chrono.controller import (ConstController, - SinControllerChrono, YaxisShaker) +from rostok.control_chrono.controller import (ConstController, SinControllerChrono, YaxisShaker) from rostok.criterion.simulation_flags import SimulationSingleEvent from rostok.graph_grammar.node import GraphGrammar -from rostok.simulation_chrono.simulation import (ChronoSystems, EnvCreator, - SingleRobotSimulation, ChronoVisManager) +from rostok.simulation_chrono.simulation import (ChronoSystems, EnvCreator, SingleRobotSimulation, + ChronoVisManager) from rostok.simulation_chrono.simulation_utils import \ set_covering_sphere_based_position, set_covering_ellipsoid_based_position from rostok.utils.json_encoder import RostokJSONEncoder -from rostok.virtual_experiment.sensors import (SensorCalls, - SensorObjectClassification) +from rostok.virtual_experiment.sensors import (SensorCalls, SensorObjectClassification) from rostok.block_builder_chrono.block_builder_chrono_api import \ ChronoBlockCreatorInterface as creator from rostok.control_chrono.controller import ForceControllerTemplate #from rostok.control_chrono.tendon_controller import TendonController_2p + class ParametrizedSimulation: def __init__(self, step_length, simulation_length): @@ -40,7 +39,12 @@ def __str__(self) -> str: class GraspScenario(ParametrizedSimulation): - def __init__(self, step_length, simulation_length, tendon = True, smc = False, obj_external_forces: Optional[ForceControllerTemplate] = None) -> None: + def __init__(self, + step_length, + simulation_length, + tendon=True, + smc=False, + obj_external_forces: Optional[ForceControllerTemplate] = None) -> None: super().__init__(step_length, simulation_length) self.grasp_object_callback = None self.event_container: List[SimulationSingleEvent] = [] @@ -55,31 +59,39 @@ def reset_events(self): for event in self.event_container: event.reset() - def run_simulation(self, graph: GraphGrammar, data, starting_positions = None, vis=False, delay=False): + def run_simulation(self, + graph: GraphGrammar, + data, + starting_positions=None, + vis=False, + delay=False): # events should be reset before every simulation self.reset_events() # build simulation from the subclasses - + if self.smc: system = ChronoSystems.chrono_SMC_system(gravity_list=[0, 0, 0]) else: system = ChronoSystems.chrono_NSC_system(gravity_list=[0, 0, 0]) - # setup the auxiliary + # setup the auxiliary env_creator = EnvCreator([]) vis_manager = ChronoVisManager(delay) simulation = SingleRobotSimulation(system, env_creator, vis_manager) - + grasp_object = creator.create_environment_body(self.grasp_object_callback) grasp_object.body.SetNameString("Grasp_object") set_covering_ellipsoid_based_position(grasp_object, - reference_point=chrono.ChVectorD(0, 0.1, 0)) + reference_point=chrono.ChVectorD(0, 0.1, 0)) simulation.env_creator.add_object(grasp_object, read_data=True, force_torque_controller=self.obj_external_forces) - + # add design and determine the outer force if self.tendon: - simulation.add_design(graph, data, TendonController_2p, starting_positions=starting_positions) + simulation.add_design(graph, + data, + TendonController_2p, + starting_positions=starting_positions) else: simulation.add_design(graph, data, starting_positions=starting_positions) # setup parameters for the data store From faf976d582967cbd63cb0aadc0fef1ae135ace86 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Tue, 3 Oct 2023 11:35:41 +0300 Subject: [PATCH 06/16] Add deepcopy for external forces --- rostok/simulation_chrono/simulation_scenario.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index 12db690e..73505af1 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -1,3 +1,4 @@ +from copy import deepcopy import json from typing import Dict, List, Optional, Tuple @@ -84,7 +85,7 @@ def run_simulation(self, reference_point=chrono.ChVectorD(0, 0.1, 0)) simulation.env_creator.add_object(grasp_object, read_data=True, - force_torque_controller=self.obj_external_forces) + force_torque_controller=deepcopy(self.obj_external_forces)) # add design and determine the outer force if self.tendon: From 6ec303bd5454be86b5a89489e2bc7caf98b9a11d Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Thu, 5 Oct 2023 14:01:22 +0300 Subject: [PATCH 07/16] working null gravity --- rostok/control_chrono/external_force.py | 181 ++++++++++++++++-- rostok/virtual_experiment/external_forces.py | 182 ------------------- rostok/virtual_experiment/sensors.py | 2 + 3 files changed, 169 insertions(+), 196 deletions(-) delete mode 100644 rostok/virtual_experiment/external_forces.py diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index e3c442b0..1be0bc16 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -1,10 +1,48 @@ from abc import abstractmethod from dataclasses import dataclass -from math import sin -from typing import Any, Callable +import numpy as np +from typing import Any, Callable, List import pychrono.core as chrono +def random_3d_vector(amp): + """Calculate random 3d vector with given amplitude (uniform distribution on sphere) + + Args: + amp (float): amplitude of vector + + Returns: + tuple: x, y, z components of vector + """ + phi = np.random.uniform(0, 2*np.pi) + cos_theta = np.random.uniform(-1, 1) + sin_theta = np.sqrt(1 - cos_theta**2) + z_force = amp*cos_theta + y_force = amp*sin_theta*np.sin(phi) + x_force = amp*sin_theta*np.cos(phi) + return x_force, y_force, z_force + +def random_2d_vector(amp, angle: float = 0): + """Calculate random 2d vector with given amplitude (uniform distribution on circle) + + Args: + amp (float): amplitude of vector + angle (float, optional): angle along axis z of vector. Defaults to 0. + + Returns: + tuple: x, y, z components of vector + """ + angle = np.random.uniform(0, 2*np.pi) + + el1 = np.cos(angle)*amp + el2 = np.sin(angle)*amp + + v1 = chrono.ChVectorD(el1, el2, 0) + + q1 = chrono.Q_from_AngZ(angle) + v1 = q1.Rotate(v1) + + return v1.x, v1.y, v1.z @dataclass class ForceTorque: @@ -52,8 +90,14 @@ def __init__(self, self.is_local = is_local self.setup_makers() - @abstractmethod + def get_force_torque(self, time: float, data) -> ForceTorque: + impact = ForceTorque() + impact.force = self.calculate_impact(time, data) + return impact + + @abstractmethod + def calculate_impact(self, time: float, data) -> tuple[float, float, float]: pass def update(self, time: float, data=None): @@ -111,33 +155,142 @@ def enable_data_dump(self, path): class ForceControllerOnCallback(ForceControllerTemplate): def __init__(self, callback: CALLBACK_TYPE) -> None: - super().__init__() + super().__init__(name="callback_force") self.callback = callback def get_force_torque(self, time: float, data) -> ForceTorque: return self.callback(time, data) -class YaxisShaker(ForceControllerTemplate): +class YaxisSin(ForceControllerTemplate): def __init__(self, - name: str = 'unnamed', - pos: list[float] = [0, 0, 0], - is_local=False, amp: float = 5, amp_offset: float = 1, freq: float = 5, start_time: float = 0.0) -> None: - super().__init__(is_local=is_local, name=name, pos=pos) + """Shake by sin along y axis + + Args: + amp (float, optional): Amplitude of sin. Defaults to 5. + amp_offset (float, optional): Amplitude offset of force. Defaults to 1. + freq (float, optional): Frequency of sin. Defaults to 5. + start_time (float, optional): Start time of force application. Defaults to 0.0. + """ + super().__init__(name="y_sin_force") self.amp = amp self.amp_offset = amp_offset self.freq = freq self.start_time = start_time + + def calculate_impact(self, time, data) -> tuple[float, float, float]: + y_force = 0 + if time >= self.start_time: + y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset + return 0, y_force, 0 - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() +class NullGravity(ForceControllerTemplate): + def __init__(self, start_time: float = 0.0) -> None: + """Apply force to compensate gravity + + Args: + gravitry_force (float): gravity force of object + start_time (float, optional): start time of force application. Defaults to 0.0. + """ + super().__init__(name="null_gravity_force") + self.start_time = start_time + + def calculate_impact(self, time: float, data) -> tuple[float, float, float]: + force_g = np.zeros(3) + if time >= self.start_time: + mass = data.body_map_ordered[0].body.GetMass() + g = data.grav_acc + force_g = -mass*g + return force_g[0], force_g[1], force_g[2] + +class RandomForces(ForceControllerTemplate): + def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: + """Apply force with random direction and given amplitude + + Args: + amp (float): amplitude of force + start_time (float, optional): Start time of force application. Defaults to 0.0. + width_step (int, optional): Number of steps between changes of force direction. Defaults to 20. + """ + super().__init__(name="random_force") + self.start_time = start_time + self.width_step = width_step + self.amp = amp + self.counter = 0 + self.y_force = 0 + self.x_force = 0 + self.z_force = 0 + self.args = args + + def calculate_impact(self, time: float, data) -> tuple[float, float, float]: + if time >= self.start_time: + if self.counter % self.width_step == 0: + if self.args and self.args[0] == '2d': + self.x_force, self.y_force, self.z_force = random_2d_vector(self.amp, self.args[1]) + else: + self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) + self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) + self.counter += 1 + return self.x_force, self.y_force, self.z_force + +class ClockXZForces(ForceControllerTemplate): + def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: + """Apply force with given amplitude in xz plane and rotate it with given angle step + + Args: + amp (float): amplitude of force + angle_step (float, optional): Size of angle for changing force direction. Defaults to np.pi/6. + start_time (float, optional): Start time of force application. Defaults to 0.0. + width_step (int, optional): _description_. Defaults to 20. + """ + super().__init__(name="clock_xz_force") + self.amp = amp + self.start_time = start_time + self.width_step = width_step + self.counter: int = 0 + self.angle: float = 0.0 + self.angle_step: float = angle_step + + def calculate_impact(self, time: float, data) -> tuple[float, float, float]: y_force = 0 + x_force = 0 + z_force = 0 if time >= self.start_time: - y_force = self.amp * sin(self.freq * (time - self.start_time)) + self.amp_offset - impact.force = (0, y_force, 0) - return impact + if self.counter % self.width_step == 0: + self.angle += self.angle_step + self.counter += 1 + x_force = np.cos(self.angle_step)*self.amp + z_force = np.sin(self.angle_step)*self.amp + return x_force, y_force, z_force + +class ExternalForces(ForceControllerTemplate): + def __init__(self, force_controller: ForceControllerTemplate | List[ForceControllerTemplate]) -> None: + """Class for combining several external forces + + Args: + force_controller (ForceControllerTemplate | List[ForceControllerTemplate]): Forces or list of forces + """ + super().__init__(name="external_forces") + self.force_controller = force_controller + + def add_force(self, force: ForceControllerTemplate): + if isinstance(self.force_controller, list): + self.force_controller.append(force) + else: + self.force_controller = [self.force_controller, force] + + def get_force_torque(self, time: float, data) -> ForceTorque: + if isinstance(self.force_controller, list): + v_forces = np.zeros(3) + for controller in self.force_controller: + v_forces += np.array(controller.calculate_impact(time, data)) + impact = ForceTorque() + impact.force = tuple(v_forces) + return impact + else: + return self.force_controller.get_force_torque(time, data) \ No newline at end of file diff --git a/rostok/virtual_experiment/external_forces.py b/rostok/virtual_experiment/external_forces.py deleted file mode 100644 index a298fcbe..00000000 --- a/rostok/virtual_experiment/external_forces.py +++ /dev/null @@ -1,182 +0,0 @@ -from typing import Any, Dict, List, Tuple -from abc import abstractmethod -from dataclasses import dataclass, field - -import pychrono as chrono -import numpy as np -from matplotlib.pyplot import cla - -from rostok.control_chrono.controller import ForceTorque, ForceControllerTemplate - -def random_3d_vector(amp): - """Calculate random 3d vector with given amplitude (uniform distribution on sphere) - - Args: - amp (float): amplitude of vector - - Returns: - tuple: x, y, z components of vector - """ - phi = np.random.uniform(0, 2*np.pi) - cos_theta = np.random.uniform(-1, 1) - sin_theta = np.sqrt(1 - cos_theta**2) - z_force = amp*cos_theta - y_force = amp*sin_theta*np.sin(phi) - x_force = amp*sin_theta*np.cos(phi) - return x_force, y_force, z_force - -def random_2d_vector(amp, angle: float = 0): - """Calculate random 2d vector with given amplitude (uniform distribution on circle) - - Args: - amp (float): amplitude of vector - angle (float, optional): angle along axis z of vector. Defaults to 0. - - Returns: - tuple: x, y, z components of vector - """ - angle = np.random.uniform(0, 2*np.pi) - - el1 = np.cos(angle)*amp - el2 = np.sin(angle)*amp - - v1 = chrono.ChVectorD(el1, el2, 0) - - q1 = chrono.Q_from_AngZ(angle) - v1 = q1.Rotate(v1) - - return v1.x, v1.y, v1.z - - -class YaxisSin(ForceControllerTemplate): - - def __init__(self, - amp: float = 5, - amp_offset: float = 1, - freq: float = 5, - start_time: float = 0.0) -> None: - """Shake by sin along y axis - - Args: - amp (float, optional): Amplitude of sin. Defaults to 5. - amp_offset (float, optional): Amplitude offset of force. Defaults to 1. - freq (float, optional): Frequency of sin. Defaults to 5. - start_time (float, optional): Start time of force application. Defaults to 0.0. - """ - super().__init__() - self.amp = amp - self.amp_offset = amp_offset - self.freq = freq - self.start_time = start_time - - def calculate_impact(self, time, *args) -> tuple[float, float, float]: - y_force = 0 - if time >= self.start_time: - y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset - return 0, y_force, 0 - -class NullGravity(ForceControllerTemplate): - def __init__(self, gravitry_force, start_time: float = 0.0) -> None: - """Apply force to compensate gravity - - Args: - gravitry_force (float): gravity force of object - start_time (float, optional): start time of force application. Defaults to 0.0. - """ - super().__init__() - self.gravity = gravitry_force - self.start_time = start_time - - def calculate_impact(self, time: float, data) -> ForceTorque: - y_force = 0 - x_force = 0 - z_force = 0 - if time >= self.start_time: - y_force = -self.gravity - return x_force, y_force, z_force - -class RandomForces(ForceControllerTemplate): - def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: - """Apply force with random direction and given amplitude - - Args: - amp (float): amplitude of force - start_time (float, optional): Start time of force application. Defaults to 0.0. - width_step (int, optional): Number of steps between changes of force direction. Defaults to 20. - """ - super().__init__() - self.start_time = start_time - self.width_step = width_step - self.amp = amp - self.counter = 0 - self.y_force = 0 - self.x_force = 0 - self.z_force = 0 - self.args = args - - def get_force_torque(self, time: float, data) -> tuple[float, float, float]: - if time >= self.start_time: - if self.counter % self.width_step == 0: - if self.args and self.args[0] == '2d': - self.x_force, self.y_force, self.z_force = random_2d_vector(self.amp, self.args[1]) - else: - self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) - self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) - self.counter += 1 - return self.x_force, self.y_force, self.z_force - -class ClockXZForces(ForceControllerTemplate): - def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: - """Apply force with given amplitude in xz plane and rotate it with given angle step - - Args: - amp (float): amplitude of force - angle_step (float, optional): Size of angle for changing force direction. Defaults to np.pi/6. - start_time (float, optional): Start time of force application. Defaults to 0.0. - width_step (int, optional): _description_. Defaults to 20. - """ - super().__init__() - self.amp = amp - self.start_time = start_time - self.width_step = width_step - self.counter = 0 - self.angle = 0 - self.angle_step = angle_step - - def get_force_torque(self, time: float, data) -> tuple[float, float, float]: - y_force = 0 - x_force = 0 - z_force = 0 - if time >= self.start_time: - if self.counter % self.width_step == 0: - self.angle += self.angle_step - self.counter += 1 - x_force = np.cos(self.angle_step)*self.amp - z_force = np.sin(self.angle_step)*self.amp - return x_force, y_force, z_force - -class ExternalForces(ForceControllerTemplate): - def __init__(self, force_controller: ForceControllerTemplate | List[ForceControllerTemplate]) -> None: - """Class for combining several external forces - - Args: - force_controller (ForceControllerTemplate | List[ForceControllerTemplate]): Forces or list of forces - """ - self.force_controller = force_controller - - def add_force(self, force: ForceControllerTemplate): - if isinstance(self.force_controller, list): - self.force_controller.append(force) - else: - self.force_controller = [self.force_controller, force] - - def get_force_torque(self, time: float, data) -> ForceTorque: - if isinstance(self.force_controller, list): - v_forces = np.zeros(3) - for controller in self.force_controller: - v_forces += np.array(controller.calculate_impact(time, data)) - impact = ForceTorque() - impact.force = v_forces - return impact - else: - return self.force_controller.get_force_torque(time, data) \ No newline at end of file diff --git a/rostok/virtual_experiment/sensors.py b/rostok/virtual_experiment/sensors.py index d43b2ff5..85002158 100644 --- a/rostok/virtual_experiment/sensors.py +++ b/rostok/virtual_experiment/sensors.py @@ -98,9 +98,11 @@ def __init__(self, body_map_ordered, joint_map_ordered) -> None: self.contact_reporter.set_body_map(body_map_ordered) self.body_map_ordered: Dict[int, Any] = body_map_ordered self.joint_map_ordered: Dict[int, Any] = joint_map_ordered + self.grav_acc: np.ndarray = np.array([0, -9.8, 0]) def update_current_contact_info(self, system: chrono.ChSystem): system.GetContactContainer().ReportAllContacts(self.contact_reporter) + self.grav_acc = np.array([getattr(system.Get_G_acc(), axis) for axis in ['x', 'y', 'z']]) def get_body_trajectory_point(self): output = {} From cea1b120f68a1edaf204efd1236709984787ef2e Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Thu, 5 Oct 2023 17:49:27 +0300 Subject: [PATCH 08/16] Change ForceControllerTemplate --- rostok/control_chrono/control_utils.py | 6 +- rostok/control_chrono/external_force.py | 234 +++++++++++------- rostok/control_chrono/tendon_controller.py | 41 +-- rostok/simulation_chrono/simulation.py | 11 +- .../simulation_chrono/simulation_scenario.py | 16 +- 5 files changed, 182 insertions(+), 126 deletions(-) diff --git a/rostok/control_chrono/control_utils.py b/rostok/control_chrono/control_utils.py index 65fa5734..a3d923d7 100644 --- a/rostok/control_chrono/control_utils.py +++ b/rostok/control_chrono/control_utils.py @@ -1,17 +1,17 @@ from dataclasses import dataclass, field -from rostok.control_chrono.external_force import ForceControllerTemplate +from rostok.control_chrono.external_force import ForceChronoWrapper @dataclass class ForceTorqueContainer: - controller_list: list[ForceControllerTemplate] = field(default_factory=list) + controller_list: list[ForceChronoWrapper] = field(default_factory=list) def update_all(self, time: float, data=None): for i in self.controller_list: i.update(time, data) - def add(self, controller: ForceControllerTemplate): + def add(self, controller: ForceChronoWrapper): if controller.is_bound: self.controller_list.append(controller) else: diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 1be0bc16..39afcac5 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -1,10 +1,11 @@ -from abc import abstractmethod +from abc import ABC, abstractmethod from dataclasses import dataclass import numpy as np from typing import Any, Callable, List import pychrono.core as chrono + def random_3d_vector(amp): """Calculate random 3d vector with given amplitude (uniform distribution on sphere) @@ -14,14 +15,15 @@ def random_3d_vector(amp): Returns: tuple: x, y, z components of vector """ - phi = np.random.uniform(0, 2*np.pi) + phi = np.random.uniform(0, 2 * np.pi) cos_theta = np.random.uniform(-1, 1) sin_theta = np.sqrt(1 - cos_theta**2) - z_force = amp*cos_theta - y_force = amp*sin_theta*np.sin(phi) - x_force = amp*sin_theta*np.cos(phi) + z_force = amp * cos_theta + y_force = amp * sin_theta * np.sin(phi) + x_force = amp * sin_theta * np.cos(phi) return x_force, y_force, z_force + def random_2d_vector(amp, angle: float = 0): """Calculate random 2d vector with given amplitude (uniform distribution on circle) @@ -32,18 +34,19 @@ def random_2d_vector(amp, angle: float = 0): Returns: tuple: x, y, z components of vector """ - angle = np.random.uniform(0, 2*np.pi) - - el1 = np.cos(angle)*amp - el2 = np.sin(angle)*amp - + angle = np.random.uniform(0, 2 * np.pi) + + el1 = np.cos(angle) * amp + el2 = np.sin(angle) * amp + v1 = chrono.ChVectorD(el1, el2, 0) - + q1 = chrono.Q_from_AngZ(angle) v1 = q1.Rotate(v1) - + return v1.x, v1.y, v1.z + @dataclass class ForceTorque: """Forces and torques are given in the xyz convention @@ -55,20 +58,38 @@ class ForceTorque: CALLBACK_TYPE = Callable[[float, Any], ForceTorque] -class ForceControllerTemplate(): +class ForceTemplate(ABC): + + def __init__(self, + name: str = "unnamed_force", + start_time: float = 0.0, + pos: np.ndarray = np.zeros(3)) -> None: + self.path = None + self.name = name + self.pos = pos + self.start_time = start_time + + @abstractmethod + def calculate_spatial_force(self, time, data) -> np.ndarray: + return np.zeros(6) + + def enable_data_dump(self, path): + self.path = path + with open(path, 'w') as file: + file.write('Data for external action:') + file.write(self.__dict__) + + +class ForceChronoWrapper(): """Base class for creating force and moment actions. To use it, you need to implement the get_force_torque method, which determines the force and torque at time t. """ - def __init__(self, - is_local: bool = False, - name: str = "unnamed_force", - pos: list[float] = [0, 0, 0]) -> None: - self.pos = pos - self.name = name + def __init__(self, force: ForceTemplate, is_local: bool = False) -> None: self.path = None + self.force = force self.x_force_chrono = chrono.ChFunction_Const(0) self.y_force_chrono = chrono.ChFunction_Const(0) @@ -83,6 +104,8 @@ def __init__(self, self.x_torque_chrono, self.y_torque_chrono, self.z_torque_chrono ] self.force_maker_chrono = chrono.ChForce() + + name = force.name.split("_")[0] self.force_maker_chrono.SetNameString(name + "_force") self.torque_maker_chrono = chrono.ChForce() self.torque_maker_chrono.SetNameString(name + "_torque") @@ -90,15 +113,12 @@ def __init__(self, self.is_local = is_local self.setup_makers() - def get_force_torque(self, time: float, data) -> ForceTorque: impact = ForceTorque() - impact.force = self.calculate_impact(time, data) + spatial_force = self.force.calculate_spatial_force(time, data) + impact.force = tuple(spatial_force[3:]) + impact.torque = tuple(spatial_force[:3]) return impact - - @abstractmethod - def calculate_impact(self, time: float, data) -> tuple[float, float, float]: - pass def update(self, time: float, data=None): force_torque = self.get_force_torque(time, data) @@ -128,8 +148,8 @@ def bind_body(self, body: chrono.ChBody): body.AddForce(self.force_maker_chrono) body.AddForce(self.torque_maker_chrono) self.__body = body - self.force_maker_chrono.SetVrelpoint(chrono.ChVectorD(*self.pos)) - self.torque_maker_chrono.SetVrelpoint(chrono.ChVectorD(*self.pos)) + self.force_maker_chrono.SetVrelpoint(chrono.ChVectorD(*self.force.pos.tolist())) + self.torque_maker_chrono.SetVrelpoint(chrono.ChVectorD(*self.force.pos.tolist())) self.is_bound = True def visualize_application_point(self, @@ -138,7 +158,8 @@ def visualize_application_point(self, body_opacity=0.6): sph_1 = chrono.ChSphereShape(size) sph_1.SetColor(color) - self.__body.AddVisualShape(sph_1, chrono.ChFrameD(chrono.ChVectorD(*self.pos))) + self.__body.AddVisualShape(sph_1, + chrono.ChFrameD(chrono.ChVectorD(*self.force.pos.tolist()))) self.__body.GetVisualShape(0).SetOpacity(body_opacity) @@ -149,26 +170,31 @@ def body(self): def enable_data_dump(self, path): self.path = path with open(path, 'w') as file: - file.write('Data for external action:', self.name) + file.write('Data for external action:', self.force.name) -class ForceControllerOnCallback(ForceControllerTemplate): +class ForceControllerOnCallback(ForceTemplate): - def __init__(self, callback: CALLBACK_TYPE) -> None: - super().__init__(name="callback_force") + def __init__(self, + callback: CALLBACK_TYPE, + name: str = "callback_force", + start_time: float = 0.0, + pos: np.ndarray = np.zeros(3)) -> None: + super().__init__(name, start_time, pos) self.callback = callback - def get_force_torque(self, time: float, data) -> ForceTorque: + def calculate_spatial_force(self, time, data) -> np.ndarray: return self.callback(time, data) -class YaxisSin(ForceControllerTemplate): +class YaxisSin(ForceTemplate): def __init__(self, amp: float = 5, amp_offset: float = 1, freq: float = 5, - start_time: float = 0.0) -> None: + start_time: float = 0.0, + pos: np.ndarray = np.zeros(3)) -> None: """Shake by sin along y axis Args: @@ -177,19 +203,21 @@ def __init__(self, freq (float, optional): Frequency of sin. Defaults to 5. start_time (float, optional): Start time of force application. Defaults to 0.0. """ - super().__init__(name="y_sin_force") + super().__init__("y_sin_force", start_time, pos) self.amp = amp self.amp_offset = amp_offset self.freq = freq - self.start_time = start_time - - def calculate_impact(self, time, data) -> tuple[float, float, float]: - y_force = 0 + + def calculate_spatial_force(self, time, data) -> np.ndarray: + spatial_force = np.zeros(6) if time >= self.start_time: - y_force = self.amp * np.sin(self.freq * (time - self.start_time)) + self.amp_offset - return 0, y_force, 0 + spatial_force[4] = self.amp * np.sin(self.freq * + (time - self.start_time)) + self.amp_offset + return spatial_force + + +class NullGravity(ForceTemplate): -class NullGravity(ForceControllerTemplate): def __init__(self, start_time: float = 0.0) -> None: """Apply force to compensate gravity @@ -197,19 +225,26 @@ def __init__(self, start_time: float = 0.0) -> None: gravitry_force (float): gravity force of object start_time (float, optional): start time of force application. Defaults to 0.0. """ - super().__init__(name="null_gravity_force") - self.start_time = start_time - - def calculate_impact(self, time: float, data) -> tuple[float, float, float]: - force_g = np.zeros(3) + super().__init__(name="null_gravity_force", start_time=start_time, pos=np.zeros(3)) + + def calculate_spatial_force(self, time: float, data) -> np.ndarray: + spatial_force = np.zeros(6) if time >= self.start_time: mass = data.body_map_ordered[0].body.GetMass() g = data.grav_acc - force_g = -mass*g - return force_g[0], force_g[1], force_g[2] + spatial_force[3:] = -mass * g + return spatial_force + + +class RandomForces(ForceTemplate): -class RandomForces(ForceControllerTemplate): - def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *args) -> None: + def __init__(self, + amp: float, + width_step: int = 20, + start_time: float = 0.0, + pos: np.ndarray = np.zeros(3), + dimension="3d", + angle=0.0) -> None: """Apply force with random direction and given amplitude Args: @@ -217,29 +252,35 @@ def __init__(self, amp: float, start_time: float = 0.0, width_step: int = 20, *a start_time (float, optional): Start time of force application. Defaults to 0.0. width_step (int, optional): Number of steps between changes of force direction. Defaults to 20. """ - super().__init__(name="random_force") - self.start_time = start_time + super().__init__(name="random_force", start_time=start_time, pos=pos) self.width_step = width_step self.amp = amp + self.dim = dimension + self.angle = angle + self.counter = 0 - self.y_force = 0 - self.x_force = 0 - self.z_force = 0 - self.args = args - - def calculate_impact(self, time: float, data) -> tuple[float, float, float]: + self.spatial_force = np.zeros(6) + + def calculate_spatial_force(self, time: float, data) -> np.ndarray: + if time >= self.start_time: - if self.counter % self.width_step == 0: - if self.args and self.args[0] == '2d': - self.x_force, self.y_force, self.z_force = random_2d_vector(self.amp, self.args[1]) - else: - self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) - self.x_force, self.y_force, self.z_force = random_3d_vector(self.amp) - self.counter += 1 - return self.x_force, self.y_force, self.z_force - -class ClockXZForces(ForceControllerTemplate): - def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = 0.0, width_step: int = 20) -> None: + if self.counter % self.width_step == 0: + if self.dim == '2d': + self.spatial_force[3:] = random_2d_vector(self.amp, self.angle) + else: + self.spatial_force[3:] = random_3d_vector(self.amp) + self.counter += 1 + return self.spatial_force + + +class ClockXZForces(ForceTemplate): + + def __init__(self, + amp: float, + angle_step: float = np.pi / 6, + width_step: int = 20, + start_time: float = 0.0, + pos: np.ndarray = np.zeros(3)) -> None: """Apply force with given amplitude in xz plane and rotate it with given angle step Args: @@ -248,49 +289,56 @@ def __init__(self, amp: float, angle_step: float = np.pi/6, start_time: float = start_time (float, optional): Start time of force application. Defaults to 0.0. width_step (int, optional): _description_. Defaults to 20. """ - super().__init__(name="clock_xz_force") + super().__init__(name="clock_xz_force", start_time=start_time, pos=pos) self.amp = amp - self.start_time = start_time self.width_step = width_step self.counter: int = 0 self.angle: float = 0.0 self.angle_step: float = angle_step - - def calculate_impact(self, time: float, data) -> tuple[float, float, float]: - y_force = 0 - x_force = 0 - z_force = 0 + + def calculate_spatial_force(self, time: float, data) -> np.ndarray: + spatial_force = np.zeros(6) if time >= self.start_time: if self.counter % self.width_step == 0: self.angle += self.angle_step self.counter += 1 - x_force = np.cos(self.angle_step)*self.amp - z_force = np.sin(self.angle_step)*self.amp - return x_force, y_force, z_force - -class ExternalForces(ForceControllerTemplate): - def __init__(self, force_controller: ForceControllerTemplate | List[ForceControllerTemplate]) -> None: + spatial_force[3] = np.cos(self.angle_step) * self.amp + spatial_force[4] = np.sin(self.angle_step) * self.amp + return spatial_force + + +class ExternalForces(ForceTemplate): + + def __init__(self, force_controller: ForceTemplate | List[ForceTemplate]) -> None: """Class for combining several external forces Args: - force_controller (ForceControllerTemplate | List[ForceControllerTemplate]): Forces or list of forces + force_controller (ForceTemplate | List[ForceTemplate]): Forces or list of forces """ - super().__init__(name="external_forces") + + if isinstance(force_controller, list): + start_times = np.array([i.start_time for i in force_controller]) + positions = np.array([i.pos for i in force_controller]) + if np.all(start_times != start_times[0]) or np.all(positions != positions[0]): + raise Exception("All forces should have the same start time and position") + + super().__init__(name="external_forces", start_time=0.0, pos=np.zeros(3)) self.force_controller = force_controller - - def add_force(self, force: ForceControllerTemplate): + + def add_force(self, force: ForceTemplate): if isinstance(self.force_controller, list): self.force_controller.append(force) else: self.force_controller = [self.force_controller, force] - - def get_force_torque(self, time: float, data) -> ForceTorque: + + def calculate_spatial_force(self, time: float, data) -> ForceTorque: if isinstance(self.force_controller, list): v_forces = np.zeros(3) for controller in self.force_controller: - v_forces += np.array(controller.calculate_impact(time, data)) + v_forces += np.array(controller.calculate_spatial_force(time, data)) impact = ForceTorque() - impact.force = tuple(v_forces) + impact.force = tuple(v_forces[3:]) + impact.torque = tuple(v_forces[:3]) return impact else: - return self.force_controller.get_force_torque(time, data) \ No newline at end of file + return self.force_controller.calculate_spatial_force(time, data) \ No newline at end of file diff --git a/rostok/control_chrono/tendon_controller.py b/rostok/control_chrono/tendon_controller.py index 872598c4..0a324518 100644 --- a/rostok/control_chrono/tendon_controller.py +++ b/rostok/control_chrono/tendon_controller.py @@ -3,13 +3,13 @@ from pathlib import Path from typing import Dict, List, Union +import numpy as np + import pychrono as chrono from rostok.control_chrono.controller import RobotControllerChrono -from rostok.control_chrono.external_force import (ForceControllerTemplate, - ForceTorque) -from rostok.graph_grammar.graph_comprehension import (get_tip_ids, - is_star_topology) +from rostok.control_chrono.external_force import (ForceTemplate, ForceChronoWrapper) +from rostok.graph_grammar.graph_comprehension import (get_tip_ids, is_star_topology) from rostok.graph_grammar.node import GraphGrammar from rostok.graph_grammar.node_block_typing import NodeFeatures from rostok.virtual_experiment.built_graph_chrono import BuiltGraphChrono @@ -22,40 +22,40 @@ class ForceType(Enum): BASE_CONNECTION = 2 -class PulleyForce(ForceControllerTemplate): +class PulleyForce(ForceTemplate): - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + def calculate_spatial_force(self, time: float, data) -> np.ndarray: + spatial_force = np.zeros(6) pre_point = data[0] point = data[1] post_point = data[2] tension = data[3] force_v = ((post_point - point).GetNormalized() + (pre_point - point).GetNormalized()) * tension - impact.force = (force_v.x, force_v.y, force_v.z) + spatial_force[3:] = np.array([force_v.x, force_v.y, force_v.z]) if self.path: with open(self.path, 'a') as file: file.write( f'{self.name} {round(time, 5)} {round(point.x,5)} {round(point.y,5)} {round(point.z,5)} {round(force_v.x, 6)} {round(force_v.y,6)} {round(force_v.z,6)}\n' ) - return impact + return spatial_force -class TipForce(ForceControllerTemplate): +class TipForce(ForceTemplate): - def get_force_torque(self, time: float, data) -> ForceTorque: - impact = ForceTorque() + def calculate_spatial_force(self, time: float, data) -> np.ndarray: + spatial_force = np.zeros(6) pre_point = data[0] point = data[1] tension = data[2] force_v = (pre_point - point).GetNormalized() * tension - impact.force = (force_v.x, force_v.y, force_v.z) + spatial_force[3:] = np.array([force_v.x, force_v.y, force_v.z]) if self.path: with open(self.path, 'a') as file: file.write( f'{self.name} {round(time, 5)} {round(point.x,5)} {round(point.y,5)} {round(point.z,5)} {round(force_v.x, 6)} {round(force_v.y,6)} {round(force_v.z,6)}\n' ) - return impact + return spatial_force @dataclass @@ -135,8 +135,10 @@ def create_pulley_lines_2p(graph: GraphGrammar, pulleys_in_phalanx=2, finger_bas return pulley_lines + PULLEY_LINES_TYPE = list[list[list[PulleyParameters, Union[PulleyForce, TipForce]]]] + class TendonController_2p(RobotControllerChrono): def __init__(self, graph: BuiltGraphChrono, control_parameters: TendonControllerParameters): @@ -179,18 +181,21 @@ def set_forces_to_pulley_line(self, tendon_lines: PULLEY_LINES_TYPE): idx = force_point[0].body_id body = self.built_graph.body_map_ordered[idx] if force_point[0].force_type == ForceType.PULLEY: - force_point[1] = PulleyForce(pos=list(force_point[0].position), - name=f'{idx}_p_{force_point[0].pulley_number}') + pulley_force = PulleyForce(pos=np.array(force_point[0].position), + name=f'{idx}_p_{force_point[0].pulley_number}') + force_point[1] = ForceChronoWrapper(pulley_force) force_point[1].bind_body(body.body) force_point[1].visualize_application_point() elif force_point[0].force_type == ForceType.TIP: - force_point[1] = TipForce(pos=list(force_point[0].position), name=f'{idx}_t') + tip_force = TipForce(pos=np.array(force_point[0].position), name=f'{idx}_t') + force_point[1] = ForceChronoWrapper(tip_force) force_point[1].bind_body(body.body) force_point[1].visualize_application_point() elif force_point[0].force_type == ForceType.BASE_CONNECTION: - force_point[1] = TipForce(pos=list(force_point[0].position)) + tip_force = TipForce(pos=np.array(force_point[0].position)) + force_point[1] = ForceChronoWrapper(tip_force) force_point[1].bind_body(body.body) force_point[1].visualize_application_point() diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index 488e2c8b..5d5b5f6b 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -4,14 +4,12 @@ import pychrono as chrono import pychrono.irrlicht as chronoirr -from rostok.block_builder_api.block_parameters import (DefaultFrame, - FrameTransform) +from rostok.block_builder_api.block_parameters import (DefaultFrame, FrameTransform) from rostok.block_builder_chrono.block_classes import ChronoEasyShapeObject from rostok.control_chrono.control_utils import ForceTorqueContainer from rostok.control_chrono.controller import ConstController -from rostok.control_chrono.external_force import ForceControllerTemplate -from rostok.criterion.simulation_flags import (EventCommands, - SimulationSingleEvent) +from rostok.control_chrono.external_force import ForceTemplate, ForceChronoWrapper +from rostok.criterion.simulation_flags import (EventCommands, SimulationSingleEvent) from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation_utils import SimulationResult from rostok.virtual_experiment.robot_new import BuiltGraphChrono, RobotChrono @@ -108,7 +106,7 @@ def add_object(self, obj: ChronoEasyShapeObject, read_data: bool = False, is_fixed=False, - force_torque_controller: Optional[ForceControllerTemplate] = None): + force_torque_controller: Optional[ForceChronoWrapper] = None): """" Add an object to the environment Args: @@ -120,6 +118,7 @@ def add_object(self, self.objects.append(obj) if force_torque_controller: + force_torque_controller.bind_body(obj.body) self.force_torque_container.add(force_torque_controller) if read_data: diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index 8b7bdfd9..a7e2e497 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -5,8 +5,8 @@ import numpy as np import pychrono as chrono -from rostok.control_chrono.controller import (ConstController, - SinControllerChrono) +from rostok.control_chrono.controller import (ConstController, SinControllerChrono) +from rostok.control_chrono.external_force import ForceChronoWrapper, ForceTemplate from rostok.criterion.simulation_flags import SimulationSingleEvent from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation import (ChronoSystems, EnvCreator, SingleRobotSimulation, @@ -19,6 +19,7 @@ ChronoBlockCreatorInterface as creator from rostok.control_chrono.tendon_controller import TendonController_2p + class ParametrizedSimulation: def __init__(self, step_length, simulation_length): @@ -44,7 +45,7 @@ def __init__(self, simulation_length, tendon=True, smc=False, - obj_external_forces: Optional[ForceControllerTemplate] = None) -> None: + obj_external_forces: Optional[ForceTemplate] = None) -> None: super().__init__(step_length, simulation_length) self.grasp_object_callback = None self.event_container: List[SimulationSingleEvent] = [] @@ -81,11 +82,14 @@ def run_simulation(self, grasp_object = creator.create_environment_body(self.grasp_object_callback) grasp_object.body.SetNameString("Grasp_object") set_covering_ellipsoid_based_position(grasp_object, - reference_point=chrono.ChVectorD(0, 0.1, 0)) - + reference_point=chrono.ChVectorD(0, 0.1, 0)) + if self.obj_external_forces: + chrono_forces = ForceChronoWrapper(deepcopy(self.obj_external_forces)) + else: + chrono_forces = None simulation.env_creator.add_object(grasp_object, read_data=True, - force_torque_controller=None) + force_torque_controller=chrono_forces) # add design and determine the outer force if self.tendon: From 53dab1ebf4a15eb80ca76e7146db78762a80756d Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Thu, 5 Oct 2023 17:49:58 +0300 Subject: [PATCH 09/16] example null_gravity --- app/mcts_run_setup.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index 020d286b..9a08a7bf 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -18,6 +18,7 @@ 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 config_independent_torque(grasp_object_blueprint): @@ -73,7 +74,10 @@ def config_independent_torque(grasp_object_blueprint): def config_with_tendon(grasp_object_blueprint): # configurate the simulation manager - simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION) + obj_forces = [] + obj_forces.append(f_ext.NullGravity(1)) + obj_forces = f_ext.ExternalForces(obj_forces) + simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, obj_external_forces=f_ext.NullGravity(0)) simulation_manager.grasp_object_callback = grasp_object_blueprint #lambda: creator.create_environment_body( #grasp_object_blueprint) event_contact = EventContact() From 38f4b9d6d1fb98333d5b2156fabd0e768944e270 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Thu, 5 Oct 2023 17:50:59 +0300 Subject: [PATCH 10/16] Fix different start time for ext f --- rostok/control_chrono/external_force.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 39afcac5..ba72e44e 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -317,10 +317,9 @@ def __init__(self, force_controller: ForceTemplate | List[ForceTemplate]) -> Non """ if isinstance(force_controller, list): - start_times = np.array([i.start_time for i in force_controller]) positions = np.array([i.pos for i in force_controller]) - if np.all(start_times != start_times[0]) or np.all(positions != positions[0]): - raise Exception("All forces should have the same start time and position") + if np.all(positions != positions[0]): + raise Exception("All forces should have the same position") super().__init__(name="external_forces", start_time=0.0, pos=np.zeros(3)) self.force_controller = force_controller From cbc2255cbc40cd609b8dbd15799f7144e76d578d Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Mon, 9 Oct 2023 16:06:54 +0300 Subject: [PATCH 11/16] vi dodvolni? --- rostok/control_chrono/external_force.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index ba72e44e..fee5ac7c 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -34,12 +34,12 @@ def random_2d_vector(amp, angle: float = 0): Returns: tuple: x, y, z components of vector """ - angle = np.random.uniform(0, 2 * np.pi) + phi = np.random.uniform(0, 2 * np.pi) - el1 = np.cos(angle) * amp - el2 = np.sin(angle) * amp + el1 = np.cos(phi) * amp + el2 = np.sin(phi) * amp - v1 = chrono.ChVectorD(el1, el2, 0) + v1 = chrono.ChVectorD(el1, 0, el2) q1 = chrono.Q_from_AngZ(angle) v1 = q1.Rotate(v1) From 91da9bbd6a9894aedec71cf232367a61b834e742 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Wed, 11 Oct 2023 12:16:12 +0300 Subject: [PATCH 12/16] Fix docs --- rostok/control_chrono/external_force.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index fee5ac7c..5fedeea9 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -7,7 +7,7 @@ def random_3d_vector(amp): - """Calculate random 3d vector with given amplitude (uniform distribution on sphere) + """Sample random 3d vector with given amplitude (uniform distribution on sphere) Args: amp (float): amplitude of vector @@ -25,7 +25,7 @@ def random_3d_vector(amp): def random_2d_vector(amp, angle: float = 0): - """Calculate random 2d vector with given amplitude (uniform distribution on circle) + """Sample random 2d vector with given amplitude (uniform distribution on circle) Args: amp (float): amplitude of vector From 07fb0fff75ecf2f93f183989aaa95fa0ab6233b9 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Wed, 11 Oct 2023 12:43:17 +0300 Subject: [PATCH 13/16] change def random 2d vectors --- rostok/control_chrono/external_force.py | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 5fedeea9..823f6519 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -24,27 +24,32 @@ def random_3d_vector(amp): return x_force, y_force, z_force -def random_2d_vector(amp, angle: float = 0): - """Sample random 2d vector with given amplitude (uniform distribution on circle) +def random_2d_vector(amp, phi: float = 0, theta: float = 0): + """Sample random 2d vector with given amplitude (uniform distribution on circle) in xz plane. + Phi is angle along axis z, theta is angle along axis y. First rotate vector along z axis by phi, then along y axis by theta Args: amp (float): amplitude of vector - angle (float, optional): angle along axis z of vector. Defaults to 0. + phi (float, optional): angle along axis z of vector. Defaults to 0. + theta (float, optional): angle along axis y of vector. Defaults to 0. Returns: tuple: x, y, z components of vector """ - phi = np.random.uniform(0, 2 * np.pi) + angle = np.random.uniform(0, 2 * np.pi) - el1 = np.cos(phi) * amp - el2 = np.sin(phi) * amp + el1 = np.cos(angle) * amp + el2 = np.sin(angle) * amp v1 = chrono.ChVectorD(el1, 0, el2) - q1 = chrono.Q_from_AngZ(angle) + q1 = chrono.Q_from_AngZ(phi) v1 = q1.Rotate(v1) + + q2 = chrono.Q_from_AngY(theta) + v2 = q2.Rotate(v1) - return v1.x, v1.y, v1.z + return v2.x, v2.y, v2.z @dataclass From 9295afff65929887cb8a278dc2ee08de354fb49e Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Wed, 11 Oct 2023 12:43:55 +0300 Subject: [PATCH 14/16] change name --- rostok/control_chrono/external_force.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 823f6519..035a2d3c 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -24,7 +24,7 @@ def random_3d_vector(amp): return x_force, y_force, z_force -def random_2d_vector(amp, phi: float = 0, theta: float = 0): +def random_plane_vector(amp, phi: float = 0, theta: float = 0): """Sample random 2d vector with given amplitude (uniform distribution on circle) in xz plane. Phi is angle along axis z, theta is angle along axis y. First rotate vector along z axis by phi, then along y axis by theta From a57ae3dec7b14d952e72c2e404b5c9e2b27cf2cc Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Wed, 11 Oct 2023 13:02:44 +0300 Subject: [PATCH 15/16] example --- app/mcts_run_setup.py | 5 +++-- rostok/control_chrono/external_force.py | 15 +++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/app/mcts_run_setup.py b/app/mcts_run_setup.py index 9a08a7bf..c73d50e8 100644 --- a/app/mcts_run_setup.py +++ b/app/mcts_run_setup.py @@ -75,9 +75,10 @@ def config_with_tendon(grasp_object_blueprint): # configurate the simulation manager obj_forces = [] - obj_forces.append(f_ext.NullGravity(1)) + 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=f_ext.NullGravity(0)) + simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, obj_external_forces=obj_forces) simulation_manager.grasp_object_callback = grasp_object_blueprint #lambda: creator.create_environment_body( #grasp_object_blueprint) event_contact = EventContact() diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 035a2d3c..872d9240 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -249,7 +249,7 @@ def __init__(self, start_time: float = 0.0, pos: np.ndarray = np.zeros(3), dimension="3d", - angle=0.0) -> None: + angle=(0.0, 0.0)) -> None: """Apply force with random direction and given amplitude Args: @@ -271,9 +271,11 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: if time >= self.start_time: if self.counter % self.width_step == 0: if self.dim == '2d': - self.spatial_force[3:] = random_2d_vector(self.amp, self.angle) - else: + self.spatial_force[3:] = random_plane_vector(self.amp, *self.angle) + elif self.dim == '3d': self.spatial_force[3:] = random_3d_vector(self.amp) + else: + raise Exception("Wrong name of dimension. Should be 2d or 3d") self.counter += 1 return self.spatial_force @@ -337,12 +339,9 @@ def add_force(self, force: ForceTemplate): def calculate_spatial_force(self, time: float, data) -> ForceTorque: if isinstance(self.force_controller, list): - v_forces = np.zeros(3) + v_forces = np.zeros(6) for controller in self.force_controller: v_forces += np.array(controller.calculate_spatial_force(time, data)) - impact = ForceTorque() - impact.force = tuple(v_forces[3:]) - impact.torque = tuple(v_forces[:3]) - return impact + return v_forces else: return self.force_controller.calculate_spatial_force(time, data) \ No newline at end of file From 6fde9c41a4abc26869db20ce6d042b52c9118521 Mon Sep 17 00:00:00 2001 From: Yefim Osipov Date: Fri, 13 Oct 2023 12:16:38 +0300 Subject: [PATCH 16/16] change names --- rostok/control_chrono/external_force.py | 23 ++++++++++--------- rostok/control_chrono/tendon_controller.py | 6 ++--- rostok/simulation_chrono/simulation.py | 2 +- .../simulation_chrono/simulation_scenario.py | 4 ++-- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/rostok/control_chrono/external_force.py b/rostok/control_chrono/external_force.py index 872d9240..621b12f6 100644 --- a/rostok/control_chrono/external_force.py +++ b/rostok/control_chrono/external_force.py @@ -63,7 +63,7 @@ class ForceTorque: CALLBACK_TYPE = Callable[[float, Any], ForceTorque] -class ForceTemplate(ABC): +class ABCForceCalculator(ABC): def __init__(self, name: str = "unnamed_force", @@ -92,7 +92,7 @@ class ForceChronoWrapper(): which determines the force and torque at time t. """ - def __init__(self, force: ForceTemplate, is_local: bool = False) -> None: + def __init__(self, force: ABCForceCalculator, is_local: bool = False) -> None: self.path = None self.force = force @@ -178,7 +178,7 @@ def enable_data_dump(self, path): file.write('Data for external action:', self.force.name) -class ForceControllerOnCallback(ForceTemplate): +class ForceControllerOnCallback(ABCForceCalculator): def __init__(self, callback: CALLBACK_TYPE, @@ -192,7 +192,7 @@ def calculate_spatial_force(self, time, data) -> np.ndarray: return self.callback(time, data) -class YaxisSin(ForceTemplate): +class YaxisSin(ABCForceCalculator): def __init__(self, amp: float = 5, @@ -221,7 +221,7 @@ def calculate_spatial_force(self, time, data) -> np.ndarray: return spatial_force -class NullGravity(ForceTemplate): +class NullGravity(ABCForceCalculator): def __init__(self, start_time: float = 0.0) -> None: """Apply force to compensate gravity @@ -241,7 +241,7 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: return spatial_force -class RandomForces(ForceTemplate): +class RandomForces(ABCForceCalculator): def __init__(self, amp: float, @@ -280,7 +280,7 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: return self.spatial_force -class ClockXZForces(ForceTemplate): +class ClockXZForces(ABCForceCalculator): def __init__(self, amp: float, @@ -314,15 +314,15 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: return spatial_force -class ExternalForces(ForceTemplate): +class ExternalForces(ABCForceCalculator): - def __init__(self, force_controller: ForceTemplate | List[ForceTemplate]) -> None: + def __init__(self, force_controller: ABCForceCalculator | List[ABCForceCalculator]) -> None: """Class for combining several external forces Args: force_controller (ForceTemplate | List[ForceTemplate]): Forces or list of forces """ - + self.data_forces = [] if isinstance(force_controller, list): positions = np.array([i.pos for i in force_controller]) if np.all(positions != positions[0]): @@ -331,7 +331,7 @@ def __init__(self, force_controller: ForceTemplate | List[ForceTemplate]) -> Non super().__init__(name="external_forces", start_time=0.0, pos=np.zeros(3)) self.force_controller = force_controller - def add_force(self, force: ForceTemplate): + def add_force(self, force: ABCForceCalculator): if isinstance(self.force_controller, list): self.force_controller.append(force) else: @@ -342,6 +342,7 @@ def calculate_spatial_force(self, time: float, data) -> ForceTorque: v_forces = np.zeros(6) for controller in self.force_controller: v_forces += np.array(controller.calculate_spatial_force(time, data)) + 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 diff --git a/rostok/control_chrono/tendon_controller.py b/rostok/control_chrono/tendon_controller.py index 0a324518..396c41f5 100644 --- a/rostok/control_chrono/tendon_controller.py +++ b/rostok/control_chrono/tendon_controller.py @@ -8,7 +8,7 @@ import pychrono as chrono from rostok.control_chrono.controller import RobotControllerChrono -from rostok.control_chrono.external_force import (ForceTemplate, ForceChronoWrapper) +from rostok.control_chrono.external_force import (ABCForceCalculator, ForceChronoWrapper) from rostok.graph_grammar.graph_comprehension import (get_tip_ids, is_star_topology) from rostok.graph_grammar.node import GraphGrammar from rostok.graph_grammar.node_block_typing import NodeFeatures @@ -22,7 +22,7 @@ class ForceType(Enum): BASE_CONNECTION = 2 -class PulleyForce(ForceTemplate): +class PulleyForce(ABCForceCalculator): def calculate_spatial_force(self, time: float, data) -> np.ndarray: spatial_force = np.zeros(6) @@ -41,7 +41,7 @@ def calculate_spatial_force(self, time: float, data) -> np.ndarray: return spatial_force -class TipForce(ForceTemplate): +class TipForce(ABCForceCalculator): def calculate_spatial_force(self, time: float, data) -> np.ndarray: spatial_force = np.zeros(6) diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index 5d5b5f6b..2fb2b1db 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -8,7 +8,7 @@ from rostok.block_builder_chrono.block_classes import ChronoEasyShapeObject from rostok.control_chrono.control_utils import ForceTorqueContainer from rostok.control_chrono.controller import ConstController -from rostok.control_chrono.external_force import ForceTemplate, ForceChronoWrapper +from rostok.control_chrono.external_force import ABCForceCalculator, ForceChronoWrapper from rostok.criterion.simulation_flags import (EventCommands, SimulationSingleEvent) from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation_utils import SimulationResult diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index a7e2e497..14bdfd57 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -6,7 +6,7 @@ import pychrono as chrono from rostok.control_chrono.controller import (ConstController, SinControllerChrono) -from rostok.control_chrono.external_force import ForceChronoWrapper, ForceTemplate +from rostok.control_chrono.external_force import ForceChronoWrapper, ABCForceCalculator from rostok.criterion.simulation_flags import SimulationSingleEvent from rostok.graph_grammar.node import GraphGrammar from rostok.simulation_chrono.simulation import (ChronoSystems, EnvCreator, SingleRobotSimulation, @@ -45,7 +45,7 @@ def __init__(self, simulation_length, tendon=True, smc=False, - obj_external_forces: Optional[ForceTemplate] = None) -> None: + obj_external_forces: Optional[ABCForceCalculator] = None) -> None: super().__init__(step_length, simulation_length) self.grasp_object_callback = None self.event_container: List[SimulationSingleEvent] = []