[Scripts] add template for contact generation

script/scenarios/abstract_contact_generator.py

+from hpp.gepetto import Viewer
+from hpp.corbaserver.rbprm.tools.display_tools import displayContactSequence
+from hpp.corbaserver import ProblemSolver
+import time
+from abc import abstractmethod
+
+class AbstractContactGenerator:
+
+    fullbody = None
+    ps = None
+    v = None
+    q_ref = None
+    weight_postural = None
+    q_init = None
+    q_goal = None
+
+    def __init__(self, path_planner):
+        """
+        Constructor, run the guide path and save the results
+        :param path_planner: an instance of a child class of AbstractPathPlanner
+        """
+        path_planner.run()
+        self.path_planner = path_planner
+        self.pid = self.path_planner.ps.numberPaths() - 1
+        self.used_limbs = path_planner.used_limbs
+        self.dt = 0.01
+        self.robustness = 0
+        self.filter_states = True
+        self.test_reachability = True
+        self.quasi_static = True
+        self.erase_previous_states = True
+        self.init_contacts = self.used_limbs
+        self.end_contacts = self.used_limbs
+        self.configs = []
+
+    @abstractmethod
+    def load_fullbody(self):
+        pass
+
+    def set_joints_bounds(self):
+        self.fullbody.setJointBounds ("root_joint", self.path_planner.root_translation_bounds)
+        self.fullbody.client.robot.setDimensionExtraConfigSpace(self.path_planner.extra_dof)
+        self.fullbody.client.robot.setExtraConfigSpaceBounds(self.path_planner.extra_dof_bounds)
+
+    def set_reference(self, use_postural_task = True):
+        self.fullbody.setReferenceConfig(self.q_ref)
+        self.fullbody.setCurrentConfig(self.q_ref)
+        self.fullbody.setPostureWeights(self.weight_postural)
+        self.fullbody.usePosturalTaskContactCreation(use_postural_task)
+
+    def load_limbs(self, heuristic = "fixedStep06", analysis=None, nb_samples=None, octree_size=None):
+        self.fullbody.limbs_names = self.used_limbs
+        if nb_samples is None:
+            nb_samples = self.fullbody.nbSamples
+        if octree_size is None:
+            octree_size = self.fullbody.octreeSize
+        print("Generate limb DB ...")
+        t_start = time.time()
+        self.fullbody.loadAllLimbs(heuristic, analysis, nb_samples, octree_size)
+        t_generate = time.time() - t_start
+        print("Databases generated in : " + str(t_generate) + " s")
+
+
+    def init_problem(self):
+        self.ps = ProblemSolver(self.fullbody)
+        if self.path_planner.v_max >= 0:
+            self.ps.setParameter("Kinodynamic/velocityBound", self.path_planner.v_max)
+        if self.path_planner.a_max >= 0:
+            self.ps.setParameter("Kinodynamic/accelerationBound", self.path_planner.a_max)
+
+    def init_viewer(self):
+        self.v = Viewer(self.ps, viewerClient=self.path_planner.v.client, displayCoM=True)
+
+    def compute_configs_from_guide(self, use_acc_init = True, use_acc_end = False):
+        self.q_init = self.q_ref[::]
+        self.q_init[0: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]
+
+        # copy extraconfig for start and init configurations
+        configSize = self.fullbody.getConfigSize() - self.fullbody.client.robot.getDimensionExtraConfigSpace()
+        q_init_guide = self.path_planner.ps.configAtParam(self.pid, 0)
+        q_goal_guide = self.path_planner.ps.configAtParam(self.pid, self.path_planner.ps.pathLength(self.pid))
+        index_ecs = len(q_init_guide) - self.path_planner.extra_dof
+        self.q_init[configSize : configSize + 3] = q_init_guide[index_ecs : index_ecs + 3]
+        if use_acc_init:
+            self.q_init[configSize + 3 : configSize + 6] = q_init_guide[index_ecs + 3 : index_ecs + 6]
+        else:
+            self.q_init[configSize + 3 : configSize + 6] = [0, 0, 0]
+        self.q_goal[configSize : configSize + 3] = q_goal_guide[index_ecs : index_ecs + 3]
+        if use_acc_end:
+            self.q_goal[configSize + 3 : configSize + 6] = q_goal_guide[index_ecs + 3 : index_ecs + 6]
+        else:
+            self.q_goal[configSize + 3 : configSize + 6] = [0, 0, 0]
+
+    def interpolate(self):
+        # specify the full body configurations as start and goal state of the problem
+        self.fullbody.setStartState(self.q_init, self.init_contacts)
+        self.fullbody.setEndState(self.q_goal, self.end_contacts)
+
+        print("Generate contact plan ...")
+        t_start = time.time()
+        self.configs = self.fullbody.interpolate(0.01,
+                                            pathId=self.pid,
+                                            robustnessTreshold=self.robustness,
+                                            filterStates=self.filter_states,
+                                            testReachability=self.test_reachability,
+                                            quasiStatic=self.quasi_static,
+                                            erasePreviousStates=self.erase_previous_states)
+        t_interpolate_configs = time.time() - t_start
+        print("Contact plan generated in : " + str(t_interpolate_configs) + " s")
+        print("number of configs :", len(self.configs))
+
+    def display_sequence(self):
+        displayContactSequence(self.v, self.configs)
+
+    @abstractmethod
+    def run(self):
+        """
+        Must be defined in the child class to run all the methods with the correct arguments.
+        """
+        # example of definition:
+        """
+        self.load_fullbody()
+        self.set_joints_bounds()
+        self.set_reference(True)
+        self.load_limbs("fixedStep06")
+        self.init_problem()
+        self.init_viewer()
+        self.compute_configs_from_guide()
+        # force root height to be at the reference position:
+        self.q_init[2] = self.q_ref[2]
+        self.q_goal[2] = self.q_ref[2]
+        self.interpolate()
+        """
+        pass

