improvements, made pybullet work again

Author: Marc Bestmann

scripts/viz.py

@@ -2,7 +2,7 @@
 
 from optuna import Study, load_study, visualization, importance
 
-study = load_study('webots_test5',  storage='postgresql://bestmann:deepquintic@localhost/deep_quintic')
+study = load_study('pybullet3',  storage='postgresql://bestmann:deepquintic@localhost/deep_quintic')
 df = study.trials_dataframe()
 #print(df)
 print(df.keys())
@@ -42,6 +42,12 @@
 
 visualization.plot_slice(study).show()#, params=['freq', 'double_support_ratio', 'trunk_height'])
 
+visualization.plot_contour(study, params=['double_support_ratio', 'trunk_swing']).show()
+
+visualization.plot_optimization_history(study).show()
+
+visualization.plot_parallel_coordinate(study).show()
+
 #%%
 
 df_sorted

scripts/viz_study.ipynb

@@ -1,3 +1,7 @@
+# this has to be first import, otherwise there will be an error
+from bitbots_quintic_walk import PyWalk
+import cProfile as profile
+
 import argparse
 import importlib
 
@@ -25,7 +29,7 @@
 #    exit(1)
 
 seed = np.random.randint(2 ** 32 - 1)
-n_startup_trials = 1000
+n_startup_trials = 10
 
 sampler = TPESampler(n_startup_trials=n_startup_trials, seed=seed)
 #pruner = MedianPruner(n_startup_trials=n_startup_trials, n_warmup_steps=10)
@@ -34,5 +38,12 @@
                             sampler=sampler, load_if_exists=True)
 
 #objective = args.objective()
-objective = WolfgangWalkOptimization('worker', gui=True)
-study.optimize(objective.objective, n_trials=1000, show_progress_bar=True)
+pr = profile.Profile()
+pr.enable()
+try:
+    objective = WolfgangWalkOptimization('worker', gui=True, walk_as_node=False)
+    study.optimize(objective.objective, n_trials=1000, show_progress_bar=True)
+finally:
+    print("hi")
+    pr.disable()
+    pr.dump_stats('profile.pstat')

src/parallel_parameter_search/optimize.py

@@ -1,3 +1,4 @@
+# this has to be first import, otherwise there will be an error
 from bitbots_quintic_walk import PyWalk
 import argparse
 import importlib

src/parallel_parameter_search/optimize_webots.py

@@ -45,11 +45,11 @@ def __init__(self, namespace, gui):
         super(AbstractSim, self).__init__()
         self.namespace = namespace
         self.gui = gui
-        self.sim: PybulletSim = Simulation(gui)
+        self.sim: Simulation = Simulation(gui)
         self.sim_interface: ROSInterface = ROSInterface(self.sim, namespace=self.namespace + '/', node=False)
 
     def step_sim(self):
-        self.sim_interface.step()
+        self.sim.step()
 
     def set_gravity(self, on):
         self.sim.set_gravity(on)
@@ -63,6 +63,18 @@ def get_robot_pose_rpy(self):
     def reset(self):
         self.sim.reset()
 
+    def get_time(self):
+        return self.sim.time
+
+    def get_imu_msg(self):
+        return self.sim_interface.get_imu_msg()
+
+    def get_joint_state_msg(self):
+        return self.sim_interface.get_joint_state_msg()
+
+    def set_joints(self, joint_command_msg):
+        self.sim_interface.joint_goal_cb(joint_command_msg)
+
 
 class WebotsSim(AbstractSim):
 

src/parallel_parameter_search/simulators.py

@@ -57,6 +57,7 @@ def __init__(self, namespace, robot_name, walk_as_node):
     def objective(self, trial):
         # get parameter to evaluate from optuna
         self.suggest_walk_params(trial)
+        self.reset()
 
         cost = 0
         # starting with hardest first, to get faster early termination
@@ -70,7 +71,7 @@ def objective(self, trial):
             d = 0
             for direction in self.directions:
                 d += 1
-                self.reset()
+                self.reset_position()
                 early_term, cost_try = self.evaluate_direction(*direction, trial, eval_try)
                 cost += cost_try
                 # check if we failed in this direction and terminate this trial early
@@ -88,13 +89,13 @@ def suggest_walk_params(self, trial):
         raise NotImplementedError
 
     def evaluate_direction(self, x, y, yaw, trial: optuna.Trial, iteration):
