Industrial/grab pipe one robot

app/app.py

@@ -1,19 +1,18 @@
-import rule_extention
-import mcts
+from pathlib import Path
+
 import matplotlib.pyplot as plt
+import mcts
 # imports from standard libs
 import networkx as nx
-
 # chrono imports
 import pychrono as chrono
+import rule_extention
+from control_optimisation import (create_grab_criterion_fun, create_traj_fun, get_object_to_grasp)
 
-from control_optimisation import create_grab_criterion_fun, create_traj_fun, get_object_to_grasp
+from rostok.graph_generators.mcts_helper import prepare_mcts_state_and_helper, make_mcts_step
+from rostok.criterion.flags_simualtions import (FlagMaxTime, FlagNotContact, FlagSlipout)
 from rostok.graph_grammar.node import GraphGrammar
-from rostok.trajectory_optimizer.control_optimizer import ConfigRewardFunction, ControlOptimizer
-from rostok.criterion.flags_simualtions import FlagMaxTime, FlagSlipout, FlagNotContact
-from rostok.utils.result_saver  import read_report
-
-import rostok.graph_generators.graph_environment as env
+from rostok.trajectory_optimizer.control_optimizer import (ConfigRewardFunction, ControlOptimizer)
 
 
 def plot_graph(graph: GraphGrammar):
@@ -22,15 +21,13 @@ def plot_graph(graph: GraphGrammar):
                      pos=nx.kamada_kawai_layout(graph, dim=2),
                      node_size=800,
                      labels={n: graph.nodes[n]["Node"].label for n in graph})
-    plt.savefig("./results/graph.jpg")
     plt.show()
 
 
 # %% Create extension rule vocabulary
-
-# %% Create extension rule vocabulary
-
-rule_vocabul, node_features = rule_extention.init_extension_rules()
+width = [0.25, 0.35, 0.5]
+length_link = [0.4, 0.6, 0.8]
+rule_vocabul, node_features = rule_extention.init_extension_rules(length_link, width)
 
 # %% Create condig optimizing control
 
@@ -54,37 +51,38 @@ def plot_graph(graph: GraphGrammar):
 cfg.params_to_timesiries_callback = traj_generator_fun
 
 control_optimizer = ControlOptimizer(cfg)
+control_optimizer.is_visualize = False
 
 # %% Init mcts parameters
 
 # Hyperparameters mctss
-iteration_limit = 20
+iteration_limit = 2
 
 # Initialize MCTS
 searcher = mcts.mcts(iterationLimit=iteration_limit)
 finish = False
 
 G = GraphGrammar()
-max_numbers_rules = 10
-# Create graph envirenments for algorithm (not gym)
-graph_env = env.GraphVocabularyEnvironment(G, rule_vocabul, max_numbers_rules)
-
-graph_env.set_control_optimizer(control_optimizer)
-
+max_numbers_rules = 2
+# Create graph environments for algorithm (not gym)
+graph_env = prepare_mcts_state_and_helper(G, rule_vocabul, control_optimizer, max_numbers_rules, Path("./results"))
+mcts_helper = graph_env.helper
+n_steps = 0
 #%% Run first algorithm
-iter = 0
 while not finish:
-    action = searcher.search(initialState=graph_env)
-    finish, final_graph, opt_trajectory, path = graph_env.step(action, False)
-    iter += 1
-    print(
-        f"number iteration: {iter}, counter actions: {graph_env.counter_action}, reward: {graph_env.reward}"
-    )
-
-
-best_graph, best_control, reward = read_report(path, rule_vocabul)
-best_control = [float(x) for x in best_control]
+    finish, graph_env = make_mcts_step(searcher, graph_env, n_steps)
+    n_steps += 1
+    print(f"number iteration: {n_steps}, counter actions: {graph_env.counter_action} " +
+          f"reward: {mcts_helper.report.get_best_info()[1]}")
+
+report = mcts_helper.report
+report.draw_best_graph()
+best_graph, reward, best_control = mcts_helper.report.get_best_info()
 func_reward = control_optimizer.create_reward_function(best_graph)
-res = -func_reward(best_control, True)
-plot_graph(best_graph)
-print(res)
+res = -func_reward(best_control)
+print(res)
+report.plot_means()
+report.make_time_dependent_path()
+report.save()
+report.save_visuals()
+report.save_lists()

app/control_optimisation.py