script/scenarios/abstract_path_planner.py

+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:
+
+  rbprmBuilder = None
+  ps = None
+  v = None
+  afftool = None
+  pp = None
+
+  def __init__(self):
+    self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
+    self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
+    self.root_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
+    self.root_rotation_bounds = [-3.14, 3.14, -0.01, 0.01, -0.01, 0.01] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
+    # The rotation bounds are only used during the random sampling, they are not enforced along the path
+    self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
+    self.mu = 0.5 # friction coefficient between the robot and the environment
+    self.used_limbs = [] # names of the limbs that must be in contact during all the motion
+    self.size_foot_x = 0 # size of the feet along the x axis
+    self.size_foot_y = 0 # size of the feet along the y axis
+    # 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.q_init = []
+    self.q_goal = []
+
+
+  @abstractmethod
+  def load_rbprm(self):
+    """
+    Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
+    """
+    pass
+
+  def set_joints_bounds(self):
+    """
+    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
+    """
+    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
+    """
+    self.load_rbprm()
+    self.set_joints_bounds()
+    self.set_rom_filters()
+    self.ps = ProblemSolver( self.rbprmBuilder)
+    # define parameters used by various methods :
+    if self.v_max >= 0:
+      self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
+    if self.a_max >= 0:
+      self.ps.setParameter( "Kinodynamic/accelerationBound", self.a_max)
+    if self.size_foot_x > 0:
+      self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
+    if self.size_foot_y > 0:
+      self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
+    self.ps.setParameter("DynamicPlanner/friction", 0.5)
+    # sample only configuration with null velocity and acceleration :
+    self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
+
+  def init_viewer(self, env_name, env_package = "hpp_environments", 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 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])
+    self.afftool.loadObstacleModel (env_package, env_name, "planning", vf)
+    try :
+        self.v = vf.createViewer(displayArrows = True)
+    except Exception:
+        print("No viewer started !")
+        class FakeViewer():
+            def __init__(self):
+                return
+            def __call__(self,q):
+                return
+            def addLandmark(self,a,b):
+                return
+        self.v = FakeViewer()
+    self.pp = PathPlayer(self.v)
+    for aff_type in visualize_affordances:
+      self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
+
+
+
+  def init_planner(self, kinodynamic = True):
+    """
+    Select the rbprm methods, and the kinodynamic ones if required
+    :param kinodynamic: if True, also select the kinodynamic methods
+    """
+    self.ps.selectConfigurationShooter("RbprmShooter")
+    self.ps.selectPathValidation("RbprmPathValidation", 0.05)
+    if kinodynamic:
+      self.ps.addPathOptimizer("RandomShortcutDynamic")
+      self.ps.selectSteeringMethod("RBPRMKinodynamic")
+      self.ps.selectDistance("Kinodynamic")
+      self.ps.selectPathPlanner("DynamicPlanner")
+
+
+  def solve(self):
+    """
+    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():
+      raise ValueError("Goal configuration vector do not have the right size")
+    self.ps.setInitialConfig (self.q_init)
+    self.ps.addGoalConfig (self.q_goal)
+    self.v(self.q_init)
+    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)
+    """
+    if path_id < 0:
+      path_id = self.ps.numberPaths()-1
+    self.pp.dt = dt
+    self.pp.displayVelocityPath(path_id)
+
+  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)
+    """
+    if path_id < 0:
+      path_id = self.ps.numberPaths()-1
+    self.pp.dt = dt
+    self.pp(path_id)
+
+  def hide_rom(self):
+    """
+    Move visual ROM far above the meshs, as we cannot just delete it.
+    """
+    q_far = self.q_init[::]
+    q_far[2] = -3
+    self.v(q_far)
+
+  @abstractmethod
+  def run(self):
+    """
+    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()
+    """
+    pass
\ No newline at end of file