-        start_time = rospy.get_time()
+        start_time = self.sim.get_time()
         self.set_cmd_vel(x * iteration, y * iteration, yaw * iteration)
         print(F'cmd: {x * iteration} {y * iteration} {yaw * iteration}')
 
         # wait till time for test is up or stopping condition has been reached
         while not rospy.is_shutdown():
-            passed_time = rospy.get_time() - start_time
+            passed_time = self.sim.get_time() - start_time
             if passed_time > self.time_limit:
                 # reached time limit, stop robot
                 self.set_cmd_vel(0, 0, 0)
@@ -176,6 +177,13 @@ def compute_cost(self, x, y, yaw):
             early_term = True
         return early_term, cost
 
+    def reset_position(self):
+        height = self.current_params['trunk_height'] + self.reset_height_offset
+        pitch = self.current_params['trunk_pitch']
+        (x, y, z, w) = tf.transformations.quaternion_from_euler(0, pitch, 0)
+
+        self.sim.reset_robot_pose((0, 0, height), (x, y, z, w))
+
     def reset(self):
         # reset simulation
         # self.sim.reset()
@@ -193,11 +201,7 @@ def reset(self):
         else:
             self.run_walking(duration=2)
         self.sim.set_gravity(True)
-        height = self.current_params['trunk_height'] + self.reset_height_offset
-        pitch = self.current_params['trunk_pitch']
-        (x, y, z, w) = tf.transformations.quaternion_from_euler(0, pitch, 0)
-
-        self.sim.reset_robot_pose((0, 0, height), (x, y, z, w))
+        self.reset_position()
         if self.walk_as_node:
             self.sim.run_simulation(duration=2, sleep=0.01)
         else:
@@ -215,8 +219,8 @@ def set_cmd_vel(self, x, y, yaw):
 
 
 class WolfgangWalkOptimization(AbstractWalkOptimization):