@@ -1,23 +1,21 @@
 import rostok.criterion.criterion_calc as criterion
 from rostok.block_builder.envbody_shapes import Box
-from rostok.block_builder.node_render import (ChronoBodyEnv,
-                                              DefaultChronoMaterial,
-                                              FrameTransform)
+from rostok.block_builder.node_render import (ChronoBodyEnv, DefaultChronoMaterial, FrameTransform)
 from rostok.graph_grammar.node import BlockWrapper, GraphGrammar, Node
 from rostok.trajectory_optimizer.trajectory_generator import \
     create_torque_traj_from_x
 from rostok.virtual_experiment.simulation_step import SimOut
+from rostok import intexp
 
 
 def get_object_to_grasp():
     matich = DefaultChronoMaterial()
     matich.Friction = 0.65
-    matich.DampingF = 0.65
     shape_box = Box(0.1, 0.1, 0.1)
     obj = BlockWrapper(ChronoBodyEnv,
                        shape=shape_box,
                        material=matich,
-                       pos=FrameTransform([0, 0.5, 0], [0, -0.048, 0.706, 0.706]))
+                       pos=FrameTransform([0, 1.5, 0], [0, -0.048, 0.706, 0.706]))
 
     return obj
 

app/industial_example_pickup_pipe/generation_pickuper_pipe.py

@@ -0,0 +1,86 @@
+from pathlib import Path
+import mcts
+
+# chrono imports
+import pychrono as chrono
+
+from app.control_optimisation import create_grab_criterion_fun, create_traj_fun
+from rostok.graph_generators.mcts_helper import make_mcts_step, prepare_mcts_state_and_helper
+from rostok.graph_grammar.node import GraphGrammar
+from rostok.trajectory_optimizer.control_optimizer import ConfigRewardFunction, ControlOptimizer
+from rostok.criterion.flags_simualtions import (FlagMaxTime, FlagSlipout, FlagNotContact,
+                                                FlagFlyingApart)
+
+import rule_grasp_pipe
+import pickup_pipes_utils as ppu
+
+PATH_TO_PIPE_OBJ = './examples/models/custom/pipe_mul_10.obj'
+PATH_TO_PIPE_XML = './examples/models/custom/pipe.xml'
+
+# # %% Create extension rule vocabulary
+
+rule_vocabul, node_features = rule_grasp_pipe.create_rules_to_pickup_pipe_ver_2(
+    PATH_TO_PIPE_OBJ, PATH_TO_PIPE_XML)
+
+# # %% Create condig optimizing control
+
+GAIT = 2.5
+WEIGHT = [3, 1, 1, 2]
+
+max_time = 1
+cfg = ConfigRewardFunction()
+cfg.bound = (800, 1000)
+cfg.iters = 2
+cfg.sim_config = {"Set_G_acc": chrono.ChVectorD(0, 0, 0)}
+cfg.time_step = 0.001
+cfg.time_sim = max_time
+cfg.flags = [FlagMaxTime(max_time),
+            FlagNotContact(max_time / 2 - 0.2),
+            FlagSlipout(max_time / 2 + 0.2, 0.2),
+            FlagFlyingApart(20)]
+
+criterion_callback = create_grab_criterion_fun(node_features, GAIT, WEIGHT)
+traj_generator_fun = create_traj_fun(cfg.time_sim, cfg.time_step)
+
+cfg.criterion_callback = criterion_callback
+cfg.get_rgab_object_callback = ppu.create_builder_grab_object(PATH_TO_PIPE_OBJ, PATH_TO_PIPE_XML)
+cfg.params_to_timesiries_callback = traj_generator_fun
+
+control_optimizer = ControlOptimizer(cfg)
+control_optimizer.is_visualize = False
+
+# # %% Init mcts parameters
+if __name__ == "__main__":
+    # Hyperparameters mctss
+    iteration_limit = 4
+
+    # Initialize MCTScl
+    searcher = mcts.mcts(iterationLimit=iteration_limit)
+    finish = False
+
+    G = GraphGrammar()
+    max_numbers_rules = 11
+
+    # Create graph environments for algorithm (not gym)
+    graph_env = prepare_mcts_state_and_helper(G, rule_vocabul, control_optimizer, max_numbers_rules,
+                                            Path("./results"))
+    mcts_helper = graph_env.helper
+    n_steps = 0
+    #%% Run first algorithm
+    while not finish:
+        finish, graph_env = make_mcts_step(searcher, graph_env, n_steps)
+        n_steps += 1
+        print(f"number iteration: {n_steps}, counter actions: {graph_env.counter_action} " +
+            f"reward: {mcts_helper.report.get_best_info()[1]}")
+
+    # %% Save results
+
+    report = mcts_helper.report
+    best_graph, reward, best_control = mcts_helper.report.get_best_info()
+    func_reward = control_optimizer.create_reward_function(best_graph)
+    res = -func_reward(best_control)
+    report.plot_means()
+    report.make_time_dependent_path()
+    report.save()
+    report.save_visuals()
+    report.save_lists()

