diff --git a/rostok/control_chrono/controller.py b/rostok/control_chrono/controller.py index 163b3fd1..54f2dff8 100644 --- a/rostok/control_chrono/controller.py +++ b/rostok/control_chrono/controller.py @@ -19,7 +19,7 @@ class RobotControllerChrono: functions: list of functions currently attached to joints """ - def __init__(self, built_graph: BuiltGraphChrono, parameters): + def __init__(self, built_graph: BuiltGraphChrono, parameters = {}): """Initialize class fields and call the initialize_functions() to set starting state""" self.built_graph = built_graph self.graph = built_graph.graph @@ -42,14 +42,19 @@ def set_function(self): def initialize_functions(self): """Attach initial functions to the joints.""" i = 0 + self.active_j_to_fun_id = {} for idx, joint in self.joint_map_ordered.items(): if self.chrono_joint_setters[joint.input_type] == 'Uncontrol': pass else: - chr_function = chrono.ChFunction_Const(float(self.parameters["initial_value"][i])) + if 'initial_value' in self.parameters.keys(): + chr_function = chrono.ChFunction_Const(float(self.parameters["initial_value"][i])) + else: + chr_function = chrono.ChFunction_Const(0) joint_setter = getattr(joint.joint, self.chrono_joint_setters[joint.input_type]) joint_setter(chr_function) self.functions.append(chr_function) + self.active_j_to_fun_id[idx] = i i += 1 @abstractmethod @@ -65,42 +70,53 @@ def update_functions(self, time, robot_data, environment_data): class SimpleKeyBoardController(RobotControllerChrono): def __init__(self, built_graph: BuiltGraphChrono, parameters): super().__init__(built_graph, parameters) - + + self.forward_torque = self.parameters["forward"] + self.reverse_torque = self.parameters["reverse"] + + self.forward_rotate = self.parameters["forward_rotate"] + self.reverse_rotate = self.parameters["reverse_rotate"] + def update_functions(self, time, robot_data, environment_data): left_wheel = [] right_wheel = [] + ordered_list_id = list(self.active_j_to_fun_id.keys()) + for number, joint_id in enumerate(self.joint_map_ordered): self.joint_map_ordered[joint_id].name side_indicator = str(self.joint_map_ordered[joint_id].name).split('_')[0] - # Some logic about indicate - left_wheel = [0, 2] - right_wheel = [1, 3] - + if side_indicator == "RM": + right_wheel.append(joint_id) + elif side_indicator == "LM": + left_wheel.append(joint_id) + else: + pass + + if keyboard.is_pressed('a'): - for i, func in enumerate(self.functions): + for func, i in zip(self.functions, ordered_list_id): if i in right_wheel: - func.Set_yconst(-0.3) - + func.Set_yconst(self.forward_rotate) if i in left_wheel: - func.Set_yconst(0.2) + func.Set_yconst(-self.reverse_rotate) elif keyboard.is_pressed('d'): - for i, func in enumerate(self.functions): + for func, i in zip(self.functions, ordered_list_id): if i in left_wheel: - func.Set_yconst(-0.3) - + func.Set_yconst(self.forward_rotate) if i in right_wheel: - func.Set_yconst(0.2) + func.Set_yconst(-self.reverse_rotate) + elif keyboard.is_pressed('w'): - - for i, func in enumerate(self.functions): - func.Set_yconst(-0.2) + for func in self.functions: + func.Set_yconst(self.forward_torque) + elif keyboard.is_pressed('s'): - for i, func in enumerate(self.functions): - func.Set_yconst(0.2) - + for func in self.functions: + func.Set_yconst(-self.reverse_torque) + else: - for i, func in enumerate(self.functions): + for func in self.functions: func.Set_yconst(0) diff --git a/rostok/simulation_chrono/simulation.py b/rostok/simulation_chrono/simulation.py index fff12273..1019a9c1 100644 --- a/rostok/simulation_chrono/simulation.py +++ b/rostok/simulation_chrono/simulation.py @@ -54,18 +54,19 @@ def __init__(self, fps=100, delay: bool = False, is_follow_camera = False): def initialize_vis(self, chrono_system, observed_body: chrono.ChBody = None): self.vis.AttachSystem(chrono_system) self.vis.SetWindowSize(1024, 768) - self.vis.SetWindowTitle('Grab demo') + self.vis.SetWindowTitle('Lets Ride') self.vis.Initialize() self.vis.AddSkyBox() self.vis.AddCamera(chrono.ChVectorD(-0.15, 0.35, 0.40), chrono.ChVectorD(0.0, 0.1, 0)) self.vis.AddLight(chrono.ChVectorD(0.0, 0.5, -0.3), 0.28, chrono.ChColor(0.7, 0.7, 0.7)) - self.vis.AddLight(chrono.ChVectorD(0.3, 0.0, 0.3), 0.28, chrono.ChColor(0.7, 0.7, 0.7)) - self.vis.AddLight(chrono.ChVectorD(-0.3, 0.0, -0.3), 0.28, chrono.ChColor(0.7, 0.7, 0.7)) - self.vis.AddLight(chrono.ChVectorD(-0.3, 0.0, 0.3), 0.28, chrono.ChColor(0.7, 0.7, 0.7)) + self.vis.AddLight(chrono.ChVectorD(0.3, 0.0, 0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) + self.vis.AddLight(chrono.ChVectorD(-0.3, 0.0, -0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) + self.vis.AddLight(chrono.ChVectorD(-0.3, 0.0, 0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) self.vis.AddLight(chrono.ChVectorD(0.3, 0.4, -0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) self.vis.AddLight(chrono.ChVectorD(0.3, 0.4, 0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) self.vis.AddLight(chrono.ChVectorD(-0.3, 0.4, -0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) self.vis.AddLight(chrono.ChVectorD(-0.3, 0.4, 0.3), 0.5, chrono.ChColor(0.7, 0.7, 0.7)) + self.vis.AddTypicalLights() if observed_body: self.bind_camera_to_body(observed_body) # self.vis.AddTypicalLights() diff --git a/rostok/simulation_chrono/simulation_scenario.py b/rostok/simulation_chrono/simulation_scenario.py index baac927e..68ca0237 100644 --- a/rostok/simulation_chrono/simulation_scenario.py +++ b/rostok/simulation_chrono/simulation_scenario.py @@ -163,7 +163,8 @@ def run_simulation(self, controller_data, starting_positions=None, vis=False, - delay=False): + delay=False, + is_follow_camera = False): # events should be reset before every simulation event_list = self.build_events() # build simulation from the subclasses @@ -171,21 +172,21 @@ def run_simulation(self, if self.smc: system = ChronoSystems.chrono_SMC_system(solver_iterations = 500, gravity_list=[0, -9, 0]) else: - system = ChronoSystems.chrono_NSC_system(gravity_list=[0, -9, 0]) + system = ChronoSystems.chrono_NSC_system(solver_iterations = 500, gravity_list=[0, -9, 0]) # setup the auxiliary env_creator = EnvCreator([]) - vis_manager = ChronoVisManager(delay, is_follow_camera=False) + vis_manager = ChronoVisManager(delay, is_follow_camera=is_follow_camera) simulation = SingleRobotSimulation(system, env_creator, vis_manager) if self.smc: def_mat = DefaultChronoMaterialSMC() else: def_mat = DefaultChronoMaterialNSC() - floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(2, 0.1, 2), material=def_mat, color=[215, 255, 0])) - chrono_material = chrono.ChMaterialSurfaceNSC() - chrono_material = chrono.ChMaterialSurfaceSMC() - mesh = chrono.ChBodyEasyMesh("Body5.obj", 8000, True, True, True, chrono_material, 0.002) - #floor.body = mesh + floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(5, 0.05, 5), material=def_mat, color=[215, 255, 0])) + # chrono_material = chrono.ChMaterialSurfaceNSC() + # chrono_material = chrono.ChMaterialSurfaceSMC() + # mesh = chrono.ChBodyEasyMesh("Body5.obj", 8000, True, True, True, chrono_material, 0.002) + # floor.body = mesh floor.body.SetNameString("Floor") floor.body.SetPos(chrono.ChVectorD(0.5,-0.07,0)) diff --git a/tests_jupyter/wheels.py b/tests_jupyter/wheels.py index 601e4662..956fd9fc 100644 --- a/tests_jupyter/wheels.py +++ b/tests_jupyter/wheels.py @@ -55,7 +55,7 @@ def create_rules(): 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)) + Box(0.5, 0.1, 0.3), material=def_mat, color=[255, 0, 0], density=100+i*100, is_collide=False)) wheel_body_cyl = PrimitiveBodyBlueprint( Cylinder(0.03, 0.02), material=def_mat, color=[0, 120, 255], density=500) @@ -148,6 +148,12 @@ def create_rules(): stiffness=x, damping=0.02, equilibrium_position=0), stiffness)) + + no_control_joint.append(RevolveJointBlueprint(JointInputType.UNCONTROL, + stiffness=10, + damping=0.02, + equilibrium_position=0)) + motor_bp = RevolveJointBlueprint(JointInputType.TORQUE, stiffness=0, damping=0.001) neutral_bp = RevolveJointBlueprint(JointInputType.UNCONTROL, stiffness=0, damping=0.001) @@ -201,7 +207,7 @@ def create_rules(): # %% def get_wheels(): - i=5 + i=9 graph = GraphGrammar() rules = ["Init", "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", @@ -214,7 +220,7 @@ def get_wheels(): "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' + 'Terminal_Main_Body1' ] @@ -223,3 +229,50 @@ def get_wheels(): graph.apply_rule(rule_vocabul.get_rule(rule)) return graph + +def get_stiff_wheels(): + i = 10 + 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_Body1' + ] + + rule_vocabul = create_rules() + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + return graph + +def get_stiff_wheels_ell(): + i = 10 + graph = GraphGrammar() + rules = ["Init", + "Add_Leg_Base","Positive_Translation_X4","Positive_Translation_Z3","Rotation", + 'Rotation_Y', "Extension","Extension","Rotation","R_Wheel", "Ell_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", "Ell_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_Body1' + ] + + rule_vocabul = create_rules() + for rule in rules: + graph.apply_rule(rule_vocabul.get_rule(rule)) + + return graph \ No newline at end of file diff --git a/tests_jupyter/wheels_sim.py b/tests_jupyter/wheels_sim.py index 3bb80150..eaadf8fc 100644 --- a/tests_jupyter/wheels_sim.py +++ b/tests_jupyter/wheels_sim.py @@ -1,12 +1,20 @@ +from rostok.control_chrono.controller import SimpleKeyBoardController from rostok.simulation_chrono.simulation_scenario import WalkingScenario -from wheels import get_wheels +from wheels import get_stiff_wheels, get_wheels, get_stiff_wheels_ell 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 = WalkingScenario(0.001, 10000, SimpleKeyBoardController) +graph = get_stiff_wheels_ell() -scenario.run_simulation(graph, control, starting_positions=[[45,-90,0], [-90,90,0], [90,-90,0]], vis = True, delay=True) +parameters = {} +parameters["forward"] = 0.5 +parameters["reverse"]= 0.5 +parameters["forward_rotate"] = 0.3 +parameters["reverse_rotate"] = 0.2 + + + +scenario.run_simulation(graph, parameters, starting_positions=[[45,-90,0], [-90,90,0], [90,-90,0]], vis = True, delay=True, is_follow_camera = False)