[Scripts] format changes according to reviews

src/hpp/corbaserver/rbprm/scenarios/abstract_contact_generator.py

@@ -128,10 +128,10 @@ class AbstractContactGenerator:
         :param set_ref_height: if True, set the root Z position of q_init and q_goal to be equal to q_ref
         """
         self.q_init = self.q_ref[::]
-        self.q_init[0:7] = self.path_planner.ps.configAtParam(self.pid, 0.001)[0:7]
+        self.q_init[:7] = self.path_planner.ps.configAtParam(self.pid, 0.001)[0:7]
         # do not use 0 but an epsilon in order to avoid the orientation discontinuity that may happen at t=0
         self.q_goal = self.q_init[::]
-        self.q_goal[0:7] = self.path_planner.ps.configAtParam(self.pid, self.path_planner.ps.pathLength(self.pid))[0:7]
+        self.q_goal[:7] = self.path_planner.ps.configAtParam(self.pid, self.path_planner.ps.pathLength(self.pid))[0:7]
 
         # copy extraconfig for start and init configurations
         configSize = self.fullbody.getConfigSize() - self.fullbody.client.robot.getDimensionExtraConfigSpace()
@@ -181,9 +181,7 @@ class AbstractContactGenerator:
         print("Contact plan generated in : " + str(t_interpolate_configs) + " s")
         print("number of configs :", len(self.configs))
 
-    """
-    Helper methods used to display results
-    """
+    # Helper methods used to display results
 
     def display_sequence(self, dt=0.5):
         """

src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py

@@ -2,8 +2,6 @@ from abc import abstractmethod
 from hpp.gepetto import ViewerFactory, PathPlayer
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 from hpp.corbaserver import ProblemSolver
-import time
-
 
 class AbstractPathPlanner:
 
@@ -33,8 +31,8 @@ class AbstractPathPlanner:
     @abstractmethod
     def load_rbprm(self):
         """
-    Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
-    """
+        Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
+        """
         pass
 
     def set_configurations(self):
@@ -45,35 +43,41 @@ class AbstractPathPlanner:
         self.q_goal[2] = self.rbprmBuilder.ref_height
 
     def compute_extra_config_bounds(self):
+        """
+        Compute extra dof bounds from the current values of v_max and a_max
+        By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
+        """
         # bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
-        self.extra_dof_bounds = [
-            -self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0, -self.a_max, self.a_max, -self.a_max, self.a_max,
-            0, 0
-        ]
+        self.extra_dof_bounds = [-self.v_max, self.v_max,
+                                 -self.v_max, self.v_max,
+                                 0, 0,
+                                 -self.a_max, self.a_max,
+                                 -self.a_max, self.a_max,
+                                 0, 0 ]
 
     def set_joints_bounds(self):
         """
-    Set the root translation and rotation bounds as well as the the extra dofs bounds
-    """
+        Set the root translation and rotation bounds as well as the the extra dofs bounds
+        """
         self.rbprmBuilder.setJointBounds("root_joint", self.root_translation_bounds)
         self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
         self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(self.extra_dof_bounds)
 
     def set_rom_filters(self):
         """
-    Define which ROM must be in collision at all time and with which kind of affordances
-    By default it set all the roms in used_limbs to be in contact with 'support' affordances
-    """
+        Define which ROM must be in collision at all time and with which kind of affordances
+        By default it set all the roms in used_limbs to be in contact with 'support' affordances
+        """
         self.rbprmBuilder.setFilter(self.used_limbs)
         for limb in self.used_limbs:
             self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
 
     def init_problem(self):
         """
-    Load the robot, set the bounds and the ROM filters and then
-    Create a ProblemSolver instance and set the default parameters.
-    The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
-    """
+        Load the robot, set the bounds and the ROM filters and then
+        Create a ProblemSolver instance and set the default parameters.
+        The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
+        """
         self.load_rbprm()
         self.set_configurations()
         self.compute_extra_config_bounds()