app/industial_example_pickup_pipe/pickup_pipes_utils.py

@@ -0,0 +1,75 @@
+import numpy as np
+import pychrono as chrono
+
+import rostok.intexp as intexp
+from rostok.graph_grammar.node import BlockWrapper
+from rostok.block_builder.node_render import ChronoBodyEnv
+from rostok.block_builder.transform_srtucture import FrameTransform
+from rostok.block_builder.envbody_shapes import LoadedShape
+
+
+def get_main_axis_pipe(obj_db):
+
+    longest_axis_size = np.argmax(obj_db.bound_box)
+    shorthest_axis_size = np.argmin(obj_db.bound_box)
+
+    length_longest_dimension = obj_db.bound_box[longest_axis_size]
+    length_shorthest_dimension = obj_db.bound_box[shorthest_axis_size]
+
+
+    axis = {
+        0: ('x', np.array([1, 0, 0])),
+        1: ('y', np.array([0, 1, 0])),
+        2: ('z', np.array([0, 0, 1]))
+    }
+    return (longest_axis_size,length_longest_dimension), (shorthest_axis_size,length_shorthest_dimension), axis
+
+def create_builder_grab_object(path_to_pipe_obj = None,
+                                path_to_pipe_xml = None):
+
+    def get_pipe_object_n_pose():
+
+        # Create 3D mesh and setup parameters from files
+
+        obj = BlockWrapper(ChronoBodyEnv, LoadedShape(path_to_pipe_obj, path_to_pipe_xml))
+
+        obj_db = intexp.entity.TesteeObject()
+        obj_db.load_object_mesh(path_to_pipe_obj)
+        obj_db.load_object_description(path_to_pipe_xml)
+
+        long_dimension, short_dimension, axis = get_main_axis_pipe(obj_db)
+        gen_poses = intexp.poses_generator.gen_cylindrical_surface_around_object_axis(
+            obj_db, 1, short_dimension[1]*1.5,
+            long_dimension[1] / 2, axis[long_dimension[0]][0])
+
+        grab_pos = get_robot_poses_to_grasp_pipe(path_to_pipe_obj, path_to_pipe_xml)
+
+        obj_db.clear_grasping_poses_list()
+        obj_db.add_grasping_pose(grab_pos[0], [0, 0, 0, 1])
+        obj_db.add_grasping_pose(grab_pos[1], [0, 0, 0, 1])
+
+        center_pipe = FrameTransform(gen_poses[1][0],[0, 1, 0, 0])
+        return obj, center_pipe
+
+    return get_pipe_object_n_pose
+
+
+def get_robot_poses_to_grasp_pipe(path_to_pipe_obj=None,
+                                    path_to_pipe_xml=None):
+
+    obj_db = intexp.chrono_api.ChTesteeObject()
+    obj_db.create_chrono_body_from_file(path_to_pipe_obj, path_to_pipe_xml)
+    long_dim, short_dim, axis =  get_main_axis_pipe(obj_db)
+
+    gen_poses = intexp.poses_generator.gen_cylindrical_surface_around_object_axis(
+        obj_db, 1, short_dim[1] * 1.5,
+        long_dim[1]/2, axis[long_dim[0]][0])
+
+    grab_pos_1 = gen_poses[1][0] + long_dim[1] * axis[long_dim[0]][1] * 0.25
+
+    grab_pos_2 = gen_poses[1][0] - long_dim[1] * axis[long_dim[0]][1] * 0.25
+
+    grab_frame_1 = FrameTransform(grab_pos_1, [0, 1, 0, 0])
+    grab_frame_2 = FrameTransform(grab_pos_2, [0, 1, 0, 0])
+    grab_poses = [grab_frame_1, grab_frame_2]
+    return grab_poses