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

Merge pull request #62 from pFernbach/devel

Add no-viewer mode and tests
parents 810988c1 132ab57b
...@@ -72,5 +72,6 @@ endif() ...@@ -72,5 +72,6 @@ endif()
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME}) PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(tests)
CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh) CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)
...@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS ...@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_navBauzil_hard_path.py talos_navBauzil_hard_path.py
talos_navBauzil_hard.py talos_navBauzil_hard.py
talos_navBauzil_path.py talos_navBauzil_path.py
talos_navBauzil_obstacles_path.py
talos_navBauzil.py talos_navBauzil.py
talos_navBauzil_obstacles.py
talos_plateformes_path.py talos_plateformes_path.py
talos_plateformes.py talos_plateformes.py
talos_stairs10_path.py talos_stairs10_path.py
......
...@@ -14,6 +14,8 @@ parser.add_argument('scenario_name', ...@@ -14,6 +14,8 @@ parser.add_argument('scenario_name',
type=str, type=str,
help="The name of the scenario script to run. " help="The name of the scenario script to run. "
"If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended") "If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended")
parser.add_argument("-n", "--no_viewer", help="Run rbprm without visualization.",action="store_true")
args = parser.parse_args() args = parser.parse_args()
# retrieve argument # retrieve argument
scenario_name = args.scenario_name scenario_name = args.scenario_name
...@@ -31,27 +33,31 @@ except ImportError: ...@@ -31,27 +33,31 @@ except ImportError:
print("Cannot import " + scenario_name + ". Check if the path is correct") print("Cannot import " + scenario_name + ". Check if the path is correct")
sys.exit(1) sys.exit(1)
# kill already existing instance of the viewer/server # kill already existing instance of the server
subprocess.run(["killall", "gepetto-gui"])
subprocess.run(["killall", "hpp-rbprm-server"]) subprocess.run(["killall", "hpp-rbprm-server"])
# run the viewer/server in background # run the server in background :
# stdout and stderr outputs of the child process are redirected to devnull (hidden). # stdout and stderr outputs of the child process are redirected to devnull (hidden).
# preexec_fn is used to ignore ctrl-c signal send to the main script (otherwise they are forwarded to the child process) # preexec_fn is used to ignore ctrl-c signal send to the main script (otherwise they are forwarded to the child process)
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
process_server = subprocess.Popen("hpp-rbprm-server", process_server = subprocess.Popen("hpp-rbprm-server",
stdout=subprocess.PIPE, stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp) preexec_fn=os.setpgrp)
# register cleanup methods to kill server when exiting python interpreter
atexit.register(process_server.kill)
# do the same for the viewer, exept if --no-viewer flag is set
if not args.no_viewer:
subprocess.run(["killall", "gepetto-gui"])
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
atexit.register(process_viewer.kill)
# wait a little for the initialization of the server/viewer # wait a little for the initialization of the server/viewer
time.sleep(3) time.sleep(3)
# register cleanup methods to kill viewer/server when exiting python interpreter
atexit.register(process_server.kill)
atexit.register(process_viewer.kill)
# Get ContactGenerator or PathPlanner class from the imported module and run it # Get ContactGenerator or PathPlanner class from the imported module and run it
if hasattr(module_scenario, 'ContactGenerator'): if hasattr(module_scenario, 'ContactGenerator'):
print("# Run contact generation script ...") print("# Run contact generation script ...")
......
...@@ -83,7 +83,7 @@ class AbstractContactGenerator: ...@@ -83,7 +83,7 @@ class AbstractContactGenerator:
self.fullbody.setPostureWeights(self.weight_postural) self.fullbody.setPostureWeights(self.weight_postural)
self.fullbody.usePosturalTaskContactCreation(use_postural_task) self.fullbody.usePosturalTaskContactCreation(use_postural_task)
def load_limbs(self, heuristic="fixedStep06", analysis=None, nb_samples=None, octree_size=None): def load_limbs(self, heuristic="fixedStep06", analysis="ReferenceConfiguration", nb_samples=None, octree_size=None):
""" """
Generate the samples used for each limbs in 'used_limbs' Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used, :param heuristic: the name of the heuristic used,
...@@ -170,7 +170,7 @@ class AbstractContactGenerator: ...@@ -170,7 +170,7 @@ class AbstractContactGenerator:
self.v(self.q_init) self.v(self.q_init)
print("Generate contact plan ...") print("Generate contact plan ...")
t_start = time.time() t_start = time.time()
self.configs = self.fullbody.interpolate(0.01, self.configs = self.fullbody.interpolate(self.dt,
pathId=self.pid, pathId=self.pid,
robustnessTreshold=self.robustness, robustnessTreshold=self.robustness,
filterStates=self.filter_states, filterStates=self.filter_states,
......
...@@ -109,27 +109,11 @@ class AbstractPathPlanner: ...@@ -109,27 +109,11 @@ class AbstractPathPlanner:
vf = ViewerFactory(self.ps) vf = ViewerFactory(self.ps)
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(env_package, env_name, "planning", vf, reduceSizes=reduce_sizes) self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
try: "planning", vf, reduceSizes=reduce_sizes)
self.v = vf.createViewer(displayArrows=True)
except Exception: self.v = vf.createViewer(ghost = True, displayArrows=True)
print("No viewer started !") self.pp = PathPlayer(self.v)
class FakeViewer():
def __init__(self):
return
def __call__(self, q):
return
def addLandmark(self, a, b):
return
self.v = FakeViewer()
try:
self.pp = PathPlayer(self.v)
except Exception:
pass
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)
......
...@@ -5,10 +5,12 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact ...@@ -5,10 +5,12 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
class ContactGenerator(TalosContactGenerator): class ContactGenerator(TalosContactGenerator):
def __init__(self): def __init__(self):
super().__init__(PathPlanner()) super().__init__(PathPlanner())
self.robustness = 2.
def set_reference(self): def load_fullbody(self):
super().set_reference() super().load_fullbody()
self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof self.fullbody.nbSamples = 100000
self.fullbody.limbs_names=[self.fullbody.lLegId, self.fullbody.rLegId] # left feet first on this scenario
if __name__ == "__main__": if __name__ == "__main__":
......
...@@ -5,10 +5,13 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact ...@@ -5,10 +5,13 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
class ContactGenerator(TalosContactGenerator): class ContactGenerator(TalosContactGenerator):
def __init__(self): def __init__(self):
super().__init__(PathPlanner()) super().__init__(PathPlanner())
self.robustness = 2.
def load_fullbody(self):
super().load_fullbody()
self.fullbody.nbSamples = 100000
self.fullbody.limbs_names=[self.fullbody.lLegId, self.fullbody.rLegId] # left feet first on this scenario
def set_reference(self):
super().set_reference()
self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
def load_limbs(self, heuristic): def load_limbs(self, heuristic):
super().load_limbs(heuristic) super().load_limbs(heuristic)
......
...@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner ...@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner): class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
Robot.urdfName += "_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.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
def init_problem(self): def init_problem(self):
......
from hpp.corbaserver.rbprm.scenarios.demos.talos_navBauzil_obstacles_path import PathPlanner
from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContactGenerator
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.robustness = 2.
def load_fullbody(self):
super().load_fullbody()
self.fullbody.nbSamples = 100000
self.fullbody.limbs_names = [self.fullbody.lLegId, self.fullbody.rLegId] # left feet first on this scenario
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
Robot.urdfName = "talos_trunk_large" # load the model with conservative bounding boxes for trunk
self.robot_node_name = "talos_trunk_large"
self.rbprmBuilder = Robot()
def init_problem(self):
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# 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()
self.root_translation_bounds = [-1.5, 4, 0., 3.3, self.rbprmBuilder.ref_height, self.rbprmBuilder.ref_height]
self.set_joints_bounds()
self.q_init[:2] = [-0.9, 1.7]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
self.q_init[-6:-3] = [0.07, 0, 0]
self.q_goal[:2] = [2, 2.6]
self.init_viewer("multicontact/floor_bauzil_obstacles", visualize_affordances=["Support"],
reduce_sizes=[0.02, 0., 0.])
self.init_planner()
self.solve()
self.display_path()
# self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
...@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner ...@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner): class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
Robot.urdfName += "_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.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
def init_problem(self): def init_problem(self):
......
...@@ -5,7 +5,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -5,7 +5,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
# select ROM model with really conservative ROM shapes # select ROM model with really conservative ROM shapes
Robot.urdfName += "_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM"
Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
self.used_limbs = Robot.urdfNameRom self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
......
...@@ -13,6 +13,8 @@ class HRP2ContactGenerator(AbstractContactGenerator): ...@@ -13,6 +13,8 @@ class HRP2ContactGenerator(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]
def set_joints_bounds(self): def set_joints_bounds(self):
super().set_joints_bounds() super().set_joints_bounds()
......
...@@ -23,7 +23,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -23,7 +23,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
# select model with conservative collision geometry for trunk # select model with conservative collision geometry for trunk
Robot.urdfName += "_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM"
# select conservative ROM for feet # select conservative ROM for feet
Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
......
...@@ -13,7 +13,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -13,7 +13,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
# select model with conservative collision geometry for trunk # select model with conservative collision geometry for trunk
Robot.urdfName += "_large" Robot.urdfName = "talos_trunk_large"
self.robot_node_name = "talos_trunk_large"
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
def init_problem(self): def init_problem(self):
......
...@@ -8,7 +8,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -8,7 +8,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
Robot.urdfName += "_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.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
def init_problem(self): def init_problem(self):
......
...@@ -35,7 +35,8 @@ class PathPlanner(TalosPathPlanner): ...@@ -35,7 +35,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self): def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot from talos_rbprm.talos_abstract import Robot
# select ROM model with really conservative ROM shapes # select ROM model with really conservative ROM shapes
Robot.urdfName += "_large_reducedROM" Robot.urdfName = "talos_trunk_large_reducedROM"
self.robot_node_name = "talos_trunk_large_reducedROM"
Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced'] Robot.urdfNameRom = ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
self.used_limbs = Robot.urdfNameRom self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot() self.rbprmBuilder = Robot()
......
...@@ -13,6 +13,7 @@ class TalosContactGenerator(AbstractContactGenerator): ...@@ -13,6 +13,7 @@ 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]
def init_viewer(self): def init_viewer(self):
super().init_viewer() super().init_viewer()
......
...@@ -51,10 +51,11 @@ def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None): ...@@ -51,10 +51,11 @@ def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
if len(p_list) < 2: if len(p_list) < 2:
return return
gui = viewer.client.gui gui = viewer.client.gui
if name is None: node_list = gui.getNodeList()
if name is None and node_list is not None:
i = 0 i = 0
name = 'surface_' + str(i) name = 'surface_' + str(i)
while name in gui.getNodeList(): while name in node_list:
i = i + 1 i = i + 1
name = 'surface_' + str(i) name = 'surface_' + str(i)
gui.addCurve(name, p_list, color) gui.addCurve(name, p_list, color)
......
...@@ -975,7 +975,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name, ...@@ -975,7 +975,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
if (rep.success_) { if (rep.success_) {
lastStatesComputed_.push_back(rep.result_); lastStatesComputed_.push_back(rep.result_);
lastStatesComputedTime_.push_back(std::make_pair(-1., rep.result_)); lastStatesComputedTime_.push_back(std::make_pair(-1., rep.result_));
return lastStatesComputed_.size() - 1; return (CORBA::Short)(lastStatesComputed_.size() - 1);
} }
} }
return -1; return -1;
...@@ -2787,7 +2787,7 @@ CORBA::Short RbprmBuilder::isLimbInContactIntermediary(const char* limbName, uns ...@@ -2787,7 +2787,7 @@ CORBA::Short RbprmBuilder::isLimbInContactIntermediary(const char* limbName, uns
} }
} }
CORBA::Short RbprmBuilder::getNumStates() throw(hpp::Error) { return lastStatesComputed_.size(); } CORBA::Short RbprmBuilder::getNumStates() throw(hpp::Error) { return (CORBA::Short)lastStatesComputed_.size(); }
CORBA::Short RbprmBuilder::computeIntermediary(unsigned short stateFrom, CORBA::Short RbprmBuilder::computeIntermediary(unsigned short stateFrom,
unsigned short stateTo) throw(hpp::Error) try { unsigned short stateTo) throw(hpp::Error) try {
......
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