From 5eab196de331de918eba9e09cd4147a96e1e1a5a Mon Sep 17 00:00:00 2001 From: MikhailChaikovskii Date: Thu, 8 Aug 2024 18:08:32 +0300 Subject: [PATCH] wheel rule set --- rostok/block_builder_chrono/block_classes.py | 16 +- .../simulation_chrono/simulation_scenario.py | 2 +- tests_jupyter/wheels.py | 225 ++++++++++ tests_jupyter/wheels_ruleset.ipynb | 389 ++++++++++++++++++ tests_jupyter/wheels_sim.py | 12 + 5 files changed, 637 insertions(+), 7 deletions(-) create mode 100644 tests_jupyter/wheels.py create mode 100644 tests_jupyter/wheels_ruleset.ipynb create mode 100644 tests_jupyter/wheels_sim.py diff --git a/rostok/block_builder_chrono/block_classes.py b/rostok/block_builder_chrono/block_classes.py index 3ec829a2..37d0867e 100644 --- a/rostok/block_builder_chrono/block_classes.py +++ b/rostok/block_builder_chrono/block_classes.py @@ -445,20 +445,24 @@ def __init__(self, material) # pos_in_marker = chrono.ChVectorD(0, -shape.height_y * 0.5 - eps, 0) # pos_out_marker = chrono.ChVectorD(0, shape.height_y * 0.5 + eps, 0) - pos_in_marker = chrono.ChVectorD(0, 0, -shape.height_y * 0.5 - eps) - pos_out_marker = chrono.ChVectorD(0, 0, shape.height_y * 0.5 + eps) + pos_in_marker = chrono.ChVectorD(0, 0, 0) + pos_out_marker = chrono.ChVectorD(0, 0, 0) elif isinstance(shape, easy_body_shapes.Sphere): body = chrono.ChBodyEasySphere( shape.radius, density, True, True, material) - pos_in_marker = chrono.ChVectorD(0, -shape.radius * 0.5 - eps, 0) - pos_out_marker = chrono.ChVectorD(0, shape.radius * 0.5 + eps, 0) + # pos_in_marker = chrono.ChVectorD(0, -shape.radius * 0.5 - eps, 0) + # pos_out_marker = chrono.ChVectorD(0, shape.radius * 0.5 + eps, 0) + pos_in_marker = chrono.ChVectorD(0, 0, 0) + pos_out_marker = chrono.ChVectorD(0, 0, 0) elif isinstance(shape, easy_body_shapes.Ellipsoid): body = chrono.ChBodyEasyEllipsoid( chrono.ChVectorD(shape.radius_x, shape.radius_y, shape.radius_z), density, True, True, material) - pos_in_marker = chrono.ChVectorD(0, -shape.radius_y * 0.5 - eps, 0) - pos_out_marker = chrono.ChVectorD(0, shape.radius_y * 0.5 + eps, 0) + #pos_in_marker = chrono.ChVectorD(0, -shape.radius_y * 0.5 - eps, 0) + #pos_out_marker = chrono.ChVectorD(0, shape.radius_y * 0.5 + eps, 0) + pos_in_marker = chrono.ChVectorD(0, 0, 0) + pos_out_marker = chrono.ChVectorD(0, 0, 0) else: raise Exception("Unknown shape for ChronoBodyEnv object") diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index 4d3533bd..5443b24c 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -181,7 +181,7 @@ def run_simulation(self, def_mat = DefaultChronoMaterialSMC() else: def_mat = DefaultChronoMaterialNSC() - floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(10, 0.1, 10), material=def_mat, color=[215, 255, 0])) + floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(1, 0.1, 1), material=def_mat, color=[215, 255, 0])) floor.body.SetNameString("Floor") floor.body.SetPos(chrono.ChVectorD(0,-0.05,0)) #floor.body.GetVisualShape(0).SetTexture("/home/yefim-work/Packages/miniconda3/envs/rostok/share/chrono/data/textures/bluewhite.png", 10, 10) diff --git a/tests_jupyter/wheels.py b/tests_jupyter/wheels.py new file mode 100644 index 00000000..601e4662 --- /dev/null +++ b/tests_jupyter/wheels.py @@ -0,0 +1,225 @@ +# %% +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 create_rules(): + 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") + + + # %% + # 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)) + + wheel_body_cyl = PrimitiveBodyBlueprint( + Cylinder(0.03, 0.02), material=def_mat, color=[0, 120, 255], density=500) + + wheel_body_ell = PrimitiveBodyBlueprint( + Ellipsoid(0.06,0.06, 0.04), material=def_mat, color=[0, 120, 255], density=500) + + # blueprint for the base + base = PrimitiveBodyBlueprint(Box(0.02, 0.03, 0.02), + material=def_mat, + color=[120, 120, 0], + density=500) + + # 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=500), length_link)) + + # %% + 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="WE", is_terminal=True,block_blueprint=wheel_body_ell) + + # %% + 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,1,10) + + no_control_joint = list( + map(lambda x: RevolveJointBlueprint(JointInputType.UNCONTROL, + stiffness=x, + damping=0.02, + equilibrium_position=0), stiffness)) + motor_bp = RevolveJointBlueprint(JointInputType.TORQUE, stiffness=0, damping=0.001) + neutral_bp = RevolveJointBlueprint(JointInputType.UNCONTROL, stiffness=0, damping=0.001) + + # %% + 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("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)]) + + # %% + 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("Ell_Wheel",["W"],["WE"],0,0,[]) + + return rule_vocab + +# %% +def get_wheels(): + i=5 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension","Rotation","R_Wheel", "Cyl_Wheel", + "Terminal_Link3","Terminal_Link3",f'Terminal_Joint{i}',f'Terminal_Joint{i}', 'Rotation_Y', + + "Add_Leg_Base","Positive_Translation_X4","Negative_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension","Rotation","L_Wheel", "Cyl_Wheel", + "Terminal_Link3","Terminal_Link3",f'Terminal_Joint{i}',f'Terminal_Joint{i}', 'Rotation_Y', + + "Add_Leg_Base","Negative_Translation_X4","Negative_Translation_Z0", "Extension","Extension", + "Terminal_Link3","Terminal_Link3",f'Terminal_Joint{i}',f'Terminal_Joint{i}', "Wheel", "Ell_Wheel", + 'Terminal_Main_Body3' + + ] + + rule_vocabul = create_rules() + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + return graph diff --git a/tests_jupyter/wheels_ruleset.ipynb b/tests_jupyter/wheels_ruleset.ipynb new file mode 100644 index 00000000..e5be962d --- /dev/null +++ b/tests_jupyter/wheels_ruleset.ipynb @@ -0,0 +1,389 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 16, + "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": 17, + "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": 18, + "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": 19, + "metadata": {}, + "outputs": [], + "source": [ + "# blueprint for the palm\n", + "main_body = []\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))\n", + "\n", + "wheel_body_cyl = PrimitiveBodyBlueprint(\n", + " Cylinder(0.03, 0.02), material=def_mat, color=[0, 120, 255], density=500)\n", + "\n", + "wheel_body_ell = PrimitiveBodyBlueprint(\n", + " Ellipsoid(0.06,0.06, 0.04), material=def_mat, color=[0, 120, 255], density=500)\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=500)\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=500), length_link))" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "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=\"WE\", is_terminal=True,block_blueprint=wheel_body_ell)" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "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": 22, + "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": 23, + "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": 24, + "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": 25, + "metadata": {}, + "outputs": [], + "source": [ + "stiffness = np.linspace(0.1,1,10)\n", + "\n", + "no_control_joint = list(\n", + " map(lambda x: RevolveJointBlueprint(JointInputType.UNCONTROL,\n", + " stiffness=x,\n", + " damping=0.02,\n", + " equilibrium_position=0), stiffness))\n", + "motor_bp = RevolveJointBlueprint(JointInputType.TORQUE, stiffness=0, damping=0.001)\n", + "neutral_bp = RevolveJointBlueprint(JointInputType.UNCONTROL, stiffness=0, damping=0.001)" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "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": 27, + "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(\"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)])" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "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(\"Ell_Wheel\",[\"W\"],[\"WE\"],0,0,[])\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAgMAAAGFCAYAAABg2vAPAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjYuMCwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy89olMNAAAACXBIWXMAAA9hAAAPYQGoP6dpAABPAElEQVR4nO3de3gU9b0/8PfM7CZL7tlcuISETbiIEAOKBvCGgqJWxCOX1raeatWfWgstbRWqz9EixVOF9tS2Hk/VSqVqlRLswVtbbb1fjrGxMVwUCbmyCbltskk2JNnMzO+PsEsuu8kObLIzO+/X8/g8yexk/C6b98wnn5n5jqCqqgoiIiIyLTHSAyAiIqLIYjFARERkciwGiIiITI7FABERkcmxGCAiIjI5FgNEREQmx2KAiIjI5CyhrKQoCurq6pCYmAhBEMZ6TERERBQGqqqio6MDU6ZMgSgG//s/pGKgrq4O2dnZYRscERERjZ/a2lpMnTo16OshFQOJiYn+jSUlJYVnZERERDSm2tvbkZ2d7T+OBxNSMeA7NZCUlMRigIiIyGBGO8XPCwiJiIhMjsUAERGRybEYICIiMjkWA0RERCbHYoCIiMjkWAwQERGZHIsBIiIik2MxQEREZHIsBoiIiEyOxQAREZHJsRggIiIyORYDREREJsdigIiIyORYDBAREZkciwEiIiKTYzFARERkciwGiIiITI7FABERkcmxGCAiKIoa6SEQUQRZIj0AIhp/+51u7C6pRXGlC+VNnfDKKqySgBkZCSjMtWPtgmzkZyVHephENE5YDBCZSFWzBxv3lKG4ygVJFCAP6Ah4ZRWfH+vAl42d2PlRNQoddmxbXQBHenwER0xE44GnCYhMYm+pE8sfeRclNa0AMKgQGMi3vKSmFcsfeRd7S53jNkYiigx2BohMYG+pExt2lULLlQGyokKGig27SgEA187PGpOxEVHksTNAFOUqmz24u6hMUyEwkArg7qIyVDV7wjksItIRFgNEUW7TnjLI6undLSCrKjbuKQvTiIhIb3iagChKORwOtHd6kHjz7yBI/VHvri5Dw/P3IvHclYibuQiNuzfDYj/R/ldkJJ67EonzrwQAtL7zBxwv/xgQ+v9maF+0BvtXzOFdBkRRiMUAURSzpWSi50gxbLPOBwB0lr2OmEkz/a9b7FmYcvNvAAB97c1wPn4r4s+8GGJsHJIXrkLqkm/1v9bRjLonv4Odb12D7TdcNP5vhIjGFE8TEEWxlPnL0f7Z6wAApduDHuchTMg7J+C6Sm8XRKvN30UQbQn+19TebgDAp9WtYzxiIooEdgaIolhnSh763C+ir6MFx8s/RtzsC/xtfwDoczlRt2M9IPfB21YP++V3QLDE+F9v/+dL6Pj0VcgdzUi76nuoOc5dBlE0YrKJolifAsTPvRSeff9A1+GPkH7NXfAceNv/+tDTBMeevRsxk2YgdtIMAEDSuSuRdO5K9DZUoPmVX8CWezYURYUoCpF4O0Q0RlgMEBlcX18fampqcOTIEVRUVODIkSM4cuQI6urqkK7KiM9fivqnvw9rahas9uBzBViS0hE75Qx0V3/mLwZ8YibmQUpIQ3dVKdauLcL06dMxffp05OXlYfr06cjJyYHFwt0JkVExvUQG0NHRMehAP/Dr6upqyLIMAJAkCTk5OZg+fTpiY2ORbgPaE9OQuuRGWNOmjvj/ULo96D1Wjvi5lwIAeptrEJOeAwDwttajt6ECKfZ0uGtKUFRUhOrqaiiK4v//Tps2zV8kDCwU8vLykJiYOIb/OkR0ulgMEOmAqqqor68PesBvamryrxsfH+8/4K5atcp/0PX9hW61WgH031q4+Iyp+HuTgISCywP+f/3XDABQZS/i516KuJkLAQBtb/0efe4GQJQgiBLSr7gD3/i3q/DAyrsBAF6vF9XV1cPG/OGHH+KZZ56Bx3NykqKMjIxhRYLv68mTJ0MQeNqBKJIEVR19NpL29nYkJyfD7XYjKSlpPMZFFHV6enpQVVU17EBfUVGBiooKHD9+3L/u5MmThx00fV9nZGSEfPDc73RjxaPvh+09vLLuwpDmGVBVFU1NTQELm4qKCtTX1/vXnTBhwqAuwsD37HA4EBsbG7bxE5lNqMdvFgNEYdTa2hrwAHjkyBEcPXoUvrhZrVbk5uYGPADm5eUhLi4ubGP66uMfoaSmNeiDiUIhiQIW5KTiT7cvDsuYurq6/EXQ0H+vyspKeL1eAIAgCMjOzg5aGKWmpoZlPETRisUA0RiQZRlHjx4N2s5va2vzr5uamhr0IJaVlQVJksZlzFXNHix/5F30ysopbyNGEvH6hovH5XHGsizD6XQGLaoG/hunpKQEvE5hvP+NifSKxQDRKRrpr9aqqir09vYCCPxX68ADkp7+aj2Vpxb6CAAe+dp83Ty1MFj3paKiArW1tf7uS0xMDBwOx7h0X4j0isUAURBmPZ+9t9SJu4v6H1oUyikDSRQgCQK2rynQTSEwGt91GYEKuUDXZQT6XLVel0GkZywGyNS8Xq//3vtAB/zOzk7/upmZmUHb+ZMmTYqqg0JVswcb95ShuMoFSRQCFgW+5Qtz7Xh4VcG4nBoYD6qq4tixY0F/JxobG/3rJiQkBP2dGHjHBpHesRigqNfe3h5wp37kyBHU1NT47723WCz+e+ADtYvNeA/8/x1y4r/+9yNUd1nQ0C0CogQRKjJjZVwyNxs3nD/ddE8nHDiXw9DfqerqavT19QEYPJdDoN8p7iNJT1gMkOEpioL6+vqgB/zm5mb/uomJiUEvJMvOzubseEM899xzuOGGGwYsEYATVxQ8++yz+OY3vxmRcemVb5bHYBeOdnR0+NdNT08Pevph8uTJEEU+H47GD4sBMoSenh5UVlYG3MFWVlaiu7vbv25WVlbQ1m1aWlpUtfPHWmdnJ7KzswddmQ/0X51fW1uLhISEwD9Iw6iqipaWlqCnH5xOp39dm83m70gN/T3Ozc011DUoZAyhHr/55xKNKVVV4XK5gv5F5XQ6/Vd/x8bG+u+9v+yyywbtLHNzczFhwoQIv5vokZCQgHvuuQc//vGP/f/+giDgnnvuYSGgkSAISE9PR3p6OhYuXDjs9ePHj6OysnJYBv72t7+hsrISPT09/u1kZWUFPf1gt9tZ8NKYYWeATpssy6itrQ3azne73f517XZ70Hb+lClT2EIdR0O7A+wKjD9FUeB0OoMWyy6Xy79ucnLysMz4vs/OzuacChQQTxNQWHk8noAXV/kelOObMU4UReTk5ATcYeXl5SElJSWyb4QG2bZtGzZt2gQAePjhh7Fx48YIj4gGamtrC3pRY01Njf9BUVarFQ6HI+Dph7y8PMTHR8cdIaQdiwHSRFVVNDQ0BP0LpaGhwb9uXFxc0FbmtGnTEBMTE8F3Qlp0dnbCbrcDAFwuF7sCBtLb2xvwQVG+r7u6uvzrTpw4MWBmp0+fjszMTJ5+iGKmvmZAUVSIIn+5hwq08xj4oJyBT5mbNGmSf6exfPnyQTsQ7jyiR0JCAn75y18CEFgIGExMTAxmzpyJmTNnDnttpOL+9ddfH1Tcx8fHB51Yi8V9cNF2nImKzsB+pxu7S2pRXOlCeVMnvLIKqyRgRkYCCnPtWLsg2zT3TLvd7qBXNbOtSD7MjLkFO+1XUVGBqqqqEU/7Dfw6Odk8vyNGzYwpThNomU2t0GHHttXGn01NURTU1dUN+8t+pAuOAlX8vODInMyYGdKGFwQPZvTMRH0xEM3zrAe7Fcl37z1vRaJTEc2ZofFxqrcKD903GeVW4WjITFQXA0Z/Alu4JilxOByw2WwRex9kHEbPDBlDd3c3qqqqAt51VFFR4f9DBhg+idjAfZseJhGLlsxEbTFQ2ezBFWP4bPbm5ma88cYbuP7660/rl7Gvr8/fagv09LT29nb/ur7pSwNV0Jy+lE7XWGeGKBRaphdPSkoKep3C6U4vrqoqXnjhBVx++eVIT08PuE40ZSZqi4GvPv4RSmpaQ2rZBCOJAhbkpOJPty8etPyll17Ct7/9bbhcLnz22WcoKCgYcTudnZ1Bf7H5YBPSi7HMDFG4tLe3Bz39EOqDx6ZPnz7qXTGfffYZ5s+fj7S0NPz+97/HNddcM2ydaMpMVBQDbW1tmDdvHl544QUsXrwY+5xuLPn2JnQd+hDK8Q5AENDXWg8xLhlibBwAIOPaTWj566Poa2+CGBsHta8XcbMvROrF/z542+89B/cHz+Nf//oXcnNz8f3vfx87d+6EIAhQVRV79uzBdddd53/kaaCW18BHnsbHxw9qdw385eQjT2m8jFdm5s+fH4F3R2Y10iPJjxw5Mui26MzMzKCFwqRJk/Diiy9izZo1/n19fHw8/vznP+Pyyy8HANy7dTv+68lnRs2Lt6kaWXf8DqKt/y//pj//JyZML0RCwWX+sSjeHlhfuheJcTaUlpaO3z/YAFExz0BKSgoef/xx3HTTTSgtLcXjL7+P9g92YeK3fg5ryiQAwLHnfoyk865F3KzB1Zd92a2Im7UYcncn6nd8D7GTZyFuZv+84T11h9B77DCSM6fg/fffx/Lly9HS0gIA/otf7rzzTtxwww04fvy4f5uTJ0/G9OnTMWvWLFx11VWDfskyMjIifo6LaDwyQzTerFar/8A+lKqqaGpqCtilffPNN1FfX+9fd8KECf4Dom9f39XVhauuugpFRUWYO3cufv3zhzDxm9shJE0EEDwvQmwc3P+3G6mX3BR03O53nsZ0Rz6UxvLT/ScYc7ouBgDgyiuvxJIlS3DXXXfhj6+9g+SLvunfqYVCsiUgdsoseF39F+Up3m643vgtMq67F03PbcT69euH/YwgCMjIyMCmTZv8v4C5ubmIi4sL2/siGitjmRnXC/eM1bCJTokgCMjMzERmZiYWLx7eku/q6hr0ZNSnnnoKjY2N/mJAVVXIsozrrrsO06dPx7Tl34bnRCEwkuRFq9H23nNIXHANLIlpw14/XlWKvvYWWBd9DT0sBsLjF7/4BfLy8tATPwWZZ1+l6Wf72pvRc/QgEs/+CgCg7a3fI/Hsr8CSlAEFAi699FJ8/vnnOHbsGCwWC/r6+qCqKiZOnIgf/OAHY/F2iMbcWGXGexoXVBFFQlxcHObOnYu5c+cCAF555RUcOHAAQP/1XLIsIzMzE21tbZg8eTKOzVoKyCHcRhifisT5V8L9/nNIu+p7g15TujvR9tbvkfnVB3C0tRYZ4X9bYWeIy9Tfe+89xMbGorf1GJSertF/AIDrH79D3Y71aHrxQSSffz1s0wpwvPJf6HM3IqGg/9yQqgI///kvUFdXh5KSEvzwhz9EVlb/rSBHjx4ds/dDNNbGMjPKaVxURRRpvlu3s7Ky8KMf/QiffvopduzYgYyMDNTW1qKnyzPKFk5KWrgax8s/gbeldtBy1xu/RdLitZDiUyCHUFjoge47Ay6XC3fccQdefPFFLL9zC1rf/N2wKiwQ3/nPgbqrP0NvwxEcfexmAIDc0YwVK67G448/jmuuuQbnnHMOHnroIXz88cc8/0+GNZ6ZITKanTt3QlVVLFy4EIIgwOVy4dprr8WLL76InTt34tm3nkLqlcNPHwcixsYhadEatL69E8KAW8C7aw+iu/YgWt/cAci9cPV6cMYZZ+DQoUNj9bZOm+6Lge9+97u44YYbUFhYiPO+uh5v/+wmHK/8FBNyz9G8rdRLbhp0sUfjE7fitddeG3RltCAIWLRoURhGThQZ450ZIiNZuHDhoO8H5mXu3Ln4/bRZmvKSePZX0P7PlwAAE6YXAgCm3rnD//rEziPoeW9HxO4mCJWuTxMUFRVh//792Lx5MwDg/NlZyLh6A1r+8hso3aG3cgKRRAExFl2/fSLNmBmi0A3NS3x8PNb+cCtcGvIiWKxIueibkN0Nw16TRAGzJyWGc8hjRtfzDAy13+nGikffD9v2Xll3oS6fMkUULswMkTbRlplQj9+GKvPzs5JR6LBDOs1nSEuigEKHnTs1inrMDJE2Zs2MoYoBANi2ugDSaV7cJwkCtq0eeaphomjBzBBpY8bMGK4YcKTHY/uaApzqxyQA2L5GX8+bJhpLzAyRNmbMjO7vJgjE91hIoz9nmmi8MDNE2pgtM4a6gHCoqmYPNu4pQ3GVC5IoBPywfMsX5trx8CpjVWpE4cbMEGlj9MxExVMLQ/W3Tw7iwT/+Ay7Jjk4xAZAsEFQFCXI7lszNxh2XzzPMRRxE4+HVD8vw0J/eQavFjg4xAcKAzCw9y4H/tyyfmSEaYO+7n+LnL34QIDMduGyeA7dcOleXmYmKpxaGSnLX4b1HvgdRFE88fEKAIACKouDeP/4R+VkXR3qIRLoiN1fh/V8FzsyDe/fqcqdGFEldzkNBM/PI3/9u+MxERWdAVVXMmzcPBw4cgKL0P0hFEARkZWWhoqICVqs1wiMk0hdZljF79mxUVFT4MyOKIqZPn47PP/8ckiRFeIRE+tLb24vp06fD6XT6n3goiiLy8/NRWlqq2ynso3KegWAEQcDWrVv9OzWgv0DYvHkzCwGiACRJwpYtWwZlRlEUbNmyhYUAUQAxMTH4yU9+goF/PyuKgq1bt+q2ENAiKjoDwMnuwP79+6GqKqZOncquANEIfN2B8vL+Z63PnDmTXQGiEfi6A76n2hYUFOi6KwCYrDMAnOwO+GobdgWIRubrDviwK0A0Ml93wCdaugJAFHUGgP7uQFpaGo4fP4729nYWA0SjkGUZKSkpAIC2tjYWA0Sj8Hq9SEpKwoQJE9DS0qL7YsBUdxP4CIKA999/Hx2dnSwEiEIgSRLef/99KKrKQoAoBFarFe+++y4mxMXpvhDQIiqKgf1ON3aX1KK40oXypk54ZRXWl17DjIwEFObasXZBtuFv+yAKp4CZKapjZoiCCJgZqTpqMmPo0wRaZoYqdNixbbW+ZoYiGm/MDJE2Rs9M1M9AuLfUaZo5o4nCgZkh0iYaMhPV1wzsLXViw65SjP7RnCQrKmSo2LCrFAB080ERjQdmhkgbs2XGcLcWVjZ7cHdRmaYPaCAV/U+hqmr2hHNYRLrFzBBpY8bMGK4Y2LSnv2VzOmRVxcY9ZWEaEZG+MTNE2pgxM7q+ZuCll17C/fff7/++2yujvKYOcmcrrJkOAIDsaQUUBVJiGgAgaeEq9Lnq0PHpq5AS06DKXsRk5MJ+5Xch2RLQ/Ooj6D1WDggCZk1Owa9+sR3Lli0bt/dENJZOJzPuD57HxG8+BFt2PgCgveRl9NaXI33FD+BtO4bmP/8MuWkTYBFUnHnmmXjiiSeQmpo67u+RKJzGKjMA0FVejPh/PQ+LoOKss87C008/Pe7X3UXlBYSb/vghfrluLRIXrUHivOUAgLb3noPS44H9stv86w1cpioymv73IViSJ8K+7FYo3Z0QbQmQRAHLMjwo2nI7mpubIYqGa5IQjUpLZjr3vwlLQiom/fvPAQzesal9XoiCim9dOBMPrMzH97//fQDAr371q/F/U0RjKFyZUXqPo+63/w+3b9+J/75zJdatW4cJEyZg+/bt4/p+om46YkVR8PsHf4TYaQX+DygUgihhgmM++lxOAIBoSwDQf6HHZ0fqx2SsRHqgNTNxMwqhyjK6Dn047DXBYoUqxeCTqlbIsgyPxxNVE64QAeHNzPGKElgn5qGyr797duedd+L5558P+5jDxTB3E/zkJz9Bu7sNE7+xSdPPKd4edH35f4jNmu1f1vr20+j64n3Udnfi76+9xK4ARSXNmREEpFxyI1xv/BYTZi4c9rIqe/GPB29E+k9aUVBQgJdeeinMIyaKrHBmRnY3wpKcicONHQAAh8OB+vp69PX1wWLR36HXEEfBvXv34qmnnkL6v90DQQptmmHPgbdRt+N7OPbM3bCmZyNp0Rr/a6mX3ISsO36H9H/7MTZu3Ije3t6xGjpRRJxKZgBggmM+LIkZ6Cx7Y9hrgmTF5Jt/g/r6Y5g9ezYef/zxcA6ZKKLGIjMA4JVVKCHMURBpui8GDh06hFtuuQV/+tOfMCElPeSfi597Cabc/GtMufnXsF92G0Rr7LB1kqafjY6ODuzbty+cQyaKqFPNjE/KJTfC/cELUL09w16zSgJstlh8+9vfxjPPPBOO4RJF3FhkRkrORJ+7EVZJgCgKqKqqwuTJk3XZFQB0Xgx0dHTguuuuwwMPPIALL7wQMzISTmt7qtwHb2ud//uM47VobGxEXl7e6Q6VSBfCkZnYSTMQO3UOOv71F/+yPncjFG83ZmYmQlEU7N69GwUFBeEcOlFEjFVmJuSeg96GI5isugAAjz32GK6//vqwjTvc9FminPDf//3fOHToEJ588kk8+eSTONbejVZPL1QAmWs3w3LiNo9QqUofWl75JZQeDwRRgmxPRlFREW+PoqgRrsykLPkW6p64w/99b2Ml3O8+g544KwqeiME555yDX//612P0LojGz1hlRoyNQ8ZXvoeDO+/DjF2bkZ+fj507d47Ruzh9hrq1cL/TjRWPvh+27b2y7kJDP2WKaDTMDJE20ZaZqLu1EADys5JR6LBDEk/vliZJFFDosHOnRlGPmSHSxqyZMVQxAADbVhdAOs37myVBwLbVPN9J5sDMEGljxswYrhhwpMdj+5oCnOrHJADYvkZfz5smGkvMDJE2ZsyMri8gDMb3WEijP2eaaLwwM0TamC0zhrqAcKiqZg827ilDcZULkigE/LB8yxfm2vHwKmNVakThxswQaWP0zETlg4qC2fX6h7j3dy9DtjugJk2CIFkAuQ9oP4alZ+Xgh/92vmEu4iAaD8+88jZ+svOvUNJyB2VG6DiGy+bl4fvXFDIzRAP87sW/4T+ffxNKWt6QzDTginNmYN1XFugyM6Eevw15mmCoMzLjULH7oQFLBAD9Nc433nhDlx8QUSTNSItFZdHDA5aczMzNH37IzBAN4UiSUFm0bcCSk5m541//MnxmDHcBYSDz58/HihUrIEnSiSUqJElCYWEhli1bFtGxEenR4sWLcemllw7LzNKlS7F48eKIjo1Ij5YtW4bzzjtvUGYsFguuueYazJ8/P5JDC4uoKAYA4IEHHoAsy/7vZVnG1q1b+ZhVoiC2bNkyLDNbtmyJ4IiI9EsQBGzdunVQZvr6+vDAAw9EcFThEzXFwDnnnINrrrnGf/AvLCzEZZddFuFREenXhRdeiEsvvRSCIEAQBCxduhQXXHBBpIdFpFuXX345zjvvPAD9xcHKlStx9tlnR3hU4RE1xQDQ3x3wXQ/JrgDR6LZs2QJVVaGqKrsCRKPwdQcAQFVVbN68ObIDCqOouJtgoDPOOANutxv19fUsBohCMG3aNABAdXV1hEdCpH+qqmLy5MlISUnBF198EenhjMpUdxMMdPDgQfT1ySwEiEJ05MgRKCFMqEJE/d2B2tpa4JTnJ9SnqCgG9jvd2F1Si+JKF8qbOuGVVVglATMyElCYa8faBdmGv+2DKJyYGSJtoj0zhj5NoGVmqEKHHdtW62tmKKLxxswQaWP0zET9DIR7S52mmTOaKByYGSJtoiEzUX3NwN5SJzbsKoWWs5yyokKGig27SgFANx8U0XhgZoi0MVtmDHdrYWWzB3cXlWn6gAZS0f8UqqpmTziHRaRbzAyRNmbMjOE6A5v29Ldshjr62M0QLFYIlhioshcxE6cj7cr1EGNsw9aVVRUb95ThT7dz2lWKfswMkTZmzIyhOgP7nG4UV7mCnrvJuHYTptz8G0y59TGoPR549v094HqyoqK4yoX9TvdYDpco4pgZIm3MmhlDFQNFJbWwiCHc2yn3QfH2QLQlBF1FEgXsLqkN4+iI9IeZIdLGrJkx1GmC4koX+ka4orNp78MQLDHoczcidtJ0xJ15UdB1ZUXFJ1WtYzFMIt1gZoi0MWtmDFUMlDd1jvh6xrWbEDMxD6oio+Wvj6L1rd/DvuzWoOsfbuwI9xCJdIWZIdLGrJkxzGkCRVHhlUO7tlMQJcSfcT66Kz8dcT2vrHIaVopazAyRNmbOjGGKAVEUYJVCnwu6u7oMFvvI93haJQFiKOeGiAyImSHSxsyZMdRpghkZCfj8WPCWi+9cDhQZluRM2K/47ojbm5mZGO4hEukKM0OkjVkzY6hioDDXji8bOwPe8jH1zh2atiWJAs5zpIZraES6xMwQaWPWzBjmNAEArF2QHdL80KGQFRVrF2SHZVtEesXMEGlj1swYqhjIz0pGocMO6TTPv0iigEKH3dCPmyQKBTNDpI1ZM2OoYgAAtq0ugCSc5ockCNi2uiBMIyLSN2aGSBszZsZwxYAjPR7b1xTgVD8mAcD2Nfp63jTRWGJmiLQxY2YMdQGhj++xkEZ/zjTReGFmiLQxW2YEVQ3waKYh2tvbkZycDLfbjaSkpPEYV0iqmj3YuKcMxVUuSCIgK8PXkUQBsqJiYa4dD68yVqVGFG7MDJE2Rs9MqMdvQxcDPr/906u493cvwzZ1DqzpORAkK1TZC29zDS6aPQX3ffNyw1zEQTQefvWHF/HAs68HzMylZ03DPV+9hJkhGuChx5/Dtj3vBczM5fOn4+7VF+oyM6YqBpxOJxwOB/r6+k4sEQCoEAQB+/btw9y5cyM5PCLdqaiowMyZM6Eovj9z+jMjiiIOHz6MvLy8SA6PSHcOHDiAs846CycPmf2ZsVqtqKysRFaWPk8LhHr8NtwFhIFkZWXh9ttvhyRJJ5aosFgsWLNmDQsBogDy8vJw4403wmLxXTakQpIk3HjjjSwEiAKYO3cu1qxZMywzt99+u24LAS2iojMADO8OsCtANLKh3QF2BYhGNrQ7oPeuAGCyzgBwsjsgCAIEQWBXgGgUvu6ALzPsChCNzNcdAPr/4IyWrgAQRZ0BoL87kJ2dDVVVsX//fhYDRKOoqKjA9OnTAQBHjhxhMUA0igMHDiA/Px+CIKC2tlb3xUCox29DzjMQTFZWFlatWoXm5mYWAkQhyMvLw1VXXeX/mohGNnfuXCxZsgTp6em6LwS0iKrOgI+iqIZ4fjSRXjAzRNoYJTOm6gzsd7qxu6QWxZUulDd1wiursEoCZmQkoDDXjrULsnV5/ydRpDAzRNpEe2YM3RkYPDOUEHC6SN/yQocd21bra2YoovHGzBBpY/TMRP2kQ3tLnaaZM5ooHJgZIm2iITNRfZpgb6kTG3aVYvSP5iRZUSFDxYZdpQCgmw+KaDwwM0TamC0zhptnoLLZg7uLyjR9QAOp6H8KVVWzJ5zDItItZoZIGzNmxnCdgU17+ls2TS9thyU5E6lLbvS/1rh7M2JzCtD29tOwZkwDTpwBSVlyI+JmnOdfT1ZVbNxThj/dvnjcx0803pgZIm18mRno6GM3I3P1f0CwxsL1+v9A9rQBqoLkC65H/JkXD9uG0TJjqM7APqcbxVUuyIoK+/LvwHPgbfTUfwkA6PjsdSg9XYg743wIMRMw5ebfYMotjyJlybfQ/NI2qIrs346sqCiucmG/0x2pt0I0LpgZIm0GZiaQllcfQdyZF2HKLY9i4jd+hta3fo++juZh6xktM4YqBopKamE5cV+nZEtA2pXr0PLqI/C6nHC/9yzSVvwQAgbf92mbNg9q73Eo3Z2DlkuigN0lteM2dqJIYGaItBmYmUB6GysxIe9cAIAUl4yYzFx0ff5ewHWNlBlDFQPFlS70DajWJuQtQOzUOajf+UMkX/B1WFMmDfuZrkMfwDatAFLc4Ps/ZUXFJ1WtYz5mokhiZoi0GZqZoWImzYDnwFsAAG/bMfQ4v0CfuzHgukbKjKGuGShv6hy2LGnhangOvoPEs6/yL1N7j6Nux3oo3Z1Qutox8esPBtze4caOMRsrkR4wM0TaBMrMQOlX/wCtbz6Fuh3rYUnKhG3aPEAM/ne1UTJjmGJAUVR45eHVmiCIgDD4g/Cd/1RVFe4PXkDT3m3Iuu23ECwxg9bzyqphppQk0oqZIdImWGYGsqRMRMaqe/3fN+y6H7bcs4Oub5TMGOY0gSgKsEra/jEFQUDyBddDiktCx6evDXvdKgm6/4CIThUzQ6RNKJmRPa1QVQUAcLyiBN6WWsTPWRJ0faNkxjDFAADMyEjQ/DOCICB16S1wf1wExds96LWZmYnhGhqRLjEzRNoEzYwqQ7BY0VVejLrHb4fzidvh/r8iZK79CURrbNDtGSUzhjlNAACFuXZ82dg56JYPS8pE5PxgV9DvAcCWcxay1z87aJkkCjjPkTq2AyaKMGaGSJtAmenrdEHp6YKUlIHEeVcgcd4VIW3LSJkxVGdg7YLskOaHDoWsqFi7IDss2yLSK2aGSJuhmWkv/jMa/ngPUpfeAtFq07QtI2XGUMVAflYyCh12SKd5/kUSBRQ67IZ+3CRRKJgZIm2GZiap8Dpk3fY4EudfqWk7RsuMoYoBANi2ugCScJo7NkHAttUFYRoRkb4xM0TamDEzhisGHOnx2L6mAKf6MQkAtq/R1/OmicYSM0OkjRkzY6gLCH18j4U0+nOmicYLM0OkjdkyI6iqOuo7bG9vR3JyMtxuN5KSksZjXCGpavZg454yFFe5IAJQAqwjiQJkRcXCXDseXmWsSo0o3AZnRoUS4G8fZoboJKNnJtTjt+FOEwzkSI/Hn25fjA1ndqOt5GX0NhyBKnsBAKrsRW/DEcyLa8cr6y7ErtsW6+oDIooEX2bucLShreSVgJk5N7mLmSE6wZeZGyfVB83MojSv4TNj6M6AT2dnJ7Kzs9HW1nZiiQBAhc1mQ3V1NTIzMyM4OiL9aWtrQ3Z2Njo7ffOw92cmISEBtbW1SElJieDoiPSnsbEROTk56OnpObGkPzMpKSk4evQo4uP1WQSYojPgk5CQgHvuuQeC/+pPFZIkYd26dSwEiAJISUnBj370I4j+B6yoEEURd911FwsBogAyMzOxbt06SJJ0YokKQRBw77336rYQ0CIqOgPA8O4AuwJEIxvaHWBXgGhkQ7sDqampqK2t1XUxYKrOANC/I7v33pNPkmJXgGhkvu6AD7sCRCPzdQd8oqUrAERRZwAAPB4PUlNTIcsy6uvrWQwQjaKtrQ3p6ekAgObmZhYDRKNobGzEpEmTYLVa4XK5dF8MhHr8NuQ8A8HEx8fjwQcfhMvVykKAKAQpKSm4//77oapgIUAUgszMTPz4xz9Gaqpd94WAFlHRGdjvdGN3SS2KK10ob+qEV1ZhlQTMyEhAYa4daxdkG2Z+aKLxwMwQaWPUzIR6/DZ0MTBwMgjfpA9D+ZYXOuzYtlpfk0EQjTdmhkgbo2cm6ouBvaVO00wTSRQOzAyRNtGQmai+ZmBvqRMbdpVCy1PaZUWFDBUbdpUCgG4+KKLxwMwQaWO2zBju1sLKZg/uLirT9AENpKL/wRNVzZ5wDotIt5gZIm3MmBnDFQOb9vS3bE6HrKrYuKcsTCMi0jdmhkgbM2bGUKcJ9jndKK5yDVt+9LGbkbn6PyBYY+F6/X8ge9oAVUHyBdcj/syLh60vKyqKq1zY73Tr8upPonBhZoi0GZoZX1ZiJub5l7W+8wccL/8YEPr/nk5etAbxc5YM2o7RMmOoYqCopBYWUUBfkAs5Wl59BPEFlyFx3hWQu9yof3oDYqfOgSUxfdi6kihgd0mtIT4kolPFzBBpM1pmACB54SqkLvkWAKCvoxl1T34HNsd8SHGDs2GkzBjqNEFxpWvED6i3sRIT8s4FAEhxyYjJzEXX5+8FXFdWVHxS1Tom4yTSC2aGSJvRMgMAoi3B/7Xa233ii+E/Y6TMGKoYKG/qHPH1mEkz4DnwFgDA23YMPc4v0OduDLr+4caOsI6PSG+YGSJtRsuMT/s/X4LzidtR//T3kXblOkjxKQHXM0pmDHOaQFFUeOWRq7X0q3+A1jefQt2O9bAkZcI2bR4gBq93vLIKRVEhikLQdYiMipkh0iaUzPgknbsSSeeuRG9DBZpf+QVsuWdDmjD8Pn6jZMYwxYAoCrBKwogflCVlIjJWnXxyYcOu+2HLPTvo+lZJ0P0HRHSqmBkibULJzFAxE/MgJaShu3of4mdfMOx1o2TGUKcJZmQkjPi67GmFqioAgOMVJfC21A67wnOgmZmJYR0fkd4wM0TajJYZAOhtrvF/7W2tR29DBWLScwKua5TMGKYzAACFuXZ82dg5fFpIVYZgsaKrvBjtHxUBoggpwY7MtT+BaI0NuC1JFHCeI3UcRk0UOcwMkTaBMtOw634IkuT/3mqfCtnTCogSBFGCffkdsKZnD9uWkTJjqGJg7YJs7PyoetCyvk4XlJ4uSEkZSJx3BRLnXRHStmRFxdoFwz88omjCzBBpMzQzU+/cccrbMlJmDHWaID8rGYUOO6QT51/ai/+Mhj/eg9Slt0C02kLejiQKKHTYDXHvJ9HpYGaItBmamVNltMwY7qmFVc0eLH/kXfTKyilvI0YS8fqGi3X1mEmiscLMEGkTTZkJ9fhtqM4AADjS47F9TQFOtWYTAGxfo6/nTRONJWaGSBszZsZQ1wz4+B4LafTnTBONF2aGSBuzZcZwpwkGqmr2YOOeMhRXuSCJQsAPy7d8Ya4dD68yVqVGFG7MDJE2Rs9MqMdvQxcDPm+VluNnu95Co5qENtUGiBYIqoJkdOGSuVNx69J8w1zEQTQe3ig5hG1F76JpaGaELizNn4abLzmTmSEa4C8fH8B//flDNCEJbYoNkE5m5rICB266eLYuMxPq8duQpwmG6q4/jDceug2SJEFRFAD9Mz5VyTJ+uHMn8rOGzwpFZGbtVfvx9yCZua+oCPlZiyI9RCJdaTpUgr8/HDgzP3vtNV0WAlpERWdAlmWceeaZOHLkyIkPqV9mZiaqq6ths4V+CxWRGXi9XkyfPh1Hjx6FbxcgCAKys7NRXl4Oq9Ua4RES6Ut3dzdycnLQ1NTkXyaKImbMmIGDBw9CGjApkZ5E7d0EgUiShC1btgwqBARBwH333cdCgCgAq9WKzZs3Y+DfAqqqYvPmzSwEiAKw2Wy47777IAgn7zFQFAVbtmzRbSGgRVR0BoCT3YHDhw8DYFeAaDS+7kBtbS0AICcnh10BohEM7Q7MmjVL110BwGSdAeBkd8CHXQGikfm6Az7sChCNzNcd8ImWrgAQRZ0BoL87kJqaip6eHrjdbhYDRKPwer1ITu6/8MntdrMYIBpFd3c3kpOTYbPZ4HK5dF8MmOpuAh9JkvCXv/wFbSwEiEJitVrx8ssvQz3xNRGNzGaz4X//93+RkJio+0JAi6joDOx3urG7pBbFlS6UN3XCK6uwSgJmZCSgMNeOtQuyDX/bB1E4MTNE2hg1M6aYdEjLzFCFDju2rdbXzFBE442ZIdLG6JmJ+mJgb6nTNHNGE4UDM0OkTTRkJqqvGdhb6sSGXaUY/aM5SVZUyFCxYVcpAOjmgyIaD8wMkTZmy4zhbi2sbPbg7qIyTR/QQCr6n0JV1ewJ57CIdIuZIdLGjJkxXDGwaU9/y+Z0yKqKjXvKwjQiIn1jZoi0MWNmDHWaYJ/TjeIq16BlRx+7GZmr/wMxE/P8y1rf+QOOl38MCP21TvKiNYifs8T/uqyoKK5yYb/TrcurP4nCZWhmTiUvADND5hHoOAOczI5gjYXr9f+B7GkDVAXJF1yP+DMvHra+0TJjqGKgqKQWFlFA3ygXciQvXIXUJd8CAPR1NKPuye/A5pgPKe7kByKJAnaX1BriQyI6VaFkJpS8AMwMmcNomWl59RHEF1yGxHlXQO5yo/7pDYidOgeWxPRh6xopM4Y6TVBc6Rq1EAAA0Zbg/1rt7T7xxeCfkxUVn1S1hnV8RHoTSmZCyQvAzJA5jJaZ3sZKTMg7FwAgxSUjJjMXXZ+/F3BdI2XGUJ2B8qbOkNdt/+dL6Pj0VcgdzUi76nuQ4lOGrXO4sSOMoyPSn1AzE0peAGaGot9omYmZNAOeA28hedEaeNuOocf5BSzJE4Oub5TMGKYYUBQVXjn0CzqSzl2JpHNXorehAs2v/AK23LMhTRh8j6VXVqEoKkRRCLIVIuPSkplQ8gIwMxTdQslM+tU/QOubT6Fux3pYkjJhmzYPEIM32Y2SGcOcJhBFAVZJ+z9mzMQ8SAlp6K7eN+w1qyTo/gMiOlWnkpmR8gIwMxTdQsmMJWUiMlbdiyk3/waZa+6D0uOBNX1a0PWNkhnDFAMAMCMjYfSVAPQ21/i/9rbWo7ehAjHpOcPWm5mZGLaxEelRKJkJNS8AM0PRb7TMyJ5WqKoCADheUQJvS+2wu28GMkpmDHOaAAAKc+34srFz2LSQDbvuhzDg6VFW+1TInlZAlCCIEuzL74A1PXvQz0iigPMcqeMybqJICZSZU8kLwMyQOQQ7zkCVIVis6CovRvtHRYAoQkqwI3PtTyBaYwNuy0iZMVQxsHZBNnZ+VD1o2dQ7d5zStmRFxdoFw3d4RNFkaGZONS8AM0PmEOg409fpgtLTBSkpA4nzrkDivCtC2paRMmOo0wT5WckodNghneb5F0kUUOiwG+LeT6LTwcwQaTM0M+3Ff0bDH+9B6tJbIFptIW/HaJkxVDEAANtWF0ASTnPHJgjYtrogTCMi0jdmhkibgZlJKrwOWbc9jsT5V2rahtEyY7hiwJEej+1rCnCquzYBwPY1+nreNNFYYmaItDFjZgx1zYCP77GQRn/ONNF4YWaItDFbZgRVHf3RTO3t7UhOTobb7UZS0vCJSCKlqtmDjXvKUFzlgiQKAT8s3/KFuXY8vMpYlRpRuDEzRNoYPTOhHr8NXQz4vPjWJ7jv6dfQkzgV3rh0CJIFUGRYPE24ND8H37+m0DAXcRCNh12vf4gtz76OnqTsYZm5bF4u1n1lATNDNMAzr76Dn73wFnqTB2fG2tWE5WfPwHeumK/LzIR6/DbkaYKhsuIUHPzD5gFLBAD9Nc4jL7+syw+IKJIyrT04+MwDA5aczMyTb77JzBANkap24PNnA2fmuY8/NnxmDHcBYSALFy7E0qVLIfknUlEhiiLy8/Nx9dVXR3RsRHp0ySWXYOHChYMyI0kSFi1ahEsuuSSSQyPSpauvvhr5+fkQ/c8h6M/MsmXLUFhYGNGxhUNUFAMAsGXLFsiy7P9eURRs3boVwmneUkUUjQRBwNatWwdlRpZl/PSnP2VmiALwZUZRFP8yWZaxZcuWCI4qfKKmGLjggguwdOlS/44sPz8fK1eujPCoiPRr2bJlWLhwIQRBgCAIWLRoEZYtWxbpYRHp1sqVK5Gfnw+gvzhYtmwZzj///AiPKjyiphgA+rsDvush2RUgGpnvLx1VVaGqKrsCRKPwZQYAVFWNmq4AECV3Eww0bdo0dHR0oKWlhTs2olGoqorJkycDAOrr65kZolGoqoq0tDQkJiaiurp69B+IMFPdTTDQgQMH0N3Tw50aUQgEQcChQ4egqCozQxQCQRBQXl4Oi9Ua6aGEVVQUA/udbuwuqUVxpQvlTZ3wyiqskoAZGQkozLVj7YJsw9/2QRROzAyRNtGeGUOfJtAyM1Shw45tq/U1MxTReGNmiLQxemaifgbCvaVO08wZTRQOzAyRNtGQmai+ZmBvqRMbdpVi9I/mJFlRIUPFhl2lAKCbD4poPDAzRNqYLTOGu7WwstmDu4vKNH1AA6nofwpVVbMnnMMi0i1mhkgbM2bGcMXApj39LZuBjj52M3obKuB1OdHwwn+g7ql1qPvdnfB8/m7Abciqio17ysZjuEQRx8wQaTMwM76sDNT6zh9Q99R3UbdjPep2rIfn4DvDtmG0zBjqNME+pxvFVa6gr7e8+gjiCy5D4rwrIHe5Uf/0BsROnQNLYvqg9WRFRXGVC/udbkNf/Uk0GmaGSJvRMgMAyQtXIXXJtwAAfR3NqHvyO7A55kOKO5kNo2XGUJ2BopJaWMTg90L3NlZiQt65AAApLhkxmbno+vy9gOtKooDdJbVjMk4ivWBmiLQZLTMAINoS/F+rvd0nvgh8l4FRMmOoYqC40oW+Ea7ojJk0A54DbwEAvG3H0OP8An3uxoDryoqKT6pax2ScRHrBzBBpM1pmfNr/+RKcT9yO+qe/j7Qr10GKTxm2jpEyY6jTBOVNnSO+nn71D9D65lOo27EelqRM2KbNA8Tg9c7hxo5wD5FIV5gZIm1Gy4xP0rkrkXTuSvQ2VKD5lV/Alns2pAnDb90zSmYMUwwoigqvPHK1ZkmZiIxV9/q/b9h1P2y5Zwdd3yurUBQV4igtISIjYmaItAklM0PFTMyDlJCG7up9iJ99wbDXjZIZw5wmEEUBVmnkf0zZ0wpV7X/W9PGKEnhbahE/Z0nQ9a2SoPsPiOhUMTNE2oSSGQDoba7xf+1trUdvQwVi0nMCrmuUzBimMwAAMzIS8PmxAC0XVYZgsaKrvBjtHxUBoggpwY7MtT+BaI0Nur2ZmYljOFqiyGNmiLQJlJmGXfdDkCT/91b7VMieVkCUIIgS7MvvgDU9O+D2jJIZQxUDhbl2fNnYOWhayL5OF5SeLkhJGUicdwUS510R0rYkUcB5jtSxGiqRLjAzRNoMzczUO3ec8raMlBnDnCYAgLULsgft1NqL/4yGP96D1KW3QLTaNG1LVlSsXRC4kiOKFswMkTZDM3M6jJQZQ3UG8rOSUeiwo6SmFbKiIqnwOiQVXqd5O5IoYEFOqiEmgiA6HcwMkTZDM3OqjJYZQ3UGAGDb6gJIwuldjCEJAratLgjTiIj0jZkh0saMmTFcMeBIj8f2NQU41Y9JALB9jb6eN000lpgZIm3MmBlDnSbw8T0W0ujPmSYaL8wMkTZmy4ygqgEmVB6ivb0dycnJcLvdSEoaPsNSpFQ1e7BxTxmKq1yQRCHgh+VbvjDXjodXGatSIwo3ZoZIG6NnJtTjt6GLAZ8df34DG3/7ImKyzoQ1PQeCZIUqe+FtrsGl+Tm452uXGuYiDqLx8D8vvIL/2PEqYqcOz8xl8/OwcfVFzAzRAP/1dBEe/OPfETt1zpDM1OLKBTPxw39brMvMmKoYqKiowMyZM6EoyoklAoD+t/XPf/4TCxYsiNjYiPTo4MGDyM/Px8n492dGEAQcOHAAZ555ZiSHR6Q7JSUlOPfccwcs6c+MJEn48ssvkZeXF6mhjSjU47fhLiAMJC8vDzfeeCMk/wxR/R/QihUrWAgQBTBnzhx89atfhcXiu2xIhcViwde+9jUWAkQBLFiwAFdfffWgzEiShBtvvFG3hYAWUdEZAPq7A7NmzYIsy/5lJSUlOOeccyI4KiL9GtodYFeAaGRDuwOSJOHw4cPIzc2N4KhGZqrOAHCyOyAIAgRBwIoVK1gIEI3A1x3wZYZdAaKR+boDvszcdNNNui4EtIiazgDQ3x2YMWMGVFVlV4AoBAcPHsTcuXP9X7MYIBqZrzsgCAKOHDmi+2Ig1OO3IecZCCYvLw9Lly5FS0sLCwGiEMyZMweLFy8GABYCRCFYsGAB5s+fj7S0NN0XAlpEVWfAR1FUQzw/mkgvmBkibYySGVN1BvY73dhdUoviShfKmzrhlVVYJQEzMhJQmGvH2gXZurz/kyhSmBkibaI9M4buDGiZGarQYce21fqaGYpovDEzRNoYPTNRP+nQ3lKnaeaMJgoHZoZIm2jITFSfJthb6sSGXaXQ8qRpWVEhQ8WGXaUAoJsPimg8MDNE2pgtM4abZ6Cy2YO7i8o0fUADqeh/ClVVsyecwyLSLWaGSBszZsZwxcCmPf0tG5+jj92M3oaKQeu0vvMH1D31XdTtWI+6HevhOfjOoNdlVcXGPWXjMl6iSBuYmVPJC8DMkLls2lOGykdvgvOJO6AqJ2e1rX96A7qry9D23nOofmgFug596H9NVVUc/Z9bUPPLrwEwXmYMdZpgn9ON4irXqOslL1yF1CXfAgD0dTSj7snvwOaYDymu/0pPWVFRXOXCfqfb0Fd/Eo0mlMyMlheAmSHz8GVGVQHIXnR+9joSz75q2Hoxk2ags+wNxJ1xPgCgu/ozSBOSoHR3AjBeZgzVGSgqqYUlhPs6RVuC/2u1t/vEF4MbPpIoYHdJbVjHR6Q3oWQmlLwAzAyZw8DMpFz4Dbg/fAGKt3vYerFT56Cv7RjkzlYAQGfZG0gouHzQOkbKjKE6A8WVLvSFcEUnALT/8yV0fPoq5I5mpF31PUjxKYNelxUVn1S1jsEoifQj1MyMlheAmSFzGJiZmMxc2HIK0PHJXiSf/7Vh68bPvRSd+/+BxPlXorf+S6Rc/O/AOzv9rxspM4YqBsqbOkNeN+nclUg6dyV6GyrQ/MovYMs9G9KEwbdVHG7sCPcQiXQl1MyEkheAmaHoNzQzKRfdgPo//BAJAU4VxJ+1FI0v3AfRakPc7IsgYHgXziiZMcxpAkVR4ZW1X9sZMzEPUkIauqv3DXvNK6tQQuw0EBnNqWRmpLwAzAxFt0CZsaRMRPycJXB/sGvY+pbEdEjJmWj74HkkFFwWcJtGyYxhigFRFGCVQpsHure5xv+1t7UevQ0ViEnPGbaeVRIMMbc00akINTOh5gVgZii6BctM8vlfg+fAW5A7h1+Mm3LRDUi5+N9hTZ0ScJtGyYyhThPMyEjA58eGt1wadt0PQZL831vtUyF7WgFRgiBKsC+/A9b07GE/NzMzcUzHSxRpgTJzqnkBmBmKfoEyI8UlI/Hca+B+77lh68dOnonYyTODbs8omTFUMVCYa8eXjZ2DpoWceueOU9qWJAo4z5EarqER6dLQzJxqXgBmhszBl5mhWUm54OtIueDrAADbtIKAP2tJmYicH5w8nWCkzBjmNAEArF2QHdL80KGQFRVrFwT+64coWjAzRNqYNTOGKgbys5JR6LBDOs3zL5IooNBhN8REEESng5kh0sasmTFUMQAA21YXQBJO80MSBGxbHbjNQxRtmBkibcyYGcMVA470eGxfUxDgbs7QCAC2r9HX86aJxhIzQ6SNGTNjqAsIfXyPhTT6c6aJxgszQ6SN2TIjqGqASciHaG9vR3JyMtxuN5KShs9KFilVzR5s3FOG4ioXRAEI9FlJogBZUbEw146HVxmrUiMKN2aGSBujZybU47fhThMM5EiPx59uX4wNZ3aj7Z8vo7fhCFTZCwBQZS96G45gXlw7Xll3IXbdtlhXHxBRJPgyc4ejLWhmzk3uYmaITvBl5sZJ9UEzsyjda/jMGLoz4NPW1obs7Gx0dvrmlBYAqLBYLKisrMTUqVMjOTwi3WlqakJOTg66u31PY+vPjM1mQ01NDTIyMiI5PCLdOXr0KHJzc9HX13diSX9mEhMTUVNTg5SUlAiOLjhTdAZ8UlJScNddd0EUfW9HhSRJuO2221gIEAWQkZGB9evXQ/LPRNifmfXr17MQIApg6tSpuO222wZlRhRF3HXXXbotBLSIis4AMLw7wK4A0ciGdgfYFSAa2dDugN67AoDJOgPAye6AD7sCRCPzdQd82BUgGpmvO+ATLV0BIIo6A0B/dyAjIwOyLKOmpobFANEompqaMGnSJADAsWPHWAwQjeLo0aPIycmBJEloamrSfTEQ6vHbkPMMBOPrDjS3tLAQIApBRkYG1q1bB/XE10Q0sqlTp+LWW2+FPS1N94WAFlHRGdjvdGN3SS2KK10ob+qEV1ZhlQTMyEhAYa4daxdkG2Z+aKLxwMwQaWPUzIR6/DZ0MTBwMgjfpA9D+ZYXOuzYtlpfk0EQjTdmhkgbo2cm6ouBvaVO00wTSRQOzAyRNtGQmai+ZmBvqRMbdpVCyxOnZUWFDBUbdpUCgG4+KKLxwMwQaWO2zBju1sLKZg/uLirT9AENpKL/wRNVzZ5wDotIt5gZIm3MmBnDFQOb9vS3bE6HrKrYuKcsTCMi0jdmhkgbM2bGUKcJ9jndKK5yAQCaXtoOS3ImUpfc6H+9cfdmxOYUoO3tp2HNmAac+DBTltyIuBnn+deTFRXFVS7sd7p1efUnUbgwM0Ta7HO68fqzj0Jub0baV74HAOiuPYCG5zZh4tf/E7ZpBQCAlr8+CikuGZ3734JgsUKwxPi3kb7iR4jJdBgqM4bqDBSV1MIiCgAA+/LvwHPgbfTUfwkA6PjsdSg9XYg743wIMRMw5ebfYMotjyJlybfQ/NI2qIo8aFuSKGB3Se24vwei8cTMEGlTVFKLeMc8dNfs8y/rrilDzJQzhizbB9u0eQCAjGs39efnxH8xmQ4AxsqMoYqB4koX+k5c0SnZEpB25Tq0vPoIvC4n3O89i7QVP4QAYdDP2KbNg9p7HEp356DlsqLik6rWcRs7USQwM0TaFFe6YJk8C3KnC33tzQCAnpp9SLngen8x0NfpgtzehNis2SNuy0iZMdRpgvKmwTunCXkL0PXlR6jf+UOkXnITrCmT0NfWMGidrkMfwDatAFLc8DbN4caOMR0vUaQxM0TalDd1QpCsiM2aje6aMsTPvgh9bQ2YMP08uN54AmpfL3qqyxAzZbb/1EDT3ocHnSaY9O8/h2iNBWCczBimGFAUFV55+AUdSQtXw3PwHSSefZV/mdp7HHU71kPp7oTS1Y6JX38w4Da9sgpFUSGKQsDXiYyMmSHSZmBmbNMK0F2zD5akTMRMmQUAiM06Az3OL06cIjjL/3MZ125CzMS8gNs0SmYMc5pAFAVYpeH/mIIgAsLgt+E7/5n1nR1IWrQGTXu3Qe3rHfazVknQ/QdEdKqYGSJtBmYmNqcAPdVl6K4pgy2n/8Afm52Pbt+yE9cLjMYomTFMMQAAMzISNK0vCAKSL7geUlwSOj59bdjrMzMTwzU0Il1iZoi08WUmdvJMyF1ueA687S8GbDlnwfP5u5A7WxE7eVZI2zNKZgxVDBTm2iFprLAEQUDq0lvg/rgIirfbv1wSBZznSA33EIl0hZkh0saXGUGyIHbqHKi9x2FNywYAWO1ZUHuPI3bqHAjSybPsTXsfRt2O9f7/uqv75xcwUmYM9WyC/U43Vjz6fti298q6Cw1x/yfRqWJmiLSJtsyEevw2VGcgPysZhQ7tf+kMJYkCCh127tQo6jEzRNqYNTOGKgYAYNvqAkjCaX5IgoBtqwvCNCIifWNmiLQxY2YMVww40uOxfU0BTvVjEgBsX6Ov500TjSVmhkgbM2bGMPMMDOR7LKTRnzNNNF6YGSJtzJYZQ11AOFRVswcb95ShuMoFSRQCfli+5Qtz7Xh4lbEqNaJwY2aItDF6ZkI9fhu6GPB5/0A1thW9B2evDS19VkC0QIQCu9SDJXOy8e0lsw1zEQfReHhnXwV+/uIHqOudMCQzvbg0Pwc3XjSLmSEaYL/Tjd0ltfikqhWHGzvglVVYJQEzMxNxniMVaxdk6zIzoR6/DXmaYKjWijK8vPnfIYoi+msbAYIAVCoK7nzqKeRnLYz0EIl0pf5AMV7Z/K2Ambn7hReQn3XeqNsgMpP8rORBB3sjTDGsRVR0BrxeL2bMmIHa2loMfDspKSmora1FQoK2WdiIol13dzemTZuGxsbGQcsnTpyIqqoq2Gy2CI2MiMIpKucZCMZqtWLz5s2DCgFBEHDPPfewECAKwGaz4b777oMw4PYpQRBw3333sRAgMqGo6AwAJ7sDNTU1ANgVIBrN0O4AuwJE0cdUnQHgZHfAh10BopH5ugM+7AoQmVfUdAaA/u5ASkoKent70draymKAaBTd3d1ISUkBALS1tbEYIIoyprqbwMdqteL5559Hm7udhQBRCGw2G5555hkoKlgIEJlYVHQGfPd/Fle6UN7U6b//c0ZGAgpz7bq9/5MoUpgZInMwxaRDWmaGKnTYsW21vmaGIhpvzAyRuUR9MbC31GmaOaOJwoGZITKfqL5mYG+pExt2lWL03dlJsqJChooNu0oBgDs3MhVmhohGYrhbCyubPbi7qEzTTm0gFf1Poapq9oRzWES6xcwQ0WgMVwxs2tPf5jwdsqpi456yMI2ISN+YGSIajaGKgX1ON4qrXKh+9NtwPnE76nash/OJ2+H+aDcAoK+tAdUPrUBj0U8H/Vzbe8+h+qEV6PryIwD97c/iKhf2O93j/h6IxpMvM7Ki4uhjN6O3oWLQ682v/BLVD61A77Ej/mVKTxdqfrEGdTvW+5cxM0TRzVDFQFFJLSwnnhKVce0mTLn5N5j49Qfh/r/d6Kk7BAAQYuPhbXVC9rQCAFRVgefgO7BmOAZtSxIF7C6pHdfxE423gZkJJmbSDHSWveH/3vPFe7CmZQ9bj5khil6GKgaKK13oG3IVtCUxHVb7VPS1N/mXJcy9FJ373wQAdFeVImbidIi2wZMQyYqKT6pax37QRBEUKDNDxc06H11HPoHa5wUAeMr+joSCy4etx8wQRS9DFQPlTZ3DlnlbaqEc74At5yz/svj8ZfDs+wcAoLPsDSQUXBZwe4cbO8ZmoEQ6ESgzQwnWWExwzEfX4Y/gbakFoMKaNjXguswMUXQyzK2FiqLCK5/8C6dp78OAIKCvxYnUZbdCiktGX283AMCSlA4pMR1d5cXoPVYO28q7/dcVDOSVVSiKCnGUNiqREQ3NzEgSCi5H23vPISYzF/FnBS6eAWaGKFoZphgQRQFWSfDv3DKu3YSYiXk4XlWKpqItsE2bBzFmgn/9hILL0PLqI0g8+yoIQuAGiFUSuFOjqDU0MyOJzZoNubMFnpZaTLn1MfQeKw+4HjNDFJ0MdZpgRsbwhw9NcMxHwtlXoe3dZwYtj5u5GEkLVyHh7KuCbm9mZmLYx0ikJ4EyE0zqZbchdenNEGPjgq7DzBBFJ8N0BgCgMNeOLxuHnwNNvuDrqPvt/4PcffJ8pmCxInnRmqDbkkQB5zlSx2ScRHrhy4xv+uGGXfdDkCT/61b7VGBiHoD+wnokzAxR9DLUswn2O91Y8ej7YdveK+su5JPZKKoxM0TmFurx21CnCfKzklHosEM6zXOWkiig0GHnTo2iHjNDRKEwVDEAANtWF0ASTnPHJgjYtrogTCMi0jdmhohGY7hiwJEej+1rCnCquzYBwPY1fEY7mQczQ0SjMdQFhD6+R6ny2exEoWFmiGgkhusM+Fw7Pwuvb7gYC3L6r24Odk7Ut/zcaal4fcPF3KmRaTEzRBSMoe4mCOblDz7Dlmf+hg7bRByPSYEgWSCoMmw9rbhkbja+e9UCXvhENMCf3/4nHnzhTXQGyMyyAgfuWD6fmSGKAqEevw15mmCoZNmNfz6+acASAUB/jfPgnj3cqRENMaG7GSVBMvPIX//KzBCZjGFPEwx00UUXYdGiRZD8k6moEEURM2fOxLXXXhvRsRHp0RVXXIGzzjoLoujbBfRnpqCgAMuXL4/o2Iho/EVFMSAIAn76059ClmX/MkVRsGXLlgEFAhH5CIKArVu3QlEU/zJFUbB161YIp3kbIhEZT1QUAwCwbNkyLFq0yL8jmzlzJtauXRvhURHp1zXXXIOzzjr56O+CggKsWLEigiMiokiJmmLA95eO73pIdgWIRubLjA+7AkTmFRV3E/ioqorJkyfD4/Ggra2NxQDRKFRVRVpaGgCgpaWFxQBRlDHV3QQ+giDg008/xfHubhYCRCEQBAGfffYZFFVlIUBkYlFRDOx3urG7pBbFlS6UN3XCK6uwSl9gRkYCCnPtWLsgm7dKEQ0QODP7mRkikzL0aYKqZg827ilDcZULkigEnGLVt7zQYce21ZxfncyNmSEyl1CP34YtBvaWOjnPOpEGzAyR+UT1NQN7S53YsKsUo+/OTpIVFTJUbNhVCgDcuZGpMDNENBLD3VpY2ezB3UVlmnZqA6nof3JbVbMnnMMi0i1mhohGY7hiYNOe/jbn6ZBVFRv3lIVpRET6xswQ0WgMdZpgn9ON1599FHJ7M9K+8j0AQHftATQ8twkTv/6fsE0rAAC0/PVRSHHJSL7wG3B/uAueg+9CEEVAlBA7eRZSL/02iqtU7He6ecU0RTVmhohCYahioKikFvGOeWh85RH/su6aMsRMOQPdNfv8O7bumn1Iu+K7aHntV1COd2DSt34OyZYAVVXRdegDyN2dsMYlYndJLXdsFNWYGSIKhaFOExRXumCZPAtypwt97c0AgJ6afUi54Hp01+wDAPR1uiC3N0GMS0LXFx8g7eoNkGwJAPonWImffSGsKZMgKyo+qWqN2HshGg/MDBGFwlDFQHlTJwTJitis2eiuKYPa50VfWwMmTD8PckcL1L5e9FSXIWbKbHiba2FJnQwpLvhfMYcbO8Zx9ETjj5kholAY5jSBoqjwyv0XQdmmFaC7Zh8sSZmImTILABCbdQZ6nF+caH2eNdKm/LyyCkVRIYqchpWiDzNDRKEyTGdAFAVYpf4dUGxOAXqqy9BdUwZbTv9OLDY7H92+ZdPmIWbSdPS11kM+3h50m1ZJ4E6NohYzQ0ShMkwxAAAzMvrPY8ZOngm5yw3Pgbf9OzZbzlnwfP4u5M5WxE6eBWvqFMSdcT5aXvs1lO5OAP1PaPN88QG8bccAADMzEyPzRojGCTNDRKEwzGkCACjMtePLxk7IsCB26hx4GythTcsGAFjtWVB7jyN26hwIUv/bSvvK9+H+cBfq//AjCKIEVVVgy87HBMc8SKKA8xypkXw7RGOOmSGiUBjq2QT7nW6sePT9sG3vlXUX8jYpimrMDJG5hXr8NtRpgvysZBQ67JBO85ylJAoodNi5U6Oox8wQUSgMVQwAwLbVBZCE09yxCQK2rS4I04iI9I2ZIaLRGK4YcKTHY/uaApzqrk0AsH0Nn9FO5sHMENFoDHUBoY/vUap8NjtRaJgZIhqJ4ToDPtfOz8LrGy7Ggpz+q5uDnRP1LT93Wipe33Axd2pkWswMEQVjqLsJgtnvdGN3SS0+qWrF4cYOeGUVVknAzMxEnOdIxdoF2bzwiWgAZobIHEI9fhvyNMFQ+VnJg3ZcnC6VaGTMDBENZNjTBCPhTo1IG2aGyNyishggIiKi0LEYICIiMjkWA0RERCbHYoCIiMjkWAwQERGZHIsBIiIik2MxQEREZHIsBoiIiEyOxQAREZHJsRggIiIyORYDREREJsdigIiIyORYDBAREZkciwEiIiKTYzFARERkciwGiIiITI7FABERkcmxGCAiIjI5SygrqaoKAGhvbx/TwRAREVH4+I7bvuN4MCEVAx0dHQCA7Ozs0xwWERERjbeOjg4kJycHfV1QRysXACiKgrq6OiQmJkIQhLAOkIiIiMaGqqro6OjAlClTIIrBrwwIqRggIiKi6MULCImIiEyOxQAREZHJsRggIiIyORYDREREJsdigIiIyORYDBAREZkciwEiIiKT+/+FruGT80gMCAAAAABJRU5ErkJggg==", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "\n", + "i=9\n", + "graph = GraphGrammar()\n", + "rules = [\"Init\",\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Positive_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",\"Rotation\",\"R_Wheel\", \"Cyl_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Positive_Translation_X4\",\"Negative_Translation_Z3\",\"Rotation\",\n", + " 'Rotation_Y', \"Extension\",\"Extension\",\"Rotation\",\"L_Wheel\", \"Cyl_Wheel\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', 'Rotation_Y',\n", + "\n", + " \"Add_Leg_Base\",\"Negative_Translation_X4\",\"Negative_Translation_Z0\", \"Extension\",\"Extension\",\n", + " \"Terminal_Link3\",\"Terminal_Link3\",f'Terminal_Joint{i}',f'Terminal_Joint{i}', \"Wheel\", \"Ell_Wheel\",\n", + " 'Terminal_Main_Body3'\n", + " \n", + "]\n", + "\n", + "for rule in rules:\n", + " graph.apply_rule(rule_vocab.get_rule(rule))\n", + "\n", + "\n", + "plot_graph(graph)" + ] + }, + { + "cell_type": "code", + "execution_count": 30, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Irrlicht Engine version 1.8.5\n", + "Linux 6.5.0-45-generic #45~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Mon Jul 15 16:40:02 UTC 2 x86_64\n", + "Using plain X visual\n", + "This driver is not available in Linux. Try OpenGL or Software renderer.\n", + "Irrlicht Engine version 1.8.5\n", + "Linux 6.5.0-45-generic #45~22.04.1-Ubuntu SMP PREEMPT_DYNAMIC Mon Jul 15 16:40:02 UTC 2 x86_64\n", + "Using renderer: OpenGL 4.6\n", + "Mesa Intel(R) UHD Graphics 630 (CFL GT2): Intel\n", + "OpenGL driver version is 1.2 or better.\n", + "GLSL version: 4.6\n", + "Loaded texture: /home/be2r-lab-210134/Python/anaconda3/envs/rostok/share/chrono/data/fonts/arial80.bmp\n", + "Loaded texture: /home/be2r-lab-210134/Python/anaconda3/envs/rostok/share/chrono/data/skybox/sky_lf.jpg\n", + "Loaded texture: /home/be2r-lab-210134/Python/anaconda3/envs/rostok/share/chrono/data/skybox/sky_up.jpg\n", + "Loaded texture: /home/be2r-lab-210134/Python/anaconda3/envs/rostok/share/chrono/data/skybox/sky_dn.jpg\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Cannot use default video driver - fall back to OpenGL\n" + ] + } + ], + "source": [ + "from rostok.simulation_chrono.simulation_scenario import WalkingScenario\n", + "from rostok.library.rule_sets.leg_rules import get_biped\n", + "from rostok.library.rule_sets.ruleset_simple_wheels import get_four_wheels\n", + "from rostok.graph_grammar.graph_utils import plot_graph\n", + "\n", + "scenario = WalkingScenario(0.0001, 3)\n", + "\n", + "control = control_parameters = {\"initial_value\": [0.05]*2}\n", + "\n", + "scenario.run_simulation(graph, control, starting_positions=[[45,-90,0], [-90,90,0], [90,-90,0]], vis = True, delay=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 +} diff --git a/tests_jupyter/wheels_sim.py b/tests_jupyter/wheels_sim.py new file mode 100644 index 00000000..3bb80150 --- /dev/null +++ b/tests_jupyter/wheels_sim.py @@ -0,0 +1,12 @@ +from rostok.simulation_chrono.simulation_scenario import WalkingScenario + +from wheels import get_wheels +from rostok.graph_grammar.graph_utils import plot_graph + +scenario = WalkingScenario(0.0001, 3) +graph = get_wheels() + +control = control_parameters = {"initial_value": [0.05]*2} + +scenario.run_simulation(graph, control, starting_positions=[[45,-90,0], [-90,90,0], [90,-90,0]], vis = True, delay=True) +