Commit 05cfb37b authored by Pierre Fernbach's avatar Pierre Fernbach

[Python] AbstractPathPlanner constructor take an optionnal 'context' argument

parent 2720cf90
from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import ProblemSolver, Client
from hpp.corbaserver import createContext, loadServerPlugin
from hpp.corbaserver.rbprm import Client as RbprmClient
class AbstractPathPlanner:
......@@ -13,7 +15,11 @@ class AbstractPathPlanner:
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self):
def __init__(self, context = None):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
"""
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)
......@@ -27,6 +33,17 @@ class AbstractPathPlanner:
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
self.context = context
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
self.hpp_client = Client(context=context)
self.hpp_client.problem.selectProblem(context)
self.rbprm_client = RbprmClient(context=context)
else:
self.hpp_client = None
self.rbprm_client = None
@abstractmethod
def load_rbprm(self):
......@@ -107,7 +124,11 @@ class AbstractPathPlanner:
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
if self.context:
self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig() # FIXME: this should be called in afftool constructor
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
"planning", vf, reduceSizes=reduce_sizes)
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class AnymalPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.4, 0.5]
# set default used limbs to be both feet
......@@ -20,7 +20,7 @@ class AnymalPathPlanner(AbstractPathPlanner):
def load_rbprm(self):
from hpp.corbaserver.rbprm.anymal_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class HRP2PathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.5, 0.8]
# set default used limbs to be both feet
......@@ -20,4 +20,4 @@ class HRP2PathPlanner(AbstractPathPlanner):
def load_rbprm(self):
from hpp.corbaserver.rbprm.hrp2_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class HyqPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.5, 0.75]
# set default used limbs to be both feet
......@@ -20,7 +20,7 @@ class HyqPathPlanner(AbstractPathPlanner):
def load_rbprm(self):
from hpp.corbaserver.rbprm.hyq_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class TalosPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.95, 1.05]
# set default used limbs to be both feet
......@@ -20,7 +20,7 @@ class TalosPathPlanner(AbstractPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment