Commit 721735bb authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

fix for python 2

parent f408152a
Pipeline #11722 failed with stage
in 7 minutes and 31 seconds
from abc import abstractmethod from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver import Client, ProblemSolver, createContext, loadServerPlugin
from hpp.corbaserver.affordance.affordance import AffordanceTool from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver, Client
from hpp.corbaserver import createContext, loadServerPlugin
from hpp.corbaserver.rbprm import Client as RbprmClient from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.gepetto import PathPlayer, ViewerFactory
class AbstractPathPlanner: class AbstractPathPlanner:
...@@ -15,7 +16,7 @@ class AbstractPathPlanner: ...@@ -15,7 +16,7 @@ class AbstractPathPlanner:
extra_dof_bounds = None extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self, context = None): def __init__(self, context=None):
""" """
Constructor Constructor
:param context: An optional string that give a name to a corba context instance :param context: An optional string that give a name to a corba context instance
...@@ -27,7 +28,7 @@ class AbstractPathPlanner: ...@@ -27,7 +28,7 @@ class AbstractPathPlanner:
0.01] # bounds on the rotation of the root (-z, z, -y, y, -x, x) 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 # 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.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.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.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_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis self.size_foot_y = 0 # size of the feet along the y axis
...@@ -65,12 +66,10 @@ class AbstractPathPlanner: ...@@ -65,12 +66,10 @@ class AbstractPathPlanner:
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0 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 # 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.extra_dof_bounds = [
-self.v_max, self.v_max, -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, 0, 0
-self.a_max, self.a_max, ]
-self.a_max, self.a_max,
0, 0 ]
def set_joints_bounds(self): def set_joints_bounds(self):
""" """
...@@ -126,14 +125,17 @@ class AbstractPathPlanner: ...@@ -126,14 +125,17 @@ class AbstractPathPlanner:
vf = ViewerFactory(self.ps) vf = ViewerFactory(self.ps)
if self.context: if self.context:
self.afftool = AffordanceTool(context=self.context) self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig() # FIXME: this should be called in afftool constructor self.afftool.client.affordance.affordance.resetAffordanceConfig(
) # FIXME: this should be called in afftool constructor
else: else:
self.afftool = AffordanceTool() self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf", self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
"planning", vf, reduceSizes=reduce_sizes) "planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost = True, displayArrows=True) self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v) self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances: for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown) self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
...@@ -156,7 +158,7 @@ class AbstractPathPlanner: ...@@ -156,7 +158,7 @@ class AbstractPathPlanner:
else: else:
self.ps.addPathOptimizer("RandomShortcut") self.ps.addPathOptimizer("RandomShortcut")
def solve(self, display_roadmap = False): def solve(self, display_roadmap=False):
""" """
Solve the path planning problem. Solve the path planning problem.
q_init and q_goal must have been defined before calling this method q_init and q_goal must have been defined before calling this method
...@@ -174,7 +176,6 @@ class AbstractPathPlanner: ...@@ -174,7 +176,6 @@ class AbstractPathPlanner:
t = self.ps.solve() t = self.ps.solve()
print("Guide planning time : ", t) print("Guide planning time : ", t)
def display_path(self, path_id=-1, dt=0.1): def display_path(self, path_id=-1, dt=0.1):
""" """
Display the path in the viewer, if no path specified display the last one Display the path in the viewer, if no path specified display the last one
...@@ -223,7 +224,7 @@ class AbstractPathPlanner: ...@@ -223,7 +224,7 @@ class AbstractPathPlanner:
# define initial and goal position # define initial and goal position
self.q_init[:2] = [0, 0] self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0] self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"]) self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner() self.init_planner()
self.solve() self.solve()
......
...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.scenarios.demos.talos_flatGround_path import PathPlan ...@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.scenarios.demos.talos_flatGround_path import PathPlan
from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
class ContactGenerator(TalosContactGenerator): class ContactGenerator(TalosContactGenerator, object):
def __init__(self): def __init__(self):
super().__init__(PathPlanner()) super(ContactGenerator, self).__init__(PathPlanner())
if __name__ == "__main__": if __name__ == "__main__":
......
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner): class PathPlanner(TalosPathPlanner, object):
def init_problem(self): def init_problem(self):
self.a_max = 0.1 self.a_max = 0.1
super().init_problem() super(PathPlanner, self).init_problem()
# greatly increase the number of loops of the random shortcut # greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50) self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# force the base orientation to follow the direction of motion along the Z axis # force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True) self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self): def run(self):
self.init_problem() self.init_problem()
......
from .abstract_contact_generator import AbstractContactGenerator
import time import time
from .abstract_contact_generator import AbstractContactGenerator
class TalosContactGenerator(AbstractContactGenerator): class TalosContactGenerator(AbstractContactGenerator, object):
def __init__(self, path_planner): def __init__(self, path_planner):
super().__init__(path_planner) super(TalosContactGenerator, self).__init__(path_planner)
self.robustness = 2 self.robustness = 2
self.robot_node_name = "talos" self.robot_node_name = "talos"
...@@ -13,8 +14,8 @@ class TalosContactGenerator(AbstractContactGenerator): ...@@ -13,8 +14,8 @@ class TalosContactGenerator(AbstractContactGenerator):
self.fullbody = Robot() self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
self.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId] self.fullbody.limbs_names = [self.fullbody.rLegId, self.fullbody.lLegId]
def init_viewer(self): def init_viewer(self):
super().init_viewer() super(TalosContactGenerator, self).init_viewer()
self.v.addLandmark('talos/base_link', 0.3) self.v.addLandmark('talos/base_link', 0.3)
from .abstract_path_planner import AbstractPathPlanner from .abstract_path_planner import AbstractPathPlanner
class TalosPathPlanner(AbstractPathPlanner): class TalosPathPlanner(AbstractPathPlanner, object):
def __init__(self, context = None): def __init__(self, context=None):
super().__init__(context) super(TalosPathPlanner, self).__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value # 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] self.root_translation_bounds = [-5., 5., -5., 5., 0.95, 1.05]
# set default used limbs to be both feet # set default used limbs to be both feet
...@@ -24,11 +24,11 @@ class TalosPathPlanner(AbstractPathPlanner): ...@@ -24,11 +24,11 @@ class TalosPathPlanner(AbstractPathPlanner):
self.root_translation_bounds[-2:] = [self.rbprmBuilder.ref_height] * 2 self.root_translation_bounds[-2:] = [self.rbprmBuilder.ref_height] * 2
def set_joints_bounds(self): def set_joints_bounds(self):
super().set_joints_bounds() super(TalosPathPlanner, self).set_joints_bounds()
self.rbprmBuilder.setJointBounds('torso_1_joint', [0, 0]) self.rbprmBuilder.setJointBounds('torso_1_joint', [0, 0])
self.rbprmBuilder.setJointBounds('torso_2_joint', [0.006761, 0.006761]) self.rbprmBuilder.setJointBounds('torso_2_joint', [0.006761, 0.006761])
def set_configurations(self): def set_configurations(self):
super().set_configurations() super(TalosPathPlanner, self).set_configurations()
self.q_init[8] = 0.006761 self.q_init[8] = 0.006761
self.q_goal[8] = 0.006761 self.q_goal[8] = 0.006761
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