123

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)
 
 

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()

rostok/simulation_chrono/simulation_scenario.py

@@ -163,29 +163,30 @@ 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
 
         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))
 

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

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)