script/scenarios/demos/talos_flatGround.py

-from talos_rbprm.talos import Robot
-from hpp.gepetto import Viewer
-from hpp.corbaserver.rbprm.tools.display_tools import *
-import time
-print("Plan guide trajectory ...")
-from . import talos_flatGround_path as tp
-print("Done.")
-import time
+from scenarios.demos.talos_flatGround_path import PathPlanner
+from scenarios.talos_contact_generator import TalosContactGenerator
 
-pId = tp.ps.numberPaths() -1
-fullBody = Robot ()
+class ContactGenerator(TalosContactGenerator):
 
-# Set the bounds for the root
-fullBody.setJointBounds ("root_joint",  [-5,5, -1.5, 1.5, 0.95, 1.05])
-# add the 6 extraDof for velocity and acceleration (see *_path.py script)
-fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
-fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
-ps = tp.ProblemSolver( fullBody )
-ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
-ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
-#load the viewer
-v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
+    def __init__(self):
+        super().__init__(PathPlanner())
 
-# load a reference configuration
-q_ref = fullBody.referenceConfig[::]+[0]*6
-fullBody.setReferenceConfig(q_ref)
-fullBody.setCurrentConfig (q_ref)
-fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
-fullBody.usePosturalTaskContactCreation(True)
+    def run(self):
+        self.load_fullbody()
+        self.set_joints_bounds()
+        self.set_reference(True)
+        self.load_limbs("fixedStep06")
+        self.init_problem()
+        self.init_viewer()
+        self.compute_configs_from_guide()
+        # force root height to be at the reference position:
+        self.q_init[2] = self.q_ref[2]
+        self.q_goal[2] = self.q_ref[2]
+        self.interpolate()
 
-print("Generate limb DB ...")
-tStart = time.time()
-# generate databases :
-fullBody.limbs_names = tp.used_limbs
-fullBody.loadAllLimbs("fixedStep06")
-tGenerate =  time.time() - tStart
-print("Done.")
-print("Databases generated in : "+str(tGenerate)+" s")
 
-#define initial and final configurations :
-q_init = q_ref[::]
-q_init[0:7] = tp.ps.configAtParam(pId,0.001)[0:7] 
-q_goal = q_init[::]
-q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
+if __name__ == "__main__":
+    cg = ContactGenerator()
+    cg.run()
 
-# force root height to be at the reference position: 
-q_init[2] = q_ref[2]
-q_goal[2] = q_ref[2]
-
-# copy extraconfig for start and init configurations
-configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
-q_init[configSize:configSize+3] = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
-q_init[configSize+3:configSize+6] = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
-q_goal[configSize:configSize+3] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
-q_goal[configSize+3:configSize+6] = [0,0,0]
-
-
-fullBody.setStaticStability(True)
-v.addLandmark('talos/base_link',0.3)
-v(q_init)
-
-# specify the full body configurations as start and goal state of the problem
-fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
-fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
-
-
-print("Generate contact plan ...")
-tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,quasiStatic=True)
-tInterpolateConfigs = time.time() - tStart
-print("Done.")
-print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
-print("number of configs :", len(configs))
-#raw_input("Press Enter to display the contact sequence ...")
-#displayContactSequence(v,configs)
 
 
 

script/scenarios/demos/talos_flatGround_path.py

-from scenarios.template_talos_path import TalosPathPlanning
+from scenarios.talos_path_planner import TalosPathPlanner
 
-class PathPlanning(TalosPathPlanning):
+class PathPlanner(TalosPathPlanner):
 
     def run(self):
         self.init_problem()
@@ -12,8 +12,9 @@ class PathPlanning(TalosPathPlanning):
         self.init_planner()
         self.solve()
         self.display_path()
-        self.play_path()
-
+        #self.play_path()
+        self.hide_rom()
 
 if __name__ == "__main__":
-    PathPlanning().run()
\ No newline at end of file
+    planner = PathPlanner()
+    planner.run()
\ No newline at end of file