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

fix for python 2

parent f408152a
Pipeline #11722 failed with stage
in 7 minutes and 31 seconds
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 import ProblemSolver, Client
from hpp.corbaserver import createContext, loadServerPlugin
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.gepetto import PathPlayer, ViewerFactory
class AbstractPathPlanner:
......@@ -15,7 +16,7 @@ class AbstractPathPlanner:
extra_dof_bounds = None
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
:param context: An optional string that give a name to a corba context instance
......@@ -27,7 +28,7 @@ class AbstractPathPlanner:
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.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
......@@ -65,12 +66,10 @@ class AbstractPathPlanner:
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):
"""
......@@ -126,14 +125,17 @@ class AbstractPathPlanner:
vf = ViewerFactory(self.ps)
if 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:
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)
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
"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)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
......@@ -156,7 +158,7 @@ class AbstractPathPlanner:
else:
self.ps.addPathOptimizer("RandomShortcut")
def solve(self, display_roadmap = False):
def solve(self, display_roadmap=False):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
......@@ -174,7 +176,6 @@ class AbstractPathPlanner:
t = self.ps.solve()
print("Guide planning time : ", t)
def display_path(self, path_id=-1, dt=0.1):
"""
Display the path in the viewer, if no path specified display the last one
......@@ -223,7 +224,7 @@ class AbstractPathPlanner:
# 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()
......
......@@ -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
class ContactGenerator(TalosContactGenerator):
class ContactGenerator(TalosContactGenerator, object):
def __init__(self):
super().__init__(PathPlanner())
super(ContactGenerator, self).__init__(PathPlanner())
if __name__ == "__main__":
......
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner):
class PathPlanner(TalosPathPlanner, object):
def init_problem(self):
self.a_max = 0.1
super().init_problem()
super(PathPlanner, self).init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self):
self.init_problem()
......
from .abstract_contact_generator import AbstractContactGenerator
import time
from .abstract_contact_generator import AbstractContactGenerator
class TalosContactGenerator(AbstractContactGenerator):
class TalosContactGenerator(AbstractContactGenerator, object):
def __init__(self, path_planner):
super().__init__(path_planner)
super(TalosContactGenerator, self).__init__(path_planner)
self.robustness = 2
self.robot_node_name = "talos"
......@@ -13,8 +14,8 @@ class TalosContactGenerator(AbstractContactGenerator):
self.fullbody = Robot()
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.fullbody.limbs_names=[self.fullbody.rLegId, self.fullbody.lLegId]
self.fullbody.limbs_names = [self.fullbody.rLegId, self.fullbody.lLegId]
def init_viewer(self):
super().init_viewer()
super(TalosContactGenerator, self).init_viewer()
self.v.addLandmark('talos/base_link', 0.3)
from .abstract_path_planner import AbstractPathPlanner
class TalosPathPlanner(AbstractPathPlanner):
def __init__(self, context = None):
super().__init__(context)
class TalosPathPlanner(AbstractPathPlanner, object):
def __init__(self, context=None):
super(TalosPathPlanner, self).__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
......@@ -24,11 +24,11 @@ class TalosPathPlanner(AbstractPathPlanner):
self.root_translation_bounds[-2:] = [self.rbprmBuilder.ref_height] * 2
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_2_joint', [0.006761, 0.006761])
def set_configurations(self):
super().set_configurations()
super(TalosPathPlanner, self).set_configurations()
self.q_init[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