diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index d390a309..5d974111 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -230,12 +230,13 @@ def __init__(self, simulation_length, initial_vertical_pos, controller_cls = ConstController, - smc=False) -> None: + smc=False,is_fixed=False) -> None: super().__init__(step_length, simulation_length) self.event_builder_container: List[EventBuilder] = [] self.controller_cls = controller_cls self.smc = smc - self.initial_vertical_pos = initial_vertical_pos + self.initial_vertical_pos = initial_ver + self.is_fixed=is_fixed def add_event_builder(self, event_builder): @@ -285,7 +286,7 @@ def run_simulation(self, controller_data, self.controller_cls, Frame=FrameTransform([0, self.initial_vertical_pos, 0], [0,0,0,1]), - starting_positions=starting_positions, is_fixed=False) + starting_positions=starting_positions, is_fixed=self.is_fixed) # setup parameters for the data store diff --git a/tests_jupyter/track_sim.py b/tests_jupyter/track_sim.py new file mode 100644 index 00000000..3db73959 --- /dev/null +++ b/tests_jupyter/track_sim.py @@ -0,0 +1,508 @@ +# %% +import numpy as np +import pychrono as chrono + +from rostok.block_builder_api.block_blueprints import (PrimitiveBodyBlueprint, + RevolveJointBlueprint, + TransformBlueprint) +from rostok.block_builder_api.block_parameters import JointInputType +from rostok.block_builder_api.easy_body_shapes import Box, Cylinder, Ellipsoid +from rostok.block_builder_chrono.blocks_utils import FrameTransform +from rostok.graph_grammar import rule_vocabulary +from rostok.graph_grammar.node import ROOT +from rostok.graph_grammar.node_vocabulary import NodeVocabulary +from rostok.utils.dataset_materials.material_dataclass_manipulating import ( + DefaultChronoMaterialNSC, DefaultChronoMaterialSMC) +from rostok.graph_grammar.node import GraphGrammar +from rostok.graph_grammar.graph_utils import plot_graph + +# %% +def get_density_box(mass: float, box: Box): + volume = box.height_z * box.length_y * box.width_x + return mass / volume + +def rotation_x(alpha): + quat_X_ang_alpha = chrono.Q_from_AngX(np.deg2rad(alpha)) + return [quat_X_ang_alpha.e0, quat_X_ang_alpha.e1, quat_X_ang_alpha.e2, quat_X_ang_alpha.e3] + +def rotation_y(alpha): + quat_Y_ang_alpha = chrono.Q_from_AngY(np.deg2rad(alpha)) + return [quat_Y_ang_alpha.e0, quat_Y_ang_alpha.e1, quat_Y_ang_alpha.e2, quat_Y_ang_alpha.e3] + +def rotation_z(alpha): + quat_Z_ang_alpha = chrono.Q_from_AngZ(np.deg2rad(alpha)) + return [quat_Z_ang_alpha.e0, quat_Z_ang_alpha.e1, quat_Z_ang_alpha.e2, quat_Z_ang_alpha.e3] + +def_mat = DefaultChronoMaterialNSC() + +# %% +node_vocab = NodeVocabulary() +node_vocab.add_node(ROOT) +node_vocab.create_node(label="MB") +node_vocab.create_node(label="L") +node_vocab.create_node(label="W") +node_vocab.create_node(label="J") +node_vocab.create_node(label="R") +node_vocab.create_node(label="G") + +node_vocab.create_node(label="XT") +node_vocab.create_node(label="ZT") + + +# %% +from copy import deepcopy +# blueprint for the palm +main_body = [] + +for i in range(10): + main_body.append(PrimitiveBodyBlueprint( + Box(0.5, 0.1, 0.3), material=def_mat, color=[255, 0, 0], density=100+i*100,is_collide=True)) + +wheel_def_mat = deepcopy(def_mat) +wheel_def_mat.Friction = 0.5 +wheel_def_mat.RollingFriction=0.001 +wheel_def_mat.SpinningFriction=0.001 +wheel_body_cyl = PrimitiveBodyBlueprint( + Cylinder(0.03, 0.02), material=wheel_def_mat, color=[120, 120, 120], density=1000) + +wheel_body_cyl_wide = PrimitiveBodyBlueprint( + Cylinder(0.03, 0.06), material=wheel_def_mat, color=[120, 120, 120], density=3000) + +wheel_body_ell = PrimitiveBodyBlueprint( + Ellipsoid(0.06,0.06, 0.04), material=def_mat, color=[120, 120, 120], density=1000) + +# blueprint for the base +base = PrimitiveBodyBlueprint(Box(0.02, 0.03, 0.02), + material=def_mat, + color=[120, 120, 0], + density=1000) + +# sets effective density for the links, the real links are considered to be extendable. + +length_link = np.linspace(0.05, 0.1, 5) +# create link blueprints using mass and length parameters +link = list(map(lambda x: PrimitiveBodyBlueprint(Box(0.02, x, 0.02), + material=def_mat, + color=[0, 120, 255], + density=1000), length_link)) + +dummy_link = PrimitiveBodyBlueprint( + Box(0.01, 0.001, 0.01), material=def_mat, density=1000000) + +# %% +0.3**2*3.14*0.2*500/(0.01**3*0.1) + + +# %% +for i, bp in enumerate(main_body): + node_vocab.create_node(label=f"MB{i}", is_terminal=True, block_blueprint=bp) + +for i, bp in enumerate(link): + node_vocab.create_node(label=f"L{i}", is_terminal=True, block_blueprint=bp) + +node_vocab.create_node(label="B", is_terminal=True,block_blueprint=base) +node_vocab.create_node(label="WC", is_terminal=True,block_blueprint=wheel_body_cyl) +node_vocab.create_node(label="WC2", is_terminal=True,block_blueprint=wheel_body_cyl_wide) +node_vocab.create_node(label="WE", is_terminal=True,block_blueprint=wheel_body_ell) + +node_vocab.create_node(label="DB", is_terminal=True, + block_blueprint=dummy_link) + +# %% +x_translation_values = np.linspace(0.0,0.25,5) +X_TRANSLATIONS_POSITIVE = list( + map(lambda x: FrameTransform([x, 0, 0], [1, 0, 0, 0]), x_translation_values)) +X_TRANSLATIONS_NEGATIVE=list( + map(lambda x: FrameTransform([-x, 0, 0], [1, 0, 0, 0]), x_translation_values)) + # create transform blueprints from the values +x_translation_positive = list( + map(lambda x: TransformBlueprint(x), X_TRANSLATIONS_POSITIVE)) +x_translation_negative = list( + map(lambda x: TransformBlueprint(x), X_TRANSLATIONS_NEGATIVE)) + +z_translation_values = np.linspace(0.0,0.15,5) +Z_TRANSLATIONS_POSITIVE = list( + map(lambda x: FrameTransform([0, 0, x], [1, 0, 0, 0]), z_translation_values)) +Z_TRANSLATIONS_NEGATIVE = list( + map(lambda x: FrameTransform([0, 0, -x], [1, 0, 0, 0]), z_translation_values)) + +z_translation_positive = list( + map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_POSITIVE)) +z_translation_negative = list( + map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_NEGATIVE)) + +# %% +for i, bp in enumerate(x_translation_positive): + node_vocab.create_node(label=f"XTP{i}", is_terminal=True, block_blueprint=bp) + +for i, bp in enumerate(x_translation_negative): + node_vocab.create_node(label=f"XTN{i}", is_terminal=True, block_blueprint=bp) + +for i, bp in enumerate(z_translation_positive): + node_vocab.create_node(label=f"ZTP{i}", is_terminal=True, block_blueprint=bp) + +for i, bp in enumerate(z_translation_negative): + node_vocab.create_node(label=f"ZTN{i}", is_terminal=True, block_blueprint=bp) + +# %% +TURN_X = FrameTransform([0, 0, 0], rotation_x(90)) +turn_x_bp = TransformBlueprint(TURN_X) + +TURN_Y = FrameTransform([0, 0, 0], rotation_y(90)) +turn_y_bp = TransformBlueprint(TURN_Y) + +TURN_Z = FrameTransform([0, 0, 0], rotation_z(90)) +turn_z_bp = TransformBlueprint(TURN_Z) + +# %% +node_vocab.create_node(label='XR',is_terminal=True, block_blueprint=turn_x_bp) +node_vocab.create_node(label='YR',is_terminal=True, block_blueprint=turn_y_bp) +node_vocab.create_node(label='ZR',is_terminal=True, block_blueprint=turn_z_bp) + +# %% +stiffness = np.linspace(0.1,5,50) + +no_control_joint = list( + map(lambda x: RevolveJointBlueprint(JointInputType.UNCONTROL, + stiffness=x, + damping=0.005, + equilibrium_position=0), stiffness)) +motor_bp = RevolveJointBlueprint(JointInputType.TORQUE, stiffness=0, damping=0.002) +neutral_bp = RevolveJointBlueprint(JointInputType.UNCONTROL, stiffness=0, damping=0.002) + +# %% +node_vocab.create_node(label="RM", is_terminal=True, block_blueprint=motor_bp) +node_vocab.create_node(label="LM", is_terminal=True, block_blueprint=motor_bp) +node_vocab.create_node(label="NM", is_terminal=True, block_blueprint=neutral_bp) + +for i, bp in enumerate(no_control_joint): + node_vocab.create_node(label=f"J{i}", is_terminal=True, block_blueprint=bp) + +# %% +rule_vocab = rule_vocabulary.RuleVocabulary(node_vocab) +# structural rules +rule_vocab.create_rule("Init", ["ROOT"], ["MB"], 0, 0,[]) +rule_vocab.create_rule("Add_Leg_Base",["MB"],["MB","XT","ZT","B","G"],0,0,[(0,1),(1,2),(2,3),(3,4)]) + +rule_vocab.create_rule("Add_Dummy", ["G"], ["J","DB", "G"],0,0,[(0, 1), (1, 2)]) +rule_vocab.create_rule("Extension",["G"],["J","L", "G"], 0,2,[(0, 1), (1, 2)]) +rule_vocab.create_rule("R_Wheel",["G"], ["RM","W"],0,0,[(0,1)]) +rule_vocab.create_rule("L_Wheel",["G"], ["LM","W"],0,0,[(0,1)]) +rule_vocab.create_rule("Wheel",["G"], ["NM","W"],0,0,[(0,1)]) +rule_vocab.create_rule("Rotation",["G"],["R","G"],0,0,[(0,1)]) +rule_vocab.create_rule("Remove",["G"],[],0,0,[]) + +# %% +for i in range(len(main_body)): + rule_vocab.create_rule(f'Terminal_Main_Body{i}',["MB"],[f"MB{i}"],0,0,[]) + +for i in range(len(link)): + rule_vocab.create_rule(f'Terminal_Link{i}',["L"],[f"L{i}"],0,0,[]) + +for i in range(len(no_control_joint)): + rule_vocab.create_rule(f'Terminal_Joint{i}',["J"],[f"J{i}"],0,0,[]) + +for i in range(len(x_translation_positive)): + rule_vocab.create_rule(f'Positive_Translation_X{i}',["XT"],[f"XTP{i}"],0,0,[]) +for i in range(len(x_translation_negative)): + rule_vocab.create_rule(f'Negative_Translation_X{i}',["XT"],[f"XTN{i}"],0,0,[]) +for i in range(len(z_translation_positive)): + rule_vocab.create_rule(f'Positive_Translation_Z{i}',["ZT"],[f"ZTP{i}"],0,0,[]) +for i in range(len(z_translation_negative)): + rule_vocab.create_rule(f'Negative_Translation_Z{i}',["ZT"],[f"ZTN{i}"],0,0,[]) + +rule_vocab.create_rule(f'Rotation_X',["R"],["XR"],0,0,[]) +rule_vocab.create_rule(f'Rotation_Y',["R"],["YR"],0,0,[]) +rule_vocab.create_rule(f'Rotation_Z',["R"],["ZR"],0,0,[]) + +rule_vocab.create_rule("Cyl_Wheel",["W"],["WC"],0,0,[]) +rule_vocab.create_rule("Cyl_Wheel2",["W"],["WC2"],0,0,[]) +rule_vocab.create_rule("Ell_Wheel",["W"],["WE"],0,0,[]) + +# %% +def get_initial_mech(rule_vocabul): + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3", "Extension","Extension","Remove", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint2','Terminal_Joint2', + + "Add_Leg_Base","Positive_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint2','Terminal_Joint2', "Remove", + + "Add_Leg_Base","Negative_Translation_X4","Negative_Translation_Z0", "Extension","Extension", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint2','Terminal_Joint2', "Wheel", "Ell_Wheel", + + 'Terminal_Main_Body1' + + ] + + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + starting_angles = [ [15,-30,0,0], [-15,30,0,0], [0,10,0,0]] + return graph, starting_angles + +def get_zero_rotation_four_legs(rule_vocabul): + i=3 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X2","Positive_Translation_Z2", "Extension","Extension", f'Terminal_Joint{i}',f'Terminal_Joint{i}', + "R_Wheel", "Cyl_Wheel", "Terminal_Link2","Terminal_Link2", + + "Add_Leg_Base","Positive_Translation_X2","Negative_Translation_Z2", "Extension","Extension", f'Terminal_Joint{i}',f'Terminal_Joint{i}', + "R_Wheel", "Cyl_Wheel", "Terminal_Link2","Terminal_Link2", + + "Add_Leg_Base","Negative_Translation_X2","Positive_Translation_Z2", "Extension","Extension", f'Terminal_Joint{i}',f'Terminal_Joint{i}', + "R_Wheel", "Cyl_Wheel", "Terminal_Link2","Terminal_Link2", + + "Add_Leg_Base","Negative_Translation_X2","Negative_Translation_Z2", "Extension","Extension", f'Terminal_Joint{i}',f'Terminal_Joint{i}', + "R_Wheel", "Cyl_Wheel", "Terminal_Link2","Terminal_Link2", + + 'Terminal_Main_Body3' + + ] + + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + starting_angles = [ [15,-30,0,0], [15,-30,0,0], [15,-30,0,0], [15,-30,0,0]] + return graph, starting_angles + +def get_rotated_four_legs(rule_vocabul): + l=1 + j= 5 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X2","Positive_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}', "Rotation","R_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}",'Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X2","Negative_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}',"Rotation","L_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}", 'Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X2","Positive_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}', f'Terminal_Joint{j}',"Rotation","R_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}", 'Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X2","Negative_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}', "Rotation","L_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}",'Rotation_Y', + + 'Terminal_Main_Body2' + + ] + + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]] + return graph, starting_angles + +def get_rotated_four_legs(rule_vocabul): + l=1 + j= 5 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X2","Positive_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}', "Rotation","R_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}",'Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X2","Negative_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}',"Rotation","L_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}", 'Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X2","Positive_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}', f'Terminal_Joint{j}',"Rotation","R_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}", 'Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X2","Negative_Translation_Z2","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{j}',f'Terminal_Joint{j}', "Rotation","L_Wheel", "Cyl_Wheel", + f"Terminal_Link{l}",f"Terminal_Link{l}",'Rotation_Y', + + 'Terminal_Main_Body2' + + ] + + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]] + return graph, starting_angles + +def get_rotated_four_legs_with_db(rule_vocabul, stiffness_idx): + i=stiffness_idx + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy","Rotation","R_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint10','Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}',"Add_Dummy","Rotation","L_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint10','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}', f'Terminal_Joint{i}',"Add_Dummy","Rotation","R_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint10','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy", "Rotation","L_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint10','Rotation_Y', + + 'Terminal_Main_Body4' + + ] + + for rule in rules: + graph.apply_rule(rule_vocab.get_rule(rule)) + starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]] + + return graph, starting_angles + + +def get_rotated_six_legs_with_db(rule_vocabul, stiffness_idx): + i=stiffness_idx + k=10 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy","Rotation","R_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", f'Terminal_Joint{k}','Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}',"Add_Dummy","Rotation","L_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", f'Terminal_Joint{k}','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}', f'Terminal_Joint{i}',"Add_Dummy","Rotation","R_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", f'Terminal_Joint{k}','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy", "Rotation","L_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3",f'Terminal_Joint{k}','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X0","Positive_Translation_Z4","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}', f'Terminal_Joint{i}',"Add_Dummy","Rotation","R_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3", f'Terminal_Joint{k}','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X0","Negative_Translation_Z4","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy", "Rotation","L_Wheel", "Cyl_Wheel2", + "Terminal_Link3","Terminal_Link3",f'Terminal_Joint{k}','Rotation_Y', + + 'Terminal_Main_Body1' + + ] + + for rule in rules: + graph.apply_rule(rule_vocab.get_rule(rule)) + starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]] + + return graph, starting_angles + + +def get_rotated_six_legs_with_db_ell(rule_vocabul, stiffness_idx): + i=stiffness_idx + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy","Rotation","R_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint40','Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}',"Add_Dummy","Rotation","L_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint40','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}', f'Terminal_Joint{i}',"Add_Dummy","Rotation","R_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint40','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy", "Rotation","L_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint40','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X0","Positive_Translation_Z4","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}', f'Terminal_Joint{i}',"Add_Dummy","Rotation","R_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3", 'Terminal_Joint40','Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X0","Negative_Translation_Z4","Rotation", + 'Rotation_Y', "Extension","Extension",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Add_Dummy", "Rotation","L_Wheel", "Ell_Wheel", + "Terminal_Link3","Terminal_Link3",'Terminal_Joint40','Rotation_Y', + + 'Terminal_Main_Body1' + + ] + + for rule in rules: + graph.apply_rule(rule_vocab.get_rule(rule)) + starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]] + + return graph, starting_angles + + +# %% +from rostok.control_chrono.controller import SimpleKeyBoardController + + + +# %% + + + +# %% +from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint +from rostok.control_chrono.controller import SimpleKeyBoardController +from rostok.simulation_chrono.simulation_scenario import WalkingScenario +import pychrono as chrono +from rostok.utils.dataset_materials.material_dataclass_manipulating import DefaultChronoMaterialNSC +#from wheels import get_stiff_wheels, get_wheels, get_stiff_wheels_ell, get_stiff_wheels_4 +from rostok.graph_grammar.graph_utils import plot_graph +from rostok.block_builder_chrono.block_builder_chrono_api import \ + ChronoBlockCreatorInterface +from rostok.block_builder_api.easy_body_shapes import Box + +def create_bump_track(): + def_mat = DefaultChronoMaterialNSC() + floor = ChronoBlockCreatorInterface.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0])) + chrono_material = chrono.ChMaterialSurfaceNSC() + #chrono_material.SetFriction(0.67) + mesh = chrono.ChBodyEasyMesh("Bump.obj", 8000, True, True, True, chrono_material, 0.002) + floor.body = mesh + floor.body.SetNameString("Floor") + floor.body.SetPos(chrono.ChVectorD(1.5,-0.07,0)) + floor.body.GetVisualShape(0).SetTexture("chess.png", 0.03, 0.03) + floor.body.SetBodyFixed(True) + return floor + +def create_track(): + def_mat = DefaultChronoMaterialNSC() + floor = ChronoBlockCreatorInterface.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0])) + chrono_material = chrono.ChMaterialSurfaceNSC() + #chrono_material.SetFriction(0.67) + mesh = chrono.ChBodyEasyMesh(".\\TRACKMANIA.obj", 8000, True, True, True, chrono_material, 0.002) + floor.body = mesh + floor.body.SetNameString("Floor") + floor.body.SetPos(chrono.ChVectorD(6.6,0.02,5.2)) + floor.body.GetVisualShape(0).SetTexture(".\\chess.png", 0.0003, 0.0003) + floor.body.GetVisualShape(0).SetOpacity(0.2) + floor.body.SetBodyFixed(True) + return floor + + +graph,startings = get_rotated_six_legs_with_db(rule_vocab, 45) +floor = create_track() + +scenario = WalkingScenario(0.0001, 10000, SimpleKeyBoardController) +scenario.set_floor(floor) +#graph = get_stiff_wheels_4() + +parameters = {} +parameters["forward"] = 1 +parameters["reverse"]= 1 +parameters["forward_rotate"] = 1 +parameters["reverse_rotate"] = 1 + +scenario.run_simulation(graph, parameters, starting_positions=startings, vis = True, delay=True, is_follow_camera = True) + + diff --git a/tests_jupyter/wheels_ruleset_func.ipynb b/tests_jupyter/wheels_ruleset_func.ipynb new file mode 100644 index 00000000..30c95529 --- /dev/null +++ b/tests_jupyter/wheels_ruleset_func.ipynb @@ -0,0 +1,796 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 19, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import pychrono as chrono\n", + "\n", + "from rostok.block_builder_api.block_blueprints import (PrimitiveBodyBlueprint,\n", + " RevolveJointBlueprint,\n", + " TransformBlueprint)\n", + "from rostok.block_builder_api.block_parameters import JointInputType\n", + "from rostok.block_builder_api.easy_body_shapes import Box, Cylinder, Ellipsoid\n", + "from rostok.block_builder_chrono.blocks_utils import FrameTransform\n", + "from rostok.graph_grammar import rule_vocabulary\n", + "from rostok.graph_grammar.node import ROOT\n", + "from rostok.graph_grammar.node_vocabulary import NodeVocabulary\n", + "from rostok.utils.dataset_materials.material_dataclass_manipulating import (\n", + " DefaultChronoMaterialNSC, DefaultChronoMaterialSMC)\n", + "from rostok.graph_grammar.node import GraphGrammar\n", + "from rostok.graph_grammar.graph_utils import plot_graph" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": {}, + "outputs": [], + "source": [ + "def get_density_box(mass: float, box: Box):\n", + " volume = box.height_z * box.length_y * box.width_x\n", + " return mass / volume\n", + "\n", + "def rotation_x(alpha):\n", + " quat_X_ang_alpha = chrono.Q_from_AngX(np.deg2rad(alpha))\n", + " return [quat_X_ang_alpha.e0, quat_X_ang_alpha.e1, quat_X_ang_alpha.e2, quat_X_ang_alpha.e3]\n", + "\n", + "def rotation_y(alpha):\n", + " quat_Y_ang_alpha = chrono.Q_from_AngY(np.deg2rad(alpha))\n", + " return [quat_Y_ang_alpha.e0, quat_Y_ang_alpha.e1, quat_Y_ang_alpha.e2, quat_Y_ang_alpha.e3]\n", + "\n", + "def rotation_z(alpha):\n", + " quat_Z_ang_alpha = chrono.Q_from_AngZ(np.deg2rad(alpha))\n", + " return [quat_Z_ang_alpha.e0, quat_Z_ang_alpha.e1, quat_Z_ang_alpha.e2, quat_Z_ang_alpha.e3]\n", + "\n", + "def_mat = DefaultChronoMaterialNSC()" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [], + "source": [ + "node_vocab = NodeVocabulary()\n", + "node_vocab.add_node(ROOT)\n", + "node_vocab.create_node(label=\"MB\")\n", + "node_vocab.create_node(label=\"L\")\n", + "node_vocab.create_node(label=\"W\")\n", + "node_vocab.create_node(label=\"J\")\n", + "node_vocab.create_node(label=\"R\")\n", + "node_vocab.create_node(label=\"G\")\n", + "\n", + "node_vocab.create_node(label=\"XT\")\n", + "node_vocab.create_node(label=\"ZT\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": 22, + "metadata": {}, + "outputs": [], + "source": [ + "from copy import deepcopy\n", + "# blueprint for the palm\n", + "main_body = []\n", + "\n", + "for i in range(10):\n", + " main_body.append(PrimitiveBodyBlueprint(\n", + " Box(0.5, 0.1, 0.3), material=def_mat, color=[255, 0, 0], density=100+i*100,is_collide=True))\n", + " \n", + "wheel_def_mat = deepcopy(def_mat)\n", + "wheel_def_mat.Friction = 0.5\n", + "wheel_def_mat.RollingFriction=0.0001\n", + "wheel_def_mat.SpinningFriction=0.0001\n", + "wheel_body_cyl = PrimitiveBodyBlueprint(\n", + " Cylinder(0.03, 0.02), material=wheel_def_mat, color=[120, 120, 120], density=1000)\n", + "\n", + "wheel_body_cyl_wide = PrimitiveBodyBlueprint(\n", + " Cylinder(0.03, 0.06), material=wheel_def_mat, color=[120, 120, 120], density=3000)\n", + "\n", + "wheel_body_ell = PrimitiveBodyBlueprint(\n", + " Ellipsoid(0.06,0.06, 0.04), material=def_mat, color=[120, 120, 120], density=1000)\n", + "\n", + "# blueprint for the base\n", + "base = PrimitiveBodyBlueprint(Box(0.02, 0.03, 0.02),\n", + " material=def_mat,\n", + " color=[120, 120, 0],\n", + " density=1000)\n", + "\n", + "# sets effective density for the links, the real links are considered to be extendable.\n", + "\n", + "length_link = np.linspace(0.05, 0.1, 5)\n", + "# create link blueprints using mass and length parameters\n", + "link = list(map(lambda x: PrimitiveBodyBlueprint(Box(0.02, x, 0.02),\n", + " material=def_mat,\n", + " color=[0, 120, 255],\n", + " density=1000), length_link))\n", + "\n", + "dummy_link = PrimitiveBodyBlueprint(\n", + " Box(0.01, 0.001, 0.01), material=def_mat, density=1000000)" + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "282600000.0" + ] + }, + "execution_count": 23, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "0.3**2*3.14*0.2*500/(0.01**3*0.1)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 24, + "metadata": {}, + "outputs": [], + "source": [ + "for i, bp in enumerate(main_body):\n", + " node_vocab.create_node(label=f\"MB{i}\", is_terminal=True, block_blueprint=bp)\n", + "\n", + "for i, bp in enumerate(link):\n", + " node_vocab.create_node(label=f\"L{i}\", is_terminal=True, block_blueprint=bp)\n", + "\n", + "node_vocab.create_node(label=\"B\", is_terminal=True,block_blueprint=base)\n", + "node_vocab.create_node(label=\"WC\", is_terminal=True,block_blueprint=wheel_body_cyl)\n", + "node_vocab.create_node(label=\"WC2\", is_terminal=True,block_blueprint=wheel_body_cyl_wide)\n", + "node_vocab.create_node(label=\"WE\", is_terminal=True,block_blueprint=wheel_body_ell)\n", + "\n", + "node_vocab.create_node(label=\"DB\", is_terminal=True,\n", + " block_blueprint=dummy_link)" + ] + }, + { + "cell_type": "code", + "execution_count": 25, + "metadata": {}, + "outputs": [], + "source": [ + "x_translation_values = np.linspace(0.0,0.25,5)\n", + "X_TRANSLATIONS_POSITIVE = list(\n", + " map(lambda x: FrameTransform([x, 0, 0], [1, 0, 0, 0]), x_translation_values))\n", + "X_TRANSLATIONS_NEGATIVE=list(\n", + " map(lambda x: FrameTransform([-x, 0, 0], [1, 0, 0, 0]), x_translation_values))\n", + " # create transform blueprints from the values\n", + "x_translation_positive = list(\n", + " map(lambda x: TransformBlueprint(x), X_TRANSLATIONS_POSITIVE))\n", + "x_translation_negative = list(\n", + " map(lambda x: TransformBlueprint(x), X_TRANSLATIONS_NEGATIVE))\n", + "\n", + "z_translation_values = np.linspace(0.0,0.15,5)\n", + "Z_TRANSLATIONS_POSITIVE = list(\n", + " map(lambda x: FrameTransform([0, 0, x], [1, 0, 0, 0]), z_translation_values))\n", + "Z_TRANSLATIONS_NEGATIVE = list(\n", + " map(lambda x: FrameTransform([0, 0, -x], [1, 0, 0, 0]), z_translation_values))\n", + "\n", + "z_translation_positive = list(\n", + " map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_POSITIVE))\n", + "z_translation_negative = list(\n", + " map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_NEGATIVE))" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": {}, + "outputs": [], + "source": [ + "for i, bp in enumerate(x_translation_positive):\n", + " node_vocab.create_node(label=f\"XTP{i}\", is_terminal=True, block_blueprint=bp)\n", + "\n", + "for i, bp in enumerate(x_translation_negative):\n", + " node_vocab.create_node(label=f\"XTN{i}\", is_terminal=True, block_blueprint=bp)\n", + "\n", + "for i, bp in enumerate(z_translation_positive):\n", + " node_vocab.create_node(label=f\"ZTP{i}\", is_terminal=True, block_blueprint=bp)\n", + "\n", + "for i, bp in enumerate(z_translation_negative):\n", + " node_vocab.create_node(label=f\"ZTN{i}\", is_terminal=True, block_blueprint=bp)" + ] + }, + { + "cell_type": "code", + "execution_count": 27, + "metadata": {}, + "outputs": [], + "source": [ + "TURN_X = FrameTransform([0, 0, 0], rotation_x(90))\n", + "turn_x_bp = TransformBlueprint(TURN_X)\n", + "\n", + "TURN_Y = FrameTransform([0, 0, 0], rotation_y(90))\n", + "turn_y_bp = TransformBlueprint(TURN_Y)\n", + "\n", + "TURN_Z = FrameTransform([0, 0, 0], rotation_z(90))\n", + "turn_z_bp = TransformBlueprint(TURN_Z)" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": {}, + "outputs": [], + "source": [ + "node_vocab.create_node(label='XR',is_terminal=True, block_blueprint=turn_x_bp)\n", + "node_vocab.create_node(label='YR',is_terminal=True, block_blueprint=turn_y_bp)\n", + "node_vocab.create_node(label='ZR',is_terminal=True, block_blueprint=turn_z_bp)" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [], + "source": [ + "stiffness = np.linspace(0.1,5,50)\n", + "\n", + "no_control_joint = list(\n", + " map(lambda x: RevolveJointBlueprint(JointInputType.UNCONTROL,\n", + " stiffness=x,\n", + " damping=0.005,\n", + " equilibrium_position=0), stiffness))\n", + "motor_bp = RevolveJointBlueprint(JointInputType.TORQUE, stiffness=0, damping=0.002)\n", + "neutral_bp = RevolveJointBlueprint(JointInputType.UNCONTROL, stiffness=0, damping=0.002)" + ] + }, + { + "cell_type": "code", + "execution_count": 30, + "metadata": {}, + "outputs": [], + "source": [ + "node_vocab.create_node(label=\"RM\", is_terminal=True, block_blueprint=motor_bp)\n", + "node_vocab.create_node(label=\"LM\", is_terminal=True, block_blueprint=motor_bp)\n", + "node_vocab.create_node(label=\"NM\", is_terminal=True, block_blueprint=neutral_bp)\n", + "\n", + "for i, bp in enumerate(no_control_joint):\n", + " node_vocab.create_node(label=f\"J{i}\", is_terminal=True, block_blueprint=bp)" + ] + }, + { + "cell_type": "code", + "execution_count": 31, + "metadata": {}, + "outputs": [], + "source": [ + "rule_vocab = rule_vocabulary.RuleVocabulary(node_vocab)\n", + "# structural rules\n", + "rule_vocab.create_rule(\"Init\", [\"ROOT\"], [\"MB\"], 0, 0,[])\n", + "rule_vocab.create_rule(\"Add_Leg_Base\",[\"MB\"],[\"MB\",\"XT\",\"ZT\",\"B\",\"G\"],0,0,[(0,1),(1,2),(2,3),(3,4)])\n", + "\n", + "rule_vocab.create_rule(\"Add_Dummy\", [\"G\"], [\"J\",\"DB\", \"G\"],0,0,[(0, 1), (1, 2)])\n", + "rule_vocab.create_rule(\"Extension\",[\"G\"],[\"J\",\"L\", \"G\"], 0,2,[(0, 1), (1, 2)])\n", + "rule_vocab.create_rule(\"R_Wheel\",[\"G\"], [\"RM\",\"W\"],0,0,[(0,1)])\n", + "rule_vocab.create_rule(\"L_Wheel\",[\"G\"], [\"LM\",\"W\"],0,0,[(0,1)])\n", + "rule_vocab.create_rule(\"Wheel\",[\"G\"], [\"NM\",\"W\"],0,0,[(0,1)])\n", + "rule_vocab.create_rule(\"Rotation\",[\"G\"],[\"R\",\"G\"],0,0,[(0,1)])\n", + "rule_vocab.create_rule(\"Remove\",[\"G\"],[],0,0,[])" + ] + }, + { + "cell_type": "code", + "execution_count": 32, + "metadata": {}, + "outputs": [], + "source": [ + "for i in range(len(main_body)):\n", + " rule_vocab.create_rule(f'Terminal_Main_Body{i}',[\"MB\"],[f\"MB{i}\"],0,0,[])\n", + "\n", + "for i in range(len(link)):\n", + " rule_vocab.create_rule(f'Terminal_Link{i}',[\"L\"],[f\"L{i}\"],0,0,[])\n", + "\n", + "for i in range(len(no_control_joint)):\n", + " rule_vocab.create_rule(f'Terminal_Joint{i}',[\"J\"],[f\"J{i}\"],0,0,[])\n", + "\n", + "for i in range(len(x_translation_positive)):\n", + " rule_vocab.create_rule(f'Positive_Translation_X{i}',[\"XT\"],[f\"XTP{i}\"],0,0,[])\n", + "for i in range(len(x_translation_negative)):\n", + " rule_vocab.create_rule(f'Negative_Translation_X{i}',[\"XT\"],[f\"XTN{i}\"],0,0,[])\n", + "for i in range(len(z_translation_positive)):\n", + " rule_vocab.create_rule(f'Positive_Translation_Z{i}',[\"ZT\"],[f\"ZTP{i}\"],0,0,[])\n", + "for i in range(len(z_translation_negative)):\n", + " rule_vocab.create_rule(f'Negative_Translation_Z{i}',[\"ZT\"],[f\"ZTN{i}\"],0,0,[])\n", + "\n", + "rule_vocab.create_rule(f'Rotation_X',[\"R\"],[\"XR\"],0,0,[])\n", + "rule_vocab.create_rule(f'Rotation_Y',[\"R\"],[\"YR\"],0,0,[])\n", + "rule_vocab.create_rule(f'Rotation_Z',[\"R\"],[\"ZR\"],0,0,[])\n", + "\n", + "rule_vocab.create_rule(\"Cyl_Wheel\",[\"W\"],[\"WC\"],0,0,[])\n", + "rule_vocab.create_rule(\"Cyl_Wheel2\",[\"W\"],[\"WC2\"],0,0,[])\n", + "rule_vocab.create_rule(\"Ell_Wheel\",[\"W\"],[\"WE\"],0,0,[])" + ] + }, + { + "cell_type": "code", + "execution_count": 33, + "metadata": {}, + "outputs": [], + "source": [ + "def get_initial_mech(rule_vocabul):\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Positive_Translation_Z3\", \"Extension\",\"Extension\",\"Remove\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint2','Terminal_Joint2',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint2','Terminal_Joint2', \"Remove\",\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Negative_Translation_Z0\", \"Extension\",\"Extension\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint2','Terminal_Joint2', \"Wheel\", \"Ell_Wheel\",\n", + "\n", + " 'Terminal_Main_Body1'\n", + "\n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocabul.get_rule(rule))\n", + "\n", + " starting_angles = [ [15,-30,0,0], [-15,30,0,0], [0,10,0,0]]\n", + " return graph, starting_angles\n", + "\n", + "def get_zero_rotation_four_legs(rule_vocabul):\n", + " i=3\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Positive_Translation_Z2\", \"Extension\",\"Extension\", f'Terminal_Joint{i}',f'Terminal_Joint{i}',\n", + " \"R_Wheel\", \"Cyl_Wheel\", \"Terminal_Link2\",\"Terminal_Link2\",\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Negative_Translation_Z2\", \"Extension\",\"Extension\", f'Terminal_Joint{i}',f'Terminal_Joint{i}',\n", + " \"R_Wheel\", \"Cyl_Wheel\", \"Terminal_Link2\",\"Terminal_Link2\",\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Positive_Translation_Z2\", \"Extension\",\"Extension\", f'Terminal_Joint{i}',f'Terminal_Joint{i}',\n", + " \"R_Wheel\", \"Cyl_Wheel\", \"Terminal_Link2\",\"Terminal_Link2\",\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Negative_Translation_Z2\", \"Extension\",\"Extension\", f'Terminal_Joint{i}',f'Terminal_Joint{i}',\n", + " \"R_Wheel\", \"Cyl_Wheel\", \"Terminal_Link2\",\"Terminal_Link2\",\n", + "\n", + " 'Terminal_Main_Body3'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocabul.get_rule(rule))\n", + "\n", + " starting_angles = [ [15,-30,0,0], [15,-30,0,0], [15,-30,0,0], [15,-30,0,0]]\n", + " return graph, starting_angles\n", + "\n", + "def get_rotated_four_legs(rule_vocabul):\n", + " l=1\n", + " j= 5\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Positive_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}', \"Rotation\",\"R_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\",'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Negative_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}',\"Rotation\",\"L_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\", 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Positive_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}', f'Terminal_Joint{j}',\"Rotation\",\"R_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\", 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Negative_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}', \"Rotation\",\"L_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\",'Rotation_Y',\n", + "\n", + " 'Terminal_Main_Body2'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocabul.get_rule(rule))\n", + "\n", + " starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]]\n", + " return graph, starting_angles\n", + "\n", + "def get_rotated_four_legs(rule_vocabul):\n", + " l=1\n", + " j= 5\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Positive_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}', \"Rotation\",\"R_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\",'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X2\",\"Negative_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}',\"Rotation\",\"L_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\", 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Positive_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}', f'Terminal_Joint{j}',\"Rotation\",\"R_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\", 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X2\",\"Negative_Translation_Z2\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{j}',f'Terminal_Joint{j}', \"Rotation\",\"L_Wheel\", \"Cyl_Wheel\",\n", + " f\"Terminal_Link{l}\",f\"Terminal_Link{l}\",'Rotation_Y',\n", + "\n", + " 'Terminal_Main_Body2'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocabul.get_rule(rule))\n", + "\n", + " starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]]\n", + " return graph, starting_angles\n", + "\n", + "def get_rotated_four_legs_with_db(rule_vocabul, stiffness_idx):\n", + " i=stiffness_idx\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint10','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"L_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint10','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}', f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint10','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\", \"Rotation\",\"L_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint10','Rotation_Y',\n", + "\n", + " 'Terminal_Main_Body4'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocab.get_rule(rule))\n", + " starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]]\n", + "\n", + " return graph, starting_angles\n", + "\n", + "\n", + "def get_rotated_six_legs_with_db(rule_vocabul, stiffness_idx):\n", + " i=stiffness_idx\n", + " k=10\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"L_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}', f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\", \"Rotation\",\"L_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X0\",\"Positive_Translation_Z4\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}', f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X0\",\"Negative_Translation_Z4\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\", \"Rotation\",\"L_Wheel\", \"Cyl_Wheel2\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",f'Terminal_Joint{k}','Rotation_Y',\n", + "\n", + " 'Terminal_Main_Body2'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocab.get_rule(rule))\n", + " starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]]\n", + "\n", + " return graph, starting_angles\n", + "\n", + "\n", + "def get_rotated_six_legs_with_db_ell(rule_vocabul, stiffness_idx):\n", + " i=stiffness_idx\n", + " graph = GraphGrammar()\n", + " rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint40','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"L_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint40','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}', f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint40','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\", \"Rotation\",\"L_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint40','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X0\",\"Positive_Translation_Z4\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}', f'Terminal_Joint{i}',\"Add_Dummy\",\"Rotation\",\"R_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\", 'Terminal_Joint40','Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X0\",\"Negative_Translation_Z4\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Add_Dummy\", \"Rotation\",\"L_Wheel\", \"Ell_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",'Terminal_Joint40','Rotation_Y',\n", + "\n", + " 'Terminal_Main_Body8'\n", + " \n", + " ]\n", + "\n", + " for rule in rules:\n", + " graph.apply_rule(rule_vocab.get_rule(rule))\n", + " starting_angles = [[-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0], [-30,30,0,0], [30,-30,0,0]]\n", + "\n", + " return graph, starting_angles\n" + ] + }, + { + "cell_type": "code", + "execution_count": 38, + "metadata": {}, + "outputs": [], + "source": [ + "from rostok.simulation_chrono.simulation_scenario import SuspensionCarScenario\n", + "from rostok.criterion.simulation_flags import EventBodyTooLowBuilder, EventContactInInitialPositionBuilder\n", + "from rostok.control_chrono.controller import SimpleKeyBoardController\n", + "\n", + "parameters = {}\n", + "parameters[\"forward\"] = 1\n", + "parameters[\"reverse\"]= 1\n", + "parameters[\"forward_rotate\"] = 0.5\n", + "parameters[\"reverse_rotate\"] = 0.3\n", + "control = {\"initial_value\": [0.0]*6}" + ] + }, + { + "cell_type": "code", + "execution_count": 43, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + }, + { + "ename": "KeyboardInterrupt", + "evalue": "", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[43], line 10\u001b[0m\n\u001b[0;32m 7\u001b[0m plot_graph(graph)\n\u001b[0;32m 8\u001b[0m \u001b[38;5;66;03m#scenario.add_event_builder(event_builder=EventBodyTooLowBuilder(0.15))\u001b[39;00m\n\u001b[0;32m 9\u001b[0m \u001b[38;5;66;03m#scenario.add_event_builder(event_builder=EventContactInInitialPositionBuilder())\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m result \u001b[38;5;241m=\u001b[39m \u001b[43mscenario\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mrun_simulation\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcontrol\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mstarting_positions\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mstartings\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mvis\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdelay\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m)\u001b[49m\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation_scenario.py:303\u001b[0m, in \u001b[0;36mSuspensionCarScenario.run_simulation\u001b[1;34m(self, graph, controller_data, starting_positions, vis, delay)\u001b[0m\n\u001b[0;32m 298\u001b[0m robot_data_dict \u001b[38;5;241m=\u001b[39m {\n\u001b[0;32m 299\u001b[0m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mCOG\u001b[39m\u001b[38;5;124m\"\u001b[39m: (SensorCalls\u001b[38;5;241m.\u001b[39mBODY_TRAJECTORY, SensorObjectClassification\u001b[38;5;241m.\u001b[39mBODY,\n\u001b[0;32m 300\u001b[0m SensorCalls\u001b[38;5;241m.\u001b[39mBODY_TRAJECTORY),\n\u001b[0;32m 301\u001b[0m }\n\u001b[0;32m 302\u001b[0m simulation\u001b[38;5;241m.\u001b[39madd_robot_data_type_dict(robot_data_dict)\n\u001b[1;32m--> 303\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[43msimulation\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msimulate\u001b[49m\u001b[43m(\u001b[49m\u001b[43mn_steps\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mstep_length\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m10000\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mevent_list\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mvis\u001b[49m\u001b[43m)\u001b[49m\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation.py:320\u001b[0m, in \u001b[0;36mSingleRobotSimulation.simulate\u001b[1;34m(self, number_of_steps, step_length, fps, event_container, visualize)\u001b[0m\n\u001b[0;32m 318\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(number_of_steps):\n\u001b[0;32m 319\u001b[0m current_time \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system\u001b[38;5;241m.\u001b[39mGetChTime()\n\u001b[1;32m--> 320\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msimulate_step\u001b[49m\u001b[43m(\u001b[49m\u001b[43mstep_length\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcurrent_time\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mi\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 321\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mresult\u001b[38;5;241m.\u001b[39mtime_vector\u001b[38;5;241m.\u001b[39mappend(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system\u001b[38;5;241m.\u001b[39mGetChTime())\n\u001b[0;32m 322\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m visualize:\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation.py:266\u001b[0m, in \u001b[0;36mSingleRobotSimulation.simulate_step\u001b[1;34m(self, step_length, current_time, step_n)\u001b[0m\n\u001b[0;32m 264\u001b[0m robot\u001b[38;5;241m.\u001b[39msensor\u001b[38;5;241m.\u001b[39mcontact_reporter\u001b[38;5;241m.\u001b[39mreset_contact_dict()\n\u001b[0;32m 265\u001b[0m robot\u001b[38;5;241m.\u001b[39msensor\u001b[38;5;241m.\u001b[39mupdate_current_contact_info(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system)\n\u001b[1;32m--> 266\u001b[0m \u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mdata_storage\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mupdate_storage\u001b[49m\u001b[43m(\u001b[49m\u001b[43mstep_n\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 268\u001b[0m \u001b[38;5;66;03m#controller gets current states of the robot and environment and updates control functions\u001b[39;00m\n\u001b[0;32m 269\u001b[0m robot\u001b[38;5;241m.\u001b[39mcontroller\u001b[38;5;241m.\u001b[39mupdate_functions(current_time, robot\u001b[38;5;241m.\u001b[39msensor,\n\u001b[0;32m 270\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39menv_creator\u001b[38;5;241m.\u001b[39mdata_storage\u001b[38;5;241m.\u001b[39msensor)\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\virtual_experiment\\sensors.py:257\u001b[0m, in \u001b[0;36mDataStorage.update_storage\u001b[1;34m(self, step_n)\u001b[0m\n\u001b[0;32m 255\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mupdate_storage\u001b[39m(\u001b[38;5;28mself\u001b[39m, step_n):\n\u001b[0;32m 256\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m key, sensor_callback \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mcallback_dict\u001b[38;5;241m.\u001b[39mitems():\n\u001b[1;32m--> 257\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39madd_data(key, \u001b[43msensor_callback\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msensor\u001b[49m\u001b[43m)\u001b[49m, step_n)\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\virtual_experiment\\sensors.py:114\u001b[0m, in \u001b[0;36mSensor.get_body_trajectory_point\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m 108\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m idx, body \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mbody_map_ordered\u001b[38;5;241m.\u001b[39mitems():\n\u001b[0;32m 109\u001b[0m output[idx] \u001b[38;5;241m=\u001b[39m [\n\u001b[0;32m 110\u001b[0m \u001b[38;5;28mround\u001b[39m(body\u001b[38;5;241m.\u001b[39mbody\u001b[38;5;241m.\u001b[39mGetPos()\u001b[38;5;241m.\u001b[39mx, \u001b[38;5;241m4\u001b[39m),\n\u001b[0;32m 111\u001b[0m \u001b[38;5;28mround\u001b[39m(body\u001b[38;5;241m.\u001b[39mbody\u001b[38;5;241m.\u001b[39mGetPos()\u001b[38;5;241m.\u001b[39my, \u001b[38;5;241m4\u001b[39m),\n\u001b[0;32m 112\u001b[0m \u001b[38;5;28mround\u001b[39m(body\u001b[38;5;241m.\u001b[39mbody\u001b[38;5;241m.\u001b[39mGetPos()\u001b[38;5;241m.\u001b[39mz, \u001b[38;5;241m4\u001b[39m)\n\u001b[0;32m 113\u001b[0m ]\n\u001b[1;32m--> 114\u001b[0m output[idx] \u001b[38;5;241m=\u001b[39m \u001b[43mnp\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mnan_to_num\u001b[49m\u001b[43m(\u001b[49m\u001b[43moutput\u001b[49m\u001b[43m[\u001b[49m\u001b[43midx\u001b[49m\u001b[43m]\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mnan\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;241;43m9999\u001b[39;49m\u001b[43m)\u001b[49m\u001b[38;5;241m.\u001b[39mtolist()\n\u001b[0;32m 115\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m output\n", + "File \u001b[1;32md:\\Anaconda\\envs\\rostok\\lib\\site-packages\\numpy\\lib\\type_check.py:515\u001b[0m, in \u001b[0;36mnan_to_num\u001b[1;34m(x, copy, nan, posinf, neginf)\u001b[0m\n\u001b[0;32m 513\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m d \u001b[38;5;129;01min\u001b[39;00m dest:\n\u001b[0;32m 514\u001b[0m idx_nan \u001b[38;5;241m=\u001b[39m isnan(d)\n\u001b[1;32m--> 515\u001b[0m idx_posinf \u001b[38;5;241m=\u001b[39m \u001b[43misposinf\u001b[49m\u001b[43m(\u001b[49m\u001b[43md\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 516\u001b[0m idx_neginf \u001b[38;5;241m=\u001b[39m isneginf(d)\n\u001b[0;32m 517\u001b[0m _nx\u001b[38;5;241m.\u001b[39mcopyto(d, nan, where\u001b[38;5;241m=\u001b[39midx_nan)\n", + "\u001b[1;31mKeyboardInterrupt\u001b[0m: " + ] + } + ], + "source": [ + "scenario = SuspensionCarScenario(0.0001, 10,initial_vertical_pos=0.3,is_fixed=True)\n", + "graph,startings = get_initial_mech(rule_vocab)\n", + "graph,startings = get_zero_rotation_four_legs(rule_vocab)\n", + "graph,startings = get_rotated_four_legs(rule_vocab)\n", + "graph,startings = get_rotated_four_legs_with_db(rule_vocab, 15)\n", + "graph,startings = get_rotated_six_legs_with_db(rule_vocab, 45)\n", + "plot_graph(graph)\n", + "#scenario.add_event_builder(event_builder=EventBodyTooLowBuilder(0.15))\n", + "#scenario.add_event_builder(event_builder=EventContactInInitialPositionBuilder())\n", + "result = scenario.run_simulation(graph, control, starting_positions=startings, vis = True, delay=True)" + ] + }, + { + "cell_type": "code", + "execution_count": 36, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0.28181818181818186\n" + ] + } + ], + "source": [ + "# comment this cell if dont need to calculate the height\n", + "height = 0\n", + "graph,startings = get_rotated_four_legs_with_db(rule_vocab, 3)\n", + "for i in np.linspace(0.1,1,100):\n", + " scenario = SuspensionCarScenario(0.0001, 0.01,initial_vertical_pos=i)\n", + " scenario.add_event_builder(event_builder=EventBodyTooLowBuilder(0.15))\n", + " scenario.add_event_builder(event_builder=EventContactInInitialPositionBuilder())\n", + " result = scenario.run_simulation(graph, control, starting_positions=startings, vis = False, delay=False)\n", + " if len(list(result.robot_final_ds.get_data(\"COG\").items())[0][1]) > 10:\n", + " height = i\n", + " break\n", + "\n", + "print(height)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 39, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0\n", + "10\n" + ] + } + ], + "source": [ + "# comment this cell if dont need to calculate stiffness\n", + "n=20\n", + "stiffness = np.zeros(n)\n", + "for i in range(n):\n", + " if i%10 == 0: \n", + " print(i)\n", + " \n", + " #graph,startings = get_rotated_four_legs_with_db(rule_vocab, i)\n", + " graph,startings = get_rotated_six_legs_with_db(rule_vocab, i)\n", + " scenario = SuspensionCarScenario(0.0001, 1,initial_vertical_pos=0.282)\n", + " scenario.add_event_builder(event_builder=EventBodyTooLowBuilder(0.18))\n", + " result = scenario.run_simulation(graph, control, starting_positions=startings, vis = False, delay=False)\n", + " stiffness[i]=list(result.robot_final_ds.get_data(\"COG\").items())[0][1][-1][1]" + ] + }, + { + "cell_type": "code", + "execution_count": 42, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 42, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "import matplotlib.pyplot as plt\n", + "plt.scatter(np.arange(100)[:20],stiffness[:20])\n" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "metadata": {}, + "outputs": [ + { + "ename": "RuntimeError", + "evalue": "SWIG director method error. Error detected when calling 'TorqueFunctor.evaluate'", + "output_type": "error", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[1;32mIn[18], line 52\u001b[0m\n\u001b[0;32m 49\u001b[0m parameters[\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mforward_rotate\u001b[39m\u001b[38;5;124m\"\u001b[39m] \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m1\u001b[39m\n\u001b[0;32m 50\u001b[0m parameters[\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mreverse_rotate\u001b[39m\u001b[38;5;124m\"\u001b[39m] \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.3\u001b[39m\n\u001b[1;32m---> 52\u001b[0m \u001b[43mscenario\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mrun_simulation\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mparameters\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mstarting_positions\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mstartings\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mvis\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mdelay\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mis_follow_camera\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m)\u001b[49m\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation_scenario.py:220\u001b[0m, in \u001b[0;36mWalkingScenario.run_simulation\u001b[1;34m(self, graph, controller_data, starting_positions, vis, delay, is_follow_camera)\u001b[0m\n\u001b[0;32m 217\u001b[0m robot_data_dict \u001b[38;5;241m=\u001b[39m {\n\u001b[0;32m 218\u001b[0m }\n\u001b[0;32m 219\u001b[0m simulation\u001b[38;5;241m.\u001b[39madd_robot_data_type_dict(robot_data_dict)\n\u001b[1;32m--> 220\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[43msimulation\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msimulate\u001b[49m\u001b[43m(\u001b[49m\u001b[43mn_steps\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mstep_length\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m10000\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mevent_list\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mvis\u001b[49m\u001b[43m)\u001b[49m\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation.py:320\u001b[0m, in \u001b[0;36mSingleRobotSimulation.simulate\u001b[1;34m(self, number_of_steps, step_length, fps, event_container, visualize)\u001b[0m\n\u001b[0;32m 318\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(number_of_steps):\n\u001b[0;32m 319\u001b[0m current_time \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system\u001b[38;5;241m.\u001b[39mGetChTime()\n\u001b[1;32m--> 320\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msimulate_step\u001b[49m\u001b[43m(\u001b[49m\u001b[43mstep_length\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mcurrent_time\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mi\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 321\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mresult\u001b[38;5;241m.\u001b[39mtime_vector\u001b[38;5;241m.\u001b[39mappend(\u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system\u001b[38;5;241m.\u001b[39mGetChTime())\n\u001b[0;32m 322\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m visualize:\n", + "File \u001b[1;32md:\\work\\projects\\rostok\\rostok\\simulation_chrono\\simulation.py:259\u001b[0m, in \u001b[0;36mSingleRobotSimulation.simulate_step\u001b[1;34m(self, step_length, current_time, step_n)\u001b[0m\n\u001b[0;32m 251\u001b[0m \u001b[38;5;250m\u001b[39m\u001b[38;5;124;03m\"\"\"Simulate one step and update sensors and data stores\u001b[39;00m\n\u001b[0;32m 252\u001b[0m \u001b[38;5;124;03m\u001b[39;00m\n\u001b[0;32m 253\u001b[0m \u001b[38;5;124;03m Args:\u001b[39;00m\n\u001b[0;32m 254\u001b[0m \u001b[38;5;124;03m step_length (float): the time of the step\u001b[39;00m\n\u001b[0;32m 255\u001b[0m \u001b[38;5;124;03m current_time (float): current time of the simulation\u001b[39;00m\n\u001b[0;32m 256\u001b[0m \u001b[38;5;124;03m step_n: number of the current step\"\"\"\u001b[39;00m\n\u001b[0;32m 258\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mchrono_system\u001b[38;5;241m.\u001b[39mUpdate()\n\u001b[1;32m--> 259\u001b[0m \u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mchrono_system\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mDoStepDynamics\u001b[49m\u001b[43m(\u001b[49m\u001b[43mstep_length\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 260\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mupdate_data(step_n)\n\u001b[0;32m 262\u001b[0m robot: RobotChrono \u001b[38;5;241m=\u001b[39m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mrobot\n", + "File \u001b[1;32md:\\Anaconda\\envs\\rostok\\lib\\site-packages\\pychrono\\core.py:21183\u001b[0m, in \u001b[0;36mChSystem.DoStepDynamics\u001b[1;34m(self, step_size)\u001b[0m\n\u001b[0;32m 21181\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mDoStepDynamics\u001b[39m(\u001b[38;5;28mself\u001b[39m, step_size):\n\u001b[0;32m 21182\u001b[0m \u001b[38;5;250m \u001b[39m\u001b[38;5;124mr\u001b[39m\u001b[38;5;124;03m\"\"\"DoStepDynamics(ChSystem self, double step_size) -> int\"\"\"\u001b[39;00m\n\u001b[1;32m> 21183\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[43m_core\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mChSystem_DoStepDynamics\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mstep_size\u001b[49m\u001b[43m)\u001b[49m\n", + "\u001b[1;31mRuntimeError\u001b[0m: SWIG director method error. Error detected when calling 'TorqueFunctor.evaluate'" + ] + } + ], + "source": [ + "from rostok.block_builder_api.block_blueprints import EnvironmentBodyBlueprint\n", + "from rostok.control_chrono.controller import SimpleKeyBoardController\n", + "from rostok.simulation_chrono.simulation_scenario import WalkingScenario\n", + "import pychrono as chrono\n", + "from rostok.utils.dataset_materials.material_dataclass_manipulating import DefaultChronoMaterialNSC\n", + "#from wheels import get_stiff_wheels, get_wheels, get_stiff_wheels_ell, get_stiff_wheels_4\n", + "from rostok.graph_grammar.graph_utils import plot_graph\n", + "from rostok.block_builder_chrono.block_builder_chrono_api import \\\n", + " ChronoBlockCreatorInterface\n", + "from rostok.block_builder_api.easy_body_shapes import Box\n", + "\n", + "def create_bump_track():\n", + " def_mat = DefaultChronoMaterialNSC()\n", + " floor = ChronoBlockCreatorInterface.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0]))\n", + " chrono_material = chrono.ChMaterialSurfaceNSC()\n", + " #chrono_material.SetFriction(0.67)\n", + " mesh = chrono.ChBodyEasyMesh(\"Bump.obj\", 8000, True, True, True, chrono_material, 0.002)\n", + " floor.body = mesh\n", + " floor.body.SetNameString(\"Floor\")\n", + " floor.body.SetPos(chrono.ChVectorD(1.5,-0.07,0))\n", + " floor.body.GetVisualShape(0).SetTexture(\"chess.png\", 0.03, 0.03)\n", + " floor.body.SetBodyFixed(True)\n", + " return floor\n", + "\n", + "def create_track():\n", + " def_mat = DefaultChronoMaterialNSC()\n", + " floor = ChronoBlockCreatorInterface.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0]))\n", + " chrono_material = chrono.ChMaterialSurfaceNSC()\n", + " #chrono_material.SetFriction(0.67)\n", + " mesh = chrono.ChBodyEasyMesh(\"TRACKMANIA.obj\", 8000, True, True, True, chrono_material, 0.002)\n", + " floor.body = mesh\n", + " floor.body.SetNameString(\"Floor\")\n", + " floor.body.SetPos(chrono.ChVectorD(6.6,0.02,5.2))\n", + " floor.body.GetVisualShape(0).SetTexture(\"chess.png\", 0.03, 0.03)\n", + " floor.body.SetBodyFixed(True)\n", + " return floor\n", + "\n", + "\n", + "graph,startings = get_rotated_six_legs_with_db(rule_vocab, 20)\n", + "floor = create_track()\n", + "\n", + "scenario = WalkingScenario(0.0001, 10000, SimpleKeyBoardController)\n", + "scenario.set_floor(floor)\n", + "#graph = get_stiff_wheels_4()\n", + "\n", + "parameters = {}\n", + "parameters[\"forward\"] = 1\n", + "parameters[\"reverse\"]= 1\n", + "parameters[\"forward_rotate\"] = 1\n", + "parameters[\"reverse_rotate\"] = -0.3\n", + "\n", + "scenario.run_simulation(graph, parameters, starting_positions=startings, vis = True, delay=True, is_follow_camera = True)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "rostok", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.8" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}