From e96fd042ab981c4b5c2fd199421625296a7ad8d8 Mon Sep 17 00:00:00 2001 From: MikhailChaikovskii Date: Wed, 4 Oct 2023 14:15:46 +0300 Subject: [PATCH 1/4] copy from article_3 --- rostok/criterion/criterion_calculation.py | 15 ++--- rostok/criterion/simulation_flags.py | 81 +++++++++++++++++------ rostok/virtual_experiment/sensors.py | 29 +++++--- 3 files changed, 84 insertions(+), 41 deletions(-) diff --git a/rostok/criterion/criterion_calculation.py b/rostok/criterion/criterion_calculation.py index 78ae55e8..d07c1dc5 100644 --- a/rostok/criterion/criterion_calculation.py +++ b/rostok/criterion/criterion_calculation.py @@ -1,14 +1,13 @@ -from abc import ABC import json -from bisect import bisect_left -from typing import Dict, List, Optional, Tuple, Union +from abc import ABC +from typing import List import numpy as np -import pychrono.core as chrono from scipy.spatial import distance +from rostok.criterion.simulation_flags import (EventContactTimeOut, EventGrasp, + EventSlipOut) from rostok.simulation_chrono.simulation_utils import SimulationResult -from rostok.criterion.simulation_flags import SimulationSingleEvent, EventContactTimeOut, EventGrasp, EventSlipOut from rostok.utils.json_encoder import RostokJSONEncoder @@ -131,7 +130,7 @@ def calculate_reward(self, simulation_output: SimulationResult): body_outer_force_center = env_data.get_data("force_center")[0][self.grasp_event.step_n + 1] if body_outer_force_center is np.nan: - print(body_COG, body_outer_force_center) + return 0 dist = distance.euclidean(body_COG, body_outer_force_center) return 1 / (1 + dist) else: @@ -294,7 +293,7 @@ def calculate_reward(self, simulation_output, partial=False): partial_rewards = [] for criterion in self.criteria: reward = criterion.calculate_reward(simulation_output) - partial_rewards.append(round(reward,3)) + partial_rewards.append(round(reward, 3)) if partial: return partial_rewards @@ -302,7 +301,7 @@ def calculate_reward(self, simulation_output, partial=False): print([round(x, 3) for x in partial_rewards]) total_reward = sum([a * b for a, b in zip(partial_rewards, self.weights)]) - + if np.isclose(total_reward, 0, atol=1e-3): total_reward = 0.02 return round(total_reward, 3) diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index 671892eb..a6ac2a19 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -1,13 +1,12 @@ -from abc import ABC, abstractmethod import json -from typing import Dict, List, Optional, Tuple, Union +from abc import ABC, abstractmethod from enum import Enum -import types +from typing import Optional + import numpy as np -import pychrono.core as chrono -from rostok.utils.json_encoder import RostokJSONEncoder -from rostok.virtual_experiment.sensors import DataStorage, Sensor +from rostok.utils.json_encoder import RostokJSONEncoder +from rostok.virtual_experiment.sensors import Sensor class EventCommands(Enum): @@ -41,7 +40,7 @@ def reset(self): self.step_n = None @abstractmethod - def event_check(self, current_time: float, step_n: int, robot_data, env_data): + def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor): """Simulation calls that method each step to check if the event occurred. Args: @@ -64,12 +63,31 @@ class EventContact(SimulationSingleEvent): """Event of contact between robot and object """ - - def event_check(self, current_time: float, step_n: int, robot_data, env_data): - if env_data.get_amount_contacts()[0] > 0: - self.state = True - self.step_n = step_n + def __init__(self, take_from_body = False): + super().__init__() + self.from_body = take_from_body + def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor): + if self.state: return EventCommands.CONTINUE + if self.from_body: + # the contact information from the object + if env_data.get_amount_contacts()[0] > 0: + self.state = True + self.step_n = step_n + else: + # the contact information from the robot + robot_contacts = robot_data.get_amount_contacts() + # it works only with current rule set, where the palm/flat always has the smallest index among the bodies + flat_idx_ = list(robot_contacts.keys())[0] + contacts = 0 + # we calculate only the amount of unique keys, therefore the amount of unique contacting bodies + for key, value in robot_contacts.items(): + if key != flat_idx_ and value > 0: + contacts += 1 + + if contacts > 0: + self.state = True + self.step_n = step_n return EventCommands.CONTINUE @@ -94,6 +112,7 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ Returns: EventCommands: return a command for simulation """ + # if the contact has already occurred in simulation, return CONTINUE if self.contact_event.state: return EventCommands.CONTINUE @@ -160,11 +179,14 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ """ # Old variant: contact = env_data.get_amount_contacts()[0] > 0 robot_contacts = robot_data.get_amount_contacts() - flat_idx_= list(robot_contacts.keys())[0] + # it works only with current rule set, where the palm/flat always has the smallest index among the bodies + flat_idx_ = list(robot_contacts.keys())[0] contacts = 0 + # we calculate only the amount of unique keys, therefore the amount of unique contacting bodies for key, value in robot_contacts.items(): - if key != flat_idx_: - contacts += value + if key != flat_idx_ and value > 0: + contacts += 1 + contact = contacts > 0 if contact: self.time_last_contact = current_time @@ -191,7 +213,11 @@ class EventGrasp(SimulationSingleEvent): force_test_time (float): the time period of the force test of the grasp """ - def __init__(self, grasp_limit_time: float, contact_event: EventContact, verbosity: int = 0, simulation_stop = False): + def __init__(self, + grasp_limit_time: float, + contact_event: EventContact, + verbosity: int = 0, + simulation_stop:bool = False): super().__init__(verbosity=verbosity) self.grasp_steps: int = 0 self.grasp_time: Optional[float] = None @@ -207,12 +233,20 @@ def reset(self): def check_grasp_timeout(self, current_time): if current_time > self.grasp_limit_time: return True - else: - return False + return False - def check_grasp_current_step(self, env_data: Sensor): + def check_grasp_current_step(self, env_data: Sensor, robot_data: Sensor): obj_velocity = np.linalg.norm(np.array(env_data.get_velocity()[0])) - if obj_velocity <= 0.01 and env_data.get_amount_contacts()[0] >= 2: + robot_contacts = robot_data.get_amount_contacts() + # it works only with current rule set, where the palm/flat always has the smallest index among the bodies + flat_idx_ = list(robot_contacts.keys())[0] + contacts = 0 + # we calculate only the amount of unique keys, therefore the amount of unique contacting bodies + for key, value in robot_contacts.items(): + if key != flat_idx_ and value > 0: + contacts += 1 + + if obj_velocity <= 0.01 and contacts >= 2: self.grasp_steps += 1 else: self.grasp_steps = 0 @@ -220,17 +254,19 @@ def check_grasp_current_step(self, env_data: Sensor): def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor): """Return ACTIVATE if the body was in contact with the robot and after that at some point doesn't move for at least 10 steps. Return STOP if the grasp didn't occur during grasp_limit_time. - Returns: EventCommands: return a command for simulation """ + if self.state: + return EventCommands.CONTINUE + if self.check_grasp_timeout(current_time): return EventCommands.STOP if self.contact_event.state: - self.check_grasp_current_step(env_data) + self.check_grasp_current_step(env_data, robot_data) if self.grasp_steps == 10: self.state = True @@ -240,6 +276,7 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ print('Grasp event!', current_time) if self.simulation_stop > 0: input('press enter to continue') + # pass return EventCommands.ACTIVATE diff --git a/rostok/virtual_experiment/sensors.py b/rostok/virtual_experiment/sensors.py index d43b2ff5..3a83de91 100644 --- a/rostok/virtual_experiment/sensors.py +++ b/rostok/virtual_experiment/sensors.py @@ -89,7 +89,6 @@ def get_outer_contacts(self): return self.__outer_contact_dict_this_step - class Sensor: """Control data obtained in the current step of the simulation""" @@ -110,6 +109,7 @@ def get_body_trajectory_point(self): round(body.body.GetPos().y, 4), round(body.body.GetPos().z, 4) ] + output[idx] = np.nan_to_num(output[idx], nan=9999).tolist() return output def get_velocity(self): @@ -120,13 +120,17 @@ def get_velocity(self): round(body.body.GetPos_dt().y, 4), round(body.body.GetPos_dt().z, 4) ] + output[idx] = np.nan_to_num(output[idx], nan=9999).tolist() return output - + def get_rotation_velocity(self): output = {} for idx, body in self.body_map_ordered.items(): mat = body.body.GetA_dt() - output[idx] = [[mat.Get_A_Xaxis.x, mat.Get_A_Yaxis.x, mat.Get_A_Zaxis.x], [mat.Get_A_Xaxis.y, mat.Get_A_Yaxis.y, mat.Get_A_Zaxis.y], [mat.Get_A_Xaxis.z, mat.Get_A_Yaxis.z, mat.Get_A_Zaxis.z]] + output[idx] = [[mat.Get_A_Xaxis.x, mat.Get_A_Yaxis.x, mat.Get_A_Zaxis.x], + [mat.Get_A_Xaxis.y, mat.Get_A_Yaxis.y, mat.Get_A_Zaxis.y], + [mat.Get_A_Xaxis.z, mat.Get_A_Yaxis.z, mat.Get_A_Zaxis.z]] + output[idx] = np.nan_to_num(output[idx], nan=9999).tolist() return output def get_joint_z_trajectory_point(self): @@ -137,7 +141,8 @@ def get_joint_z_trajectory_point(self): relative_rot = (master_body.GetInverse() * slave_body) angle = chrono.Q_to_Euler123(chrono.ChQuaternionD(relative_rot.GetRot())) output[idx] = round(angle.z, 5) - + output[idx] = np.nan_to_num(output[idx], nan=9999).tolist() + return output def get_forces(self): @@ -147,9 +152,9 @@ def get_forces(self): contacts_idx = contacts[idx] if len(contacts_idx) > 0: output[idx] = contacts_idx + output[idx] = np.nan_to_num(output[idx], nan=0).tolist() else: output[idx] = [] - return output def get_amount_contacts(self): @@ -158,7 +163,7 @@ def get_amount_contacts(self): for idx in self.body_map_ordered: contacts_idx = contacts[idx] output[idx] = len(contacts_idx) - + output[idx] = np.nan_to_num(output[idx], nan=0).tolist() return output def get_outer_force_center(self): @@ -175,6 +180,7 @@ def get_outer_force_center(self): body_contact_coordinates_sum = body_contact_coordinates_sum * (1 / len(contacts_idx)) output[idx] = list(body_contact_coordinates_sum) + output[idx] = np.nan_to_num(output[idx], nan=9999).tolist() else: output[idx] = None @@ -189,8 +195,11 @@ def get_outer_force_center(self): # return output def get_body_map(self): return self.body_map_ordered + def get_joint_map(self): return self.joint_map_ordered + + class SensorCalls(str, Enum): """ BODY_TRAJECTORY: trajectories of all bodies from the body map, @@ -211,6 +220,7 @@ class SensorObjectClassification(str, Enum): BODY = Sensor.get_body_map JOINT = Sensor.get_joint_map + class DataStorage(): """Class aggregates data from all steps of the simulation.""" @@ -219,11 +229,8 @@ def __init__(self, sensor: Sensor): self.callback_dict = {} self.main_storage = {} - def add_data_type(self, - key: str, - sensor_callback: SensorCalls, - object_map: SensorObjectClassification, - step_number): + def add_data_type(self, key: str, sensor_callback: SensorCalls, + object_map: SensorObjectClassification, step_number): empty_dict: Dict[int, np.NDArray] = {} self.callback_dict[key] = sensor_callback starting_values = sensor_callback(self.sensor) From 1ee650bc0a6078988230f3bae953d9eee549ce27 Mon Sep 17 00:00:00 2001 From: MikhailChaikovskii Date: Wed, 4 Oct 2023 17:54:00 +0300 Subject: [PATCH 2/4] formater --- rostok/simulation_chrono/simulation.py | 5 ++--- .../simulation_chrono/simulation_scenario.py | 19 +++++++++---------- rostok/simulation_chrono/simulation_utils.py | 10 +++++----- .../virtual_experiment/built_graph_chrono.py | 6 ++++-- rostok/virtual_experiment/robot_new.py | 3 ++- 5 files changed, 22 insertions(+), 21 deletions(-) diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index 488e2c8b..4ea737dc 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -1,5 +1,5 @@ import time -from typing import Dict, List, Optional, Tuple, Union +from typing import Dict, List, Optional, Tuple import pychrono as chrono import pychrono.irrlicht as chronoirr @@ -10,8 +10,7 @@ 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.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 diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index e1ede5a0..bf9425d6 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -1,23 +1,22 @@ import json -from typing import Dict, List, Optional, Tuple +from typing import List -import numpy as np import pychrono as chrono -from rostok.control_chrono.controller import (ConstController, - SinControllerChrono) +from rostok.block_builder_chrono.block_builder_chrono_api import \ + ChronoBlockCreatorInterface as creator +from rostok.control_chrono.tendon_controller import TendonController_2p 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, + ChronoVisManager, EnvCreator, + SingleRobotSimulation) from rostok.simulation_chrono.simulation_utils import \ - set_covering_sphere_based_position, set_covering_ellipsoid_based_position + set_covering_ellipsoid_based_position from rostok.utils.json_encoder import RostokJSONEncoder 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.tendon_controller import TendonController_2p + class ParametrizedSimulation: diff --git a/rostok/simulation_chrono/simulation_utils.py b/rostok/simulation_chrono/simulation_utils.py index dcedefa7..89d14b79 100644 --- a/rostok/simulation_chrono/simulation_utils.py +++ b/rostok/simulation_chrono/simulation_utils.py @@ -9,11 +9,11 @@ def calculate_covering_sphere(obj: ChronoEasyShapeObject): - v1 = chrono.ChVectorD(0, 0, 0) - v2 = chrono.ChVectorD(0, 0, 0) - obj.body.GetTotalAABB(bbmin=v1, bbmax=v2) - local_center = (v1 + v2) * 0.5 - radius = ((v2 - v1).Length()) * 0.5 + v_1 = chrono.ChVectorD(0, 0, 0) + v_2 = chrono.ChVectorD(0, 0, 0) + obj.body.GetTotalAABB(bbmin=v_1, bbmax=v_2) + local_center = (v_1 + v_2) * 0.5 + radius = ((v_2 - v_1).Length()) * 0.5 visual = chrono.ChSphereShape(radius) visual.SetOpacity(0.3) obj.body.AddVisualShape(visual, chrono.ChFrameD(local_center)) diff --git a/rostok/virtual_experiment/built_graph_chrono.py b/rostok/virtual_experiment/built_graph_chrono.py index a4fb9536..7be2142a 100644 --- a/rostok/virtual_experiment/built_graph_chrono.py +++ b/rostok/virtual_experiment/built_graph_chrono.py @@ -4,10 +4,12 @@ import pychrono.core as chrono from pychrono.core import ChQuaternionD, ChVectorD -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_builder_chrono_api import \ ChronoBlockCreatorInterface as creator -from rostok.block_builder_chrono.block_classes import (BLOCK_CLASS_TYPES, ChronoRevolveJoint, +from rostok.block_builder_chrono.block_classes import (BLOCK_CLASS_TYPES, + ChronoRevolveJoint, PrimitiveBody) from rostok.block_builder_chrono.block_connect import place_and_connect from rostok.graph_grammar.node import GraphGrammar diff --git a/rostok/virtual_experiment/robot_new.py b/rostok/virtual_experiment/robot_new.py index 7da09447..f29613b3 100644 --- a/rostok/virtual_experiment/robot_new.py +++ b/rostok/virtual_experiment/robot_new.py @@ -1,6 +1,7 @@ import pychrono.core as chrono -from rostok.block_builder_api.block_parameters import (DefaultFrame, FrameTransform) +from rostok.block_builder_api.block_parameters import (DefaultFrame, + FrameTransform) from rostok.control_chrono.controller import ConstController from rostok.graph_grammar.node import GraphGrammar from rostok.virtual_experiment.built_graph_chrono import BuiltGraphChrono From b736aed2d9c5b44efa3464a40533d7ca3a331593 Mon Sep 17 00:00:00 2001 From: MikhailChaikovskii Date: Wed, 4 Oct 2023 19:51:16 +0300 Subject: [PATCH 3/4] check flags --- rostok/criterion/simulation_flags.py | 34 ++++++++++++++++++++------ rostok/simulation_chrono/simulation.py | 2 +- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index a6ac2a19..b47e9715 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -62,14 +62,20 @@ def __str__(self) -> str: class EventContact(SimulationSingleEvent): """Event of contact between robot and object + Attributes: + from_body (bool): flag determines the source of contact information. """ - def __init__(self, take_from_body = False): + def __init__(self, take_from_body:bool = False): super().__init__() self.from_body = take_from_body + def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_data: Sensor): if self.state: + if __debug__: + raise Exception("The EventContact.event_check is called after occurrence in simulation.") return EventCommands.CONTINUE - if self.from_body: + + if not self.from_body: # the contact information from the object if env_data.get_amount_contacts()[0] > 0: self.state = True @@ -112,6 +118,11 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ Returns: EventCommands: return a command for simulation """ + if self.state: + if __debug__: + raise Exception("The EventContactTimeOut.event_check is called after occurrence in simulation.") + return EventCommands.STOP + # if the contact has already occurred in simulation, return CONTINUE if self.contact_event.state: return EventCommands.CONTINUE @@ -141,11 +152,15 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ Returns: EventCommands: return a command for simulation """ + if self.state: + if __debug__: + raise Exception("The EventFlyingApart.event_check is called after occurrence in simulation.") + return EventCommands.STOP + trajectory_points = robot_data.get_body_trajectory_point() # It takes the position of the first block in the list, that should be the base body base_position = trajectory_points[next(iter(trajectory_points))] - for block in trajectory_points.values(): - position = block + for position in trajectory_points.values(): if np.linalg.norm(np.array(base_position) - np.array(position)) > self.max_distance: self.state = True self.step_n = step_n @@ -177,7 +192,11 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ Returns: EventCommands: return a command for simulation """ - # Old variant: contact = env_data.get_amount_contacts()[0] > 0 + if self.state: + if __debug__: + raise Exception("The EventSlipOut.event_check is called after occurrence in simulation.") + return EventCommands.STOP + robot_contacts = robot_data.get_amount_contacts() # it works only with current rule set, where the palm/flat always has the smallest index among the bodies flat_idx_ = list(robot_contacts.keys())[0] @@ -192,7 +211,7 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ self.time_last_contact = current_time return EventCommands.CONTINUE - if (not self.time_last_contact is None): + if not (self.time_last_contact is None): if current_time - self.time_last_contact > self.reference_time: self.step_n = step_n self.state = True @@ -260,6 +279,8 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ """ if self.state: + if __debug__: + raise Exception("The EventGrasp.event_check is called after occurrence in simulation.") return EventCommands.CONTINUE if self.check_grasp_timeout(current_time): @@ -276,7 +297,6 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ print('Grasp event!', current_time) if self.simulation_stop > 0: input('press enter to continue') - # pass return EventCommands.ACTIVATE diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index 4ea737dc..d9bbd852 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -10,7 +10,7 @@ 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) +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 From 9a643bb577a975349abc776dc3534c5617e4a9c6 Mon Sep 17 00:00:00 2001 From: MikhailChaikovskii Date: Mon, 16 Oct 2023 12:49:42 +0300 Subject: [PATCH 4/4] comment --- rostok/criterion/simulation_flags.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rostok/criterion/simulation_flags.py b/rostok/criterion/simulation_flags.py index b47e9715..5f603c28 100644 --- a/rostok/criterion/simulation_flags.py +++ b/rostok/criterion/simulation_flags.py @@ -304,7 +304,7 @@ def event_check(self, current_time: float, step_n: int, robot_data: Sensor, env_ class EventStopExternalForce(SimulationSingleEvent): - + """Event that controls external force and stops simulation after reference time has passed.""" def __init__(self, grasp_event: EventGrasp, force_test_time: float): super().__init__() self.grasp_event = grasp_event