-    def __init__(self, namespace, gui):
-        super(WolfgangWalkOptimization, self).__init__(namespace, 'wolfgang')
+    def __init__(self, namespace, gui, walk_as_node):
+        super(WolfgangWalkOptimization, self).__init__(namespace, 'wolfgang', walk_as_node)
         self.reset_height_offset = 0.005
         self.directions = [[0.1, 0, 0],
                            [-0.1, 0, 0],
@@ -237,36 +241,59 @@ def suggest_walk_params(self, trial):
         def add(name, min_value, max_value):
             param_dict[name] = trial.suggest_uniform(name, min_value, max_value)
 
-        add('double_support_ratio', 0.0, 1.0)
-        add('freq', 0.1, 5)
-        add('foot_apex_phase', 0.0, 1.0)
+        add('double_support_ratio', 0.0, 0.45)
+        add('freq', 1.5, 3)
         add('foot_distance', 0.1, 0.3)
-        add('foot_rise', 0.01, 0.1)
-        add('foot_overshoot_phase', 0.0, 1.0)
-        add('foot_overshoot_ratio', 0.0, 1.0)
-        add('trunk_height', 0.25, 0.5)
+        add('trunk_height', 0.38, 0.45)
         add('trunk_phase', -0.5, 0.5)
-        add('trunk_pitch', -0.5, 0.5)
         add('trunk_swing', 0.0, 1.0)
-        add('trunk_x_offset', -0.05, 0.05)
-        add('trunk_y_offset', -0.05, 0.05)
-        add('first_step_swing_factor', 0.5, 2)
-        add('first_step_trunk_phase', -0.5, 0.5)
+        add('trunk_x_offset', -0.03, 0.03)
 
         add('trunk_x_offset_p_coef_forward', -1, 1)
         add('trunk_x_offset_p_coef_turn', -1, 1)
-        add('trunk_pitch_p_coef_forward', -5, 5)
-        add('trunk_pitch_p_coef_turn', -5, 5)
-        add('foot_z_pause', 0, 1)
-        add('foot_put_down_phase', 0, 1)
-        add('trunk_pause', 0, 1)
+
+        # add('first_step_swing_factor', 0.0, 2)
+        # add('first_step_trunk_phase', -0.5, 0.5)
+        param_dict['first_step_swing_factor'] = 1
+        param_dict['first_step_trunk_phase'] = -0.5
+
+        # add('foot_overshoot_phase', 0.0, 1.0)
+        # add('foot_overshoot_ratio', 0.0, 1.0)
+        param_dict['foot_overshoot_phase'] = 1
+        param_dict['foot_overshoot_ratio'] = 0.0
+
+        # add('trunk_y_offset', -0.03, 0.03)
+        # add('foot_rise', 0.04, 0.08)
+        # add('foot_apex_phase', 0.0, 1.0)
+        param_dict['trunk_y_offset'] = 0
+        param_dict['foot_rise'] = 0.1
+        param_dict['foot_apex_phase'] = 0.5
+        # todo put this as addition arguments to trial
+
+        # add('trunk_pitch', -1.0, 1.0)
+        # add('trunk_pitch_p_coef_forward', -5, 5)
+        # add('trunk_pitch_p_coef_turn', -5, 5)
+        param_dict['trunk_pitch'] = 0
+        param_dict['trunk_pitch_p_coef_forward'] = 0
+        param_dict['trunk_pitch_p_coef_turn'] = 0
+
+        # add('foot_z_pause', 0, 1)
+        # add('foot_put_down_phase', 0, 1)
+        # add('trunk_pause', 0, 1)
+        param_dict['foot_z_pause'] = 0
+        param_dict['foot_put_down_phase'] = 1
+        param_dict['trunk_pause'] = 0
 
         # todo 'trunk' nochmal ander nennen? body?
 
         # todo also find PID values, maybe in a second step after finding walking params
         # todo de/activate phase reset while searching params? yes
 
-        self.set_params(param_dict)
+        if self.walk_as_node:
+            self.set_params(param_dict)
+        else:
+            self.current_params = param_dict
+            self.walk.set_engine_dyn_reconf(param_dict)
 
 
 class DarwinWalkOptimization(AbstractWalkOptimization):
@@ -275,9 +302,13 @@ def __init__(self, namespace, gui, walk_as_node):
         self.reset_height_offset = 0.09
         self.directions = [[0.05, 0, 0],
                            [-0.05, 0, 0],
+                           [0, 0.025, 0],
+                           [0, -0.025, 0],
                            [0, 0, 0.25],
                            [0, 0, -0.25],
-                           [0.05, 0, 0.25],
+                           [0.05, 0.25, 0],
+                           [0.05, -0.25, 0],
+                           [0.05, 0, -0.25],
                            [-0.05, 0, 0.25],
                            ]
         self.sim = WebotsSim(self.namespace, gui)
@@ -288,31 +319,34 @@ def suggest_walk_params(self, trial):
         def add(name, min_value, max_value):
             param_dict[name] = trial.suggest_uniform(name, min_value, max_value)
 
-        # todo try without overshoot
-        # todo try without apex phase
-        # todo try without pitch values, only x offset
-        add('double_support_ratio', 0.0, 0.8)
-        add('freq', 0.5, 3)
+        add('double_support_ratio', 0.0, 0.5)
+        add('freq', 1.5, 3)
         add('foot_distance', 0.08, 0.17)
-        add('foot_overshoot_phase', 0.0, 1.0)
-        add('foot_overshoot_ratio', 0.0, 1.0)
         add('trunk_height', 0.18, 0.24)
         add('trunk_phase', -0.5, 0.5)
         add('trunk_swing', 0.0, 1.0)
         add('trunk_x_offset', -0.03, 0.03)
-        add('first_step_swing_factor', 0.0, 2)
-        add('first_step_trunk_phase', -0.5, 0.5)
 
         add('trunk_x_offset_p_coef_forward', -1, 1)
         add('trunk_x_offset_p_coef_turn', -1, 1)
 
+        #add('first_step_swing_factor', 0.0, 2)
+        #add('first_step_trunk_phase', -0.5, 0.5)
+        param_dict['first_step_swing_factor'] = 1
+        param_dict['first_step_trunk_phase'] = -0.5
+
+        #add('foot_overshoot_phase', 0.0, 1.0)
+        #add('foot_overshoot_ratio', 0.0, 1.0)
+        param_dict['foot_overshoot_phase'] = 1
+        param_dict['foot_overshoot_ratio'] = 0.0
+
         # add('trunk_y_offset', -0.03, 0.03)
         # add('foot_rise', 0.04, 0.08)
         # add('foot_apex_phase', 0.0, 1.0)
         param_dict['trunk_y_offset'] = 0
-        param_dict['foot_rise'] = 0.04
+        param_dict['foot_rise'] = 0.05
         param_dict['foot_apex_phase'] = 0.5
-        # todo put this as addition arguments to trail
+        # todo put this as addition arguments to trial
 
         # add('trunk_pitch', -1.0, 1.0)
         # add('trunk_pitch_p_coef_forward', -5, 5)