@@ -95,13 +99,13 @@ class AbstractPathPlanner:
 
     def init_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[]):
         """
-    Build an instance of hpp-gepetto-viewer from the current problemSolver
-    :param env_name: name of the urdf describing the environment
-    :param env_package: name of the package containing this urdf (default to hpp_environments)
-    :param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
-    (in order to avoid putting contacts closes to the edges of the surface)
-    :param visualize_affordances: list of affordances type to visualize, default to none
-    """
+        Build an instance of hpp-gepetto-viewer from the current problemSolver
+        :param env_name: name of the urdf describing the environment
+        :param env_package: name of the package containing this urdf (default to hpp_environments)
+        :param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
+        (in order to avoid putting contacts closes to the edges of the surface)
+        :param visualize_affordances: list of affordances type to visualize, default to none
+        """
         vf = ViewerFactory(self.ps)
         self.afftool = AffordanceTool()
         self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
@@ -131,10 +135,10 @@ class AbstractPathPlanner:
 
     def init_planner(self, kinodynamic=True, optimize=True):
         """
-    Select the rbprm methods, and the kinodynamic ones if required
-    :param kinodynamic: if True, also select the kinodynamic methods
-    :param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
-    """
+        Select the rbprm methods, and the kinodynamic ones if required
+        :param kinodynamic: if True, also select the kinodynamic methods
+        :param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
+        """
         self.ps.selectConfigurationShooter("RbprmShooter")
         self.ps.selectPathValidation("RbprmPathValidation", 0.05)
         if kinodynamic:
@@ -149,9 +153,9 @@ class AbstractPathPlanner:
 
     def solve(self):
         """
-    Solve the path planning problem.
-    q_init and q_goal must have been defined before calling this method
-    """
+        Solve the path planning problem.
+        q_init and q_goal must have been defined before calling this method
+        """
         if len(self.q_init) != self.rbprmBuilder.getConfigSize():
             raise ValueError("Initial configuration vector do not have the right size")
         if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
@@ -162,18 +166,13 @@ class AbstractPathPlanner:
         t = self.ps.solve()
         print("Guide planning time : ", t)
 
-    def run(self):
-        self.init_problem()
-        self.init_viewer()
-        self.init_planner()
-        self.solve()
 
     def display_path(self, path_id=-1, dt=0.1):
         """
-    Display the path in the viewer, if no path specified display the last one
-    :param path_id: the Id of the path specified, default to the most recent one
-    :param dt: discretization step used to display the path (default to 0.1)
-    """
+        Display the path in the viewer, if no path specified display the last one
+        :param path_id: the Id of the path specified, default to the most recent one
+        :param dt: discretization step used to display the path (default to 0.1)
+        """
         if self.pp is not None:
             if path_id < 0:
                 path_id = self.ps.numberPaths() - 1
@@ -182,10 +181,10 @@ class AbstractPathPlanner:
 
     def play_path(self, path_id=-1, dt=0.01):
         """
-    play the path in the viewer, if no path specified display the last one
-    :param path_id: the Id of the path specified, default to the most recent one
-    :param dt: discretization step used to display the path (default to 0.01)
-    """
+        play the path in the viewer, if no path specified display the last one
+        :param path_id: the Id of the path specified, default to the most recent one
+        :param dt: discretization step used to display the path (default to 0.01)
+        """
         self.show_rom()
         if self.pp is not None:
             if path_id < 0:
@@ -195,32 +194,32 @@ class AbstractPathPlanner:
 
     def hide_rom(self):
         """
-    Remove the current robot from the display
-    """
+        Remove the current robot from the display
+        """
         self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
 
     def show_rom(self):
         """
-    Add the current robot to the display
-    """
+        Add the current robot to the display
+        """
         self.v.client.gui.setVisibility(self.robot_node_name, "ON")
 
     @abstractmethod
     def run(self):
         """
