Unverified Commit 2ecd7a4d authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #73 from pFernbach/devel

Update API to load robots
parents e7d6ad6c c3fe7e60
...@@ -32,21 +32,14 @@ module hpp ...@@ -32,21 +32,14 @@ module hpp
/// This function can be called several times to include several ROMs (one for each limb) /// This function can be called several times to include several ROMs (one for each limb)
/// The device automatically has an anchor joint called "base_joint" as /// The device automatically has an anchor joint called "base_joint" as
/// root joint. /// root joint.
/// \param romRobotName the name of the robot range of motion. /// \param robotName the name of the robot range of motion,
/// Load robot model
/// \param rootJointType type of root joint among "anchor", "freeflyer", /// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar", /// "planar",
/// \param packageName Name of the ROS package containing the model, /// \param urdfName name of the urdf file. It may contain
/// \param modelName Name of the package containing the model /// "file://", or "package://" prefixes.
/// \param urdfSuffix suffix for urdf file,
/// ///
/// The ros url are built as follows: void loadRobotRomModel (in string robotName, in string rootJointType,
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf" in string urdfName)
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
void loadRobotRomModel (in string romRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
raises (Error); raises (Error);
...@@ -54,39 +47,34 @@ module hpp ...@@ -54,39 +47,34 @@ module hpp
/// The device automatically has an anchor joint called "base_joint" as /// The device automatically has an anchor joint called "base_joint" as
/// root joint. /// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision. /// \param robotName the name of the robot trunk used for collision,
/// \param rootJointType type of root joint among "anchor", "freeflyer", /// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar", /// "planar",
/// \param packageName Name of the ROS package containing the model, /// \param urdfName name of the urdf file. It may contain
/// \param modelName Name of the package containing the model /// "file://", or "package://" prefixes.
/// \param urdfSuffix suffix for urdf file, /// \param srdfName name of the srdf file. It may contain
/// /// "file://", or "package://" prefixes.
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// ///
void loadRobotCompleteModel (in string trunkRobotName, in string rootJointType, void loadRobotCompleteModel (in string robotName, in string rootJointType,
in string packageName, in string modelName, in string urdfName, in string srdfName)
in string urdfSuffix, in string srdfSuffix)
raises (Error); raises (Error);
/// Load fullbody robot model
///
/// Create a RbprmFullBody object /// Create a RbprmFullBody object
/// The device automatically has an anchor joint called "base_joint" as /// The device automatically has an anchor joint called "base_joint" as
/// root joint. /// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision. ///
/// \param robotName Name of the robot that is constructed,
/// \param rootJointType type of root joint among "anchor", "freeflyer", /// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar", /// "planar",
/// \param packageName Name of the ROS package containing the model, /// \param urdfName name of the urdf file. It may contain
/// \param modelName Name of the package containing the model /// "file://", or "package://" prefixes.
/// \param urdfSuffix suffix for urdf file, /// \param srdfName name of the srdf file. It may contain
/// /// "file://", or "package://" prefixes.
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// ///
void loadFullBodyRobot (in string trunkRobotName, in string rootJointType, void loadFullBodyRobot (in string robotName, in string rootJointType,
in string packageName, in string modelName, in string urdfName, in string srdfName, in string selectedProblem)
in string urdfSuffix, in string srdfSuffix, in string selectedProblem)
raises (Error); raises (Error);
/// Create a RbprmFullBody object /// Create a RbprmFullBody object
......
% Open it on blender with default axis setting and export it with x forward.
% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
function [] = effectorRomToObj(filename, outname)
delimiterIn = ',';
headerlinesIn = 0;
points = importdata(filename,delimiterIn,headerlinesIn);
k = convhull(points);
vertface2obj(points,k,outname)
function vertface2obj(v,f,name)
% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
% VERTFACE2OBJ(v,f,fname)
% v is a Nx3 matrix of vertex coordinates.
% f is a Mx3 matrix of vertex indices.
% fname is the filename to save the obj file.
fid = fopen(name,'w');
for i=1:size(v,1)
fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
end
fprintf(fid,'g foo\n');
for i=1:size(f,1);
fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
end
fprintf(fid,'g\n');
fclose(fid);
...@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES ...@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
SET(${PROJECT_NAME}_PYTHON_TOOLS SET(${PROJECT_NAME}_PYTHON_TOOLS
affordance_centroids.py affordance_centroids.py
com_constraints.py com_constraints.py
constants_and_tools.py
constraint_to_dae.py constraint_to_dae.py
cwc_trajectory_helper.py cwc_trajectory_helper.py
cwc_trajectory.py cwc_trajectory.py
......
...@@ -26,42 +26,55 @@ from hpp.corbaserver.robot import Robot ...@@ -26,42 +26,55 @@ from hpp.corbaserver.robot import Robot
# trunk of the robot, and a set of robots describe the range of motion of each limb of the robot. # trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class Builder(Robot): class Builder(Robot):
# # Constructor # # Constructor
def __init__(self, load=True, clientRbprm=None): def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
self.tf_root = "base_link" self.tf_root = "base_link"
if clientRbprm is None: if clientRbprm is None:
self.clientRbprm = RbprmClient() self.clientRbprm = RbprmClient()
else: else:
self.clientRbprm = clientRbprm self.clientRbprm = clientRbprm
self.load = load self.load = load
if load:
self.loadModel(robotName, rootJointType)
## Return urdf and srdf filenames of the roms
#
def urdfROMFilenames (self):
urdfNameRoms = []
nameRoms = []
if hasattr (self, 'packageName') and hasattr (self, 'urdfNameRom') and \
hasattr (self, 'urdfSuffix') :
if not isinstance(self.urdfNameRom, list):
self.urdfNameRom = [self.urdfNameRom]
nameRoms = self.urdfNameRom
for urdfName in nameRoms:
urdfNameRoms += ["package://" + self.packageName + '/urdf/' + urdfName + self.urdfSuffix + '.urdf']
elif hasattr (self, 'urdfFilenameRom'):
urdfNameRoms = self.urdfFilenameRom
for urdfNameRom in urdfNameRoms:
nameRoms += [urdfNameRom.split("/")[-1].rstrip(".urdf")]
else :
raise RuntimeError (\
"""instance should have one of the following sets of members
- (packageName, urdfNameRom, urdfSuffix),
- (urdfFilenameRom)""")
return nameRoms, urdfNameRoms
# # Virtual function to load the robot model. # # Virtual function to load the robot model.
# #
# \param urdfName urdf description of the robot trunk, # This method looks for the following class attribute in order to find the files to load:
# \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add. # First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar", # \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"), # "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded def loadModel(self, robotName, rootJointType):
# \param packageName name of the package from where the robot will be loaded nameRoms, urdfROMFilenames = self.urdfROMFilenames()
# \param urdfSuffix optional suffix for the urdf of the robot package for i, urdfNameRom in enumerate(urdfROMFilenames):
# \param srdfSuffix optional suffix for the srdf of the robot package self.clientRbprm.rbprm.loadRobotRomModel(nameRoms[i], rootJointType, urdfNameRom)
def loadModel(self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None):
Robot.__init__(self, urdfName, rootJointType, False, client=client)
if (isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom,
urdfSuffix, srdfSuffix)
else:
self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms,
urdfSuffix, srdfSuffix)
self.clientRbprm.rbprm.initNewProblemSolver() self.clientRbprm.rbprm.initNewProblemSolver()
self.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix, urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
srdfSuffix) self.clientRbprm.rbprm.loadRobotCompleteModel(robotName, rootJointType, urdfFilename, srdfFilename)
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
# # Init RbprmShooter # # Init RbprmShooter
# #
......
...@@ -28,7 +28,8 @@ from numpy import array, matrix ...@@ -28,7 +28,8 @@ from numpy import array, matrix
# trunk of the robot, one for the range of motion # trunk of the robot, one for the range of motion
class FullBody(Robot): class FullBody(Robot):
# # Constructor # # Constructor
def __init__(self, load=True, clientRbprm=None): def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
self.tf_root = "base_link" self.tf_root = "base_link"
if clientRbprm == None: if clientRbprm == None:
self.clientRbprm = RbprmClient() self.clientRbprm = RbprmClient()
...@@ -36,27 +37,22 @@ class FullBody(Robot): ...@@ -36,27 +37,22 @@ class FullBody(Robot):
self.clientRbprm = clientRbprm self.clientRbprm = clientRbprm
self.load = load self.load = load
self.limbNames = [] self.limbNames = []
if load:
self.loadFullBodyModel(robotName, rootJointType)
# # Virtual function to load the fullBody robot model. # # Virtual function to load the fullBody robot model.
# # This method looks for the following class attribute in order to find the files to load:
# \param urdfName urdf description of the fullBody robot # First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar", # \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots # "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded def loadFullBodyModel(self, robotName, rootJointType):
# \param packageName name of the package from where the robot will be loaded urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
# \param urdfSuffix optional suffix for the urdf of the robot package print("selected problem: ",self.client.problem.getSelected("problem")[0])
# \param srdfSuffix optional suffix for the srdf of the robot package self.clientRbprm.rbprm.loadFullBodyRobot(robotName, rootJointType,
def loadFullBodyModel(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None): urdfFilename, srdfFilename,
Robot.__init__(self, urdfName, rootJointType, False, client)
self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
srdfSuffix,
self.client.problem.getSelected("problem")[0]) self.client.problem.getSelected("problem")[0])
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
self.octrees = {} self.octrees = {}
# Virtual function to load the fullBody robot model. # Virtual function to load the fullBody robot model.
......
from abc import abstractmethod from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver.affordance.affordance import AffordanceTool 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: class AbstractPathPlanner:
...@@ -13,7 +15,11 @@ class AbstractPathPlanner: ...@@ -13,7 +15,11 @@ 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): 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.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.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_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
...@@ -27,6 +33,17 @@ class AbstractPathPlanner: ...@@ -27,6 +33,17 @@ class AbstractPathPlanner:
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
self.q_init = [] self.q_init = []
self.q_goal = [] 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 @abstractmethod
def load_rbprm(self): def load_rbprm(self):
...@@ -107,7 +124,11 @@ class AbstractPathPlanner: ...@@ -107,7 +124,11 @@ class AbstractPathPlanner:
:param visualize_affordances: list of affordances type to visualize, default to none :param visualize_affordances: list of affordances type to visualize, default to none
""" """
vf = ViewerFactory(self.ps) 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.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)
...@@ -135,7 +156,7 @@ class AbstractPathPlanner: ...@@ -135,7 +156,7 @@ class AbstractPathPlanner:
else: else:
self.ps.addPathOptimizer("RandomShortcut") self.ps.addPathOptimizer("RandomShortcut")
def solve(self): 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
...@@ -147,7 +168,10 @@ class AbstractPathPlanner: ...@@ -147,7 +168,10 @@ class AbstractPathPlanner:
self.ps.setInitialConfig(self.q_init) self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal) self.ps.addGoalConfig(self.q_goal)
self.v(self.q_init) self.v(self.q_init)
t = self.ps.solve() if display_roadmap and self.v.client.gui.getNodeList() is not None:
t = self.v.solveAndDisplay("roadmap", 5, 0.001)
else:
t = self.ps.solve()
print("Guide planning time : ", t) print("Guide planning time : ", t)
......
...@@ -23,7 +23,7 @@ class AnymalContactGenerator(AbstractContactGenerator): ...@@ -23,7 +23,7 @@ class AnymalContactGenerator(AbstractContactGenerator):
self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end) self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
def load_fullbody(self): def load_fullbody(self):
from hpp.corbaserver.rbprm.anymal import Robot from anymal_rbprm.anymal import Robot
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
......
...@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner ...@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class AnymalPathPlanner(AbstractPathPlanner): class AnymalPathPlanner(AbstractPathPlanner):
def __init__(self): def __init__(self, context = None):
super().__init__() super().__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.4, 0.5] self.root_translation_bounds = [-5., 5., -5., 5., 0.4, 0.5]
# set default used limbs to be both feet # set default used limbs to be both feet
...@@ -19,8 +19,8 @@ class AnymalPathPlanner(AbstractPathPlanner): ...@@ -19,8 +19,8 @@ class AnymalPathPlanner(AbstractPathPlanner):
self.robot_node_name = "anymal_trunk" self.robot_node_name = "anymal_trunk"
def load_rbprm(self): def load_rbprm(self):
from hpp.corbaserver.rbprm.anymal_abstract import Robot from anymal_rbprm.anymal_abstract import Robot
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self): def set_joints_bounds(self):
super().set_joints_bounds() super().set_joints_bounds()
......
...@@ -2,14 +2,28 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner ...@@ -2,14 +2,28 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner): class PathPlanner(TalosPathPlanner):
def init_problem(self):
self.a_max = 0.1
super().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): def run(self):
self.init_problem() self.init_problem()
self.q_init[:2] = [0, 0] self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0] self.q_goal[:2] = [1., 0]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"]) self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner() self.init_planner(True, False)
self.solve() self.solve()
self.display_path() self.display_path()
# self.play_path() # self.play_path()
......
...@@ -6,7 +6,7 @@ class PathPlanner(TalosPathPlanner): ...@@ -6,7 +6,7 @@ class PathPlanner(TalosPathPlanner):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
Robot.urdfName = "talos_trunk_large" # load the model with conservative bounding boxes for trunk Robot.urdfName = "talos_trunk_large" # load the model with conservative bounding boxes for trunk
self.robot_node_name = "talos_trunk_large" self.robot_node_name = "talos_trunk_large"
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def init_problem(self): def init_problem(self):
super().init_problem() super().init_problem()
......
...@@ -11,7 +11,7 @@ class ContactGenerator(TalosContactGenerator): ...@@ -11,7 +11,7 @@ class ContactGenerator(TalosContactGenerator):
# use a model with upscaled collision geometry for the feet # use a model with upscaled collision geometry for the feet
Robot.urdfSuffix += "_safeFeet" Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot() self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof self.q_ref = self.fullbody.referenceConfig_legsApart[::] + [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
def set_joints_bounds(self): def set_joints_bounds(self):
......
...@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3] self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3]
self.init_problem() self.init_problem()
self.q_init[:3] = [0.16, 0.25, 1.14] self.q_init[:3] = [0.18, 0.25, 1.14]
self.q_goal[:3] = [1.09, 0.25, 1.14] self.q_goal[:3] = [1.07, 0.25, 1.14]
self.init_viewer("multicontact/plateforme_surfaces", self.init_viewer("multicontact/plateforme_surfaces",
reduce_sizes=[0.18, 0, 0], reduce_sizes=[0.18, 0, 0],
......
...@@ -9,7 +9,7 @@ class HRP2ContactGenerator(AbstractContactGenerator): ...@@ -9,7 +9,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
self.robot_node_name = "hrp2_14" self.robot_node_name = "hrp2_14"
def load_fullbody(self): def load_fullbody(self):
from hpp.corbaserver.rbprm.hrp2 import Robot from hrp2_rbprm.hrp2 import Robot
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
......
...@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner ...@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class HRP2PathPlanner(AbstractPathPlanner): class HRP2PathPlanner(AbstractPathPlanner):
def __init__(self): def __init__(self, context = None):
super().__init__() super().__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