-    Must be defined in the child class to run all the methods with the correct arguments.
-    """
+        Must be defined in the child class to run all the methods with the correct arguments.
+        """
         # example of definition:
         """
-    self.init_problem()
-    # define initial and goal position
-    self.q_init[0:2] = [0, 0]
-    self.q_goal[0:2] = [1, 0]
-    
-    self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
-    self.init_planner()
-    self.solve()
-    self.display_path()
-    self.play_path()
-    """
+        self.init_problem()
+        # define initial and goal position
+        self.q_init[:2] = [0, 0]
+        self.q_goal[:2] = [1, 0]
+        
+        self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
+        self.init_planner()
+        self.solve()
+        self.display_path()
+        self.play_path()
+        """
         pass

src/hpp/corbaserver/rbprm/scenarios/demos/hrp2_flatGround_path.py

@@ -5,7 +5,7 @@ class PathPlanner(HRP2PathPlanner):
     def run(self):
         self.init_problem()
 
-        self.q_init[0:2] = [0, 0]
+        self.q_init[:2] = [0, 0]
         self.q_goal[0] = 1.5
         self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
         self.init_planner()

src/hpp/corbaserver/rbprm/scenarios/demos/hrp2_plateformes_path.py

@@ -12,8 +12,8 @@ class PathPlanner(HRP2PathPlanner):
         self.v_max = 0.3
         self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.70, 0.8]
         self.init_problem()
-        self.q_init[0:3] = [0.12, 0.25, 0.75]
-        self.q_goal[0:3] = [1.08, 0.25, 0.75]
+        self.q_init[:3] = [0.12, 0.25, 0.75]
+        self.q_goal[:3] = [1.08, 0.25, 0.75]
         self.init_viewer("multicontact/plateforme_surfaces",
                          reduce_sizes=[0.1, 0, 0],
                          visualize_affordances=["Support"])

src/hpp/corbaserver/rbprm/scenarios/demos/hyq_darpa_path.py

@@ -11,8 +11,8 @@ class PathPlanner(HyqPathPlanner):
         self.root_rotation_bounds = [-0.4, 0.4, -0.3, 0.3, -0.3, 0.3]
         self.init_problem()
 
-        self.q_init[0:2] = [-2, 0]
-        self.q_goal[0:2] = [3, 0]
+        self.q_init[:2] = [-2, 0]
+        self.q_goal[:2] = [3, 0]
         self.init_viewer("multicontact/darpa", visualize_affordances=["Support"], reduce_sizes=[0.06, 0, 0])
         self.init_planner(kinodynamic=False)
         self.solve()

src/hpp/corbaserver/rbprm/scenarios/demos/hyq_slalom_debris_path.py

@@ -13,9 +13,9 @@ class PathPlanner(HyqPathPlanner):
         self.root_translation_bounds = [-1.7, 2.5, -0.2, 2, 0.64, 0.64]
         self.init_problem()
 
-        self.q_init[0:2] = [-1.5, 0]
+        self.q_init[:2] = [-1.5, 0]
         self.q_init[-6] = 0.05
-        self.q_goal[0:2] = [2.2, 0]
+        self.q_goal[:2] = [2.2, 0]
         self.q_goal[-6] = 0.05
 
         self.init_viewer("multicontact/slalom_debris")

src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround_path.py

@@ -5,14 +5,14 @@ class PathPlanner(TalosPathPlanner):
     def run(self):
         self.init_problem()
 
-        self.q_init[0:2] = [0, 0]
-        self.q_goal[0:2] = [1, 0]
+        self.q_init[:2] = [0, 0]
+        self.q_goal[:2] = [1, 0]
 
         self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
         self.init_planner()
         self.solve()
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard_path.py

@@ -21,14 +21,14 @@ class PathPlanner(TalosPathPlanner):
         ]
         self.set_joints_bounds()
 
-        self.q_init[0:2] = [-0.7, 2]
-        self.q_goal[0:2] = [0, -1]
+        self.q_init[:2] = [-0.7, 2]
+        self.q_goal[:2] = [0, -1]
 
         self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
         self.init_planner()
         self.solve()
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_path.py

@@ -19,17 +19,17 @@ class PathPlanner(TalosPathPlanner):
         self.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
         self.set_joints_bounds()
 
-        self.q_init[0:2] = [-0.9, 1.7]
+        self.q_init[:2] = [-0.9, 1.7]
         # Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
         self.q_init[-6:-3] = [0.07, 0, 0]
-        self.q_goal[0:2] = [3.6, 1.2]
+        self.q_goal[:2] = [3.6, 1.2]
         self.q_goal[-6:-3] = [0, -0.1, 0]
 
         self.init_viewer("multicontact/floor_bauzil", visualize_affordances=["Support"])
         self.init_planner()
         self.solve()
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes_path.py

@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
         self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3]
         self.init_problem()
 
-        self.q_init[0:3] = [0.16, 0.25, 1.14]
-        self.q_goal[0:3] = [1.09, 0.25, 1.14]
+        self.q_init[:3] = [0.16, 0.25, 1.14]
+        self.q_goal[:3] = [1.09, 0.25, 1.14]
 
         self.init_viewer("multicontact/plateforme_surfaces",
                          reduce_sizes=[0.18, 0, 0],
@@ -21,7 +21,7 @@ class PathPlanner(TalosPathPlanner):
         self.init_planner(kinodynamic=False)
         self.solve()
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs10_path.py

@@ -31,14 +31,14 @@ class PathPlanner(TalosPathPlanner):
         self.root_translation_bounds = [-1.5, 3, 0., 3.3, 0.95, 2.]
         self.init_problem()
 
-        self.q_init[0:3] = [0.05, 1.2, 0.98]
-        self.q_goal[0:3] = [1.87, 1.2, 1.58]
+        self.q_init[:3] = [0.05, 1.2, 0.98]
+        self.q_goal[:3] = [1.87, 1.2, 1.58]
 
         self.init_viewer("multicontact/bauzil_stairs", reduce_sizes=[0.11, 0., 0.], visualize_affordances=["Support"])
         self.init_planner()
         self.solve()
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_circle_oriented_path.py

 from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
 from pinocchio import Quaternion
 import numpy as np
+import random
+import sys
 
 
 class PathPlanner(AnymalPathPlanner):
@@ -23,17 +25,16 @@ class PathPlanner(AnymalPathPlanner):
         """
         randomly sample initial and goal configuration :
         """
-        self.q_init[0:3] = [0, 0, 0.465]
+        self.q_init[:3] = [0, 0, 0.465]
         self.q_init[3:7] = [0, 0, 0, 1]
         self.q_init[-6] = self.v_init
 
-        import random
         random.seed()
         alpha = random.uniform(0., 2. * np.pi)
         #alpha = 4.
         print("Test on a circle, alpha = ", alpha)
         self.q_goal = self.q_init[::]
-        self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
+        self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
         # set final orientation to be along the circle :
         vx = np.matrix([1, 0, 0]).T
         v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
@@ -62,7 +63,6 @@ class PathPlanner(AnymalPathPlanner):
         success = self.ps.client.problem.prepareSolveStepByStep()
         if not success:
             print("planning failed.")
-            import sys
             sys.exit(1)
         self.ps.client.problem.finishSolveStepByStep()
         self.display_path()

src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_circle_path.py

 from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
 from pinocchio import Quaternion
 import numpy as np
+import random
+import sys
 
 
 class PathPlanner(AnymalPathPlanner):
@@ -17,18 +19,17 @@ class PathPlanner(AnymalPathPlanner):
         """
         randomly sample initial and goal configuration :
         """
-        self.q_init[0:3] = [0, 0, 0.465]
+        self.q_init[:3] = [0, 0, 0.465]
         self.q_init[3:7] = [0, 0, 0, 1]
 
-        import random
         random.seed()
         alpha = random.uniform(0., 2. * np.pi)
         print("Test on a circle, alpha = ", alpha)
         self.q_goal = self.q_init[::]
-        self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
+        self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465]
         self.v(self.q_init)
-        print("initial root position : ", self.q_init[0:3])
-        print("final root position : ", self.q_goal[0:3])
+        print("initial root position : ", self.q_init[:3])
+        print("final root position : ", self.q_goal[:3])
         self.ps.setInitialConfig(self.q_init)
         self.ps.addGoalConfig(self.q_goal)
 
@@ -46,7 +47,6 @@ class PathPlanner(AnymalPathPlanner):
         success = self.ps.client.problem.prepareSolveStepByStep()
         if not success:
             print("planning failed.")
-            import sys
             sys.exit(1)
         self.ps.client.problem.finishSolveStepByStep()
         self.display_path()

src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_contact_generator.py

 from hpp.corbaserver.rbprm.scenarios.anymal_contact_generator import AnymalContactGenerator as Parent
+import sys
 
 
 class AnymalContactGenerator(Parent):
@@ -49,8 +50,6 @@ class AnymalContactGenerator(Parent):
         f.write("cg_too_many_states: " + str(cg_too_many_states) + "\n")
         f.close()
         if (not cg_success) or cg_too_many_states:
-            import sys
             sys.exit(1)
         if throw_if_goal_not_reached and not cg_reach_goal:
-            import sys
             sys.exit(1)

src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_platform_random_path.py

 from hpp.corbaserver.rbprm.scenarios.anymal_path_planner import AnymalPathPlanner
 from pinocchio import Quaternion
 import numpy as np
+import random
+import sys
 
 
 class PathPlanner(AnymalPathPlanner):
@@ -32,16 +34,15 @@ class PathPlanner(AnymalPathPlanner):
         """
         randomly sample initial and goal configuration :
         """
-        import random
         random.seed()
-        self.q_init[0:3] = [
+        self.q_init[:3] = [
             random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
             random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
         ]
         self.q_goal = self.q_init[::]
         for i in range(random.randint(0, 1000)):
             random.uniform(0., 1.)
-        self.q_goal[0:3] = [
+        self.q_goal[:3] = [
             random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]),
             random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE
         ]
@@ -54,8 +55,8 @@ class PathPlanner(AnymalPathPlanner):
         self.q_init[3:7] = quat.coeffs().tolist()
         self.q_goal[3:7] = self.q_init[3:7]
         self.v(self.q_init)
-        print("initial root position : ", self.q_init[0:3])
-        print("final root position : ", self.q_goal[0:3])
+        print("initial root position : ", self.q_init[:3])
+        print("final root position : ", self.q_goal[:3])
         self.ps.setInitialConfig(self.q_init)
         self.ps.addGoalConfig(self.q_goal)
 
@@ -73,7 +74,6 @@ class PathPlanner(AnymalPathPlanner):
         success = self.ps.client.problem.prepareSolveStepByStep()
         if not success:
             print("planning failed.")
-            import sys
             sys.exit(1)
         self.ps.client.problem.finishSolveStepByStep()
         self.display_path()

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_bauzil_with_stairs_path.py

 from hpp.corbaserver.rbprm.tools.sample_root_config import generate_random_conf_without_orientation
 from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
 from talos_rbprm.talos_abstract import Robot
+import random
 
 
 class PathPlanner(TalosPathPlanner):
@@ -50,7 +51,6 @@ class PathPlanner(TalosPathPlanner):
         """
         randomly sample initial and goal configuration :
         """
-        import random
         random.seed()
         # sample an initial and goal zone : either floor, right platform or left platform. With more weight on the floor
         # 0,1,2 are floor, 3 is platform10 4 is platform15
@@ -123,7 +123,7 @@ class PathPlanner(TalosPathPlanner):
         self.q_init = self.ps.configAtParam(pathId, 0)
         self.q_goal = self.ps.configAtParam(pathId, self.ps.pathLength(pathId))
         self.display_path()
-        #self.play_path()
+        # self.play_path()
         self.hide_rom()
 
 

src/hpp/corbaserver/rbprm/scenarios/memmo/talos_circle_oriented_path.py

 from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
 from pinocchio import Quaternion
 import numpy as np
+import random
+import sys
 
 
 class PathPlanner(TalosPathPlanner):
@@ -22,17 +24,16 @@ class PathPlanner(TalosPathPlanner):
         """
         randomly sample initial and goal configuration :
         """
-        self.q_init[0:3] = [0, 0, 1.]
+        self.q_init[:3] = [0, 0, 1.]
         self.q_init[3:7] = [0, 0, 0, 1]
         self.q_init[-6] = self.v_init
 
-        import random
         random.seed()
         alpha = random.uniform(0., 2. * np.pi)
         #alpha = 4.
         print("Test on a circle, alpha = ", alpha)
         self.q_goal = self.q_init[::]
-        self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
+        self.q_goal[:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
         # set final orientation to be along the circle :
         vx = np.matrix([1, 0, 0]).T
         v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
@@ -61,12 +62,11 @@ class PathPlanner(TalosPathPlanner):
         success = self.ps.client.problem.prepareSolveStepByStep()
         if not success:
             print("planning failed.")
-            import sys