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()
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(tests)
CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)
......@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_navBauzil_hard_path.py
talos_navBauzil_hard.py
talos_navBauzil_path.py
talos_navBauzil_obstacles_path.py
talos_navBauzil.py
talos_navBauzil_obstacles.py
talos_plateformes_path.py
talos_plateformes.py
talos_stairs10_path.py
......
......@@ -14,6 +14,8 @@ parser.add_argument('scenario_name',
type=str,
help="The name of the scenario script to run. "
"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()
# retrieve argument
scenario_name = args.scenario_name
......@@ -31,27 +33,31 @@ except ImportError:
print("Cannot import " + scenario_name + ". Check if the path is correct")
sys.exit(1)
# kill already existing instance of the viewer/server
subprocess.run(["killall", "gepetto-gui"])
# kill already existing instance of the 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).
# 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",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
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
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
if hasattr(module_scenario, 'ContactGenerator'):
print("# Run contact generation script ...")
......
......@@ -83,7 +83,7 @@ class AbstractContactGenerator:
self.fullbody.setPostureWeights(self.weight_postural)
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'
:param heuristic: the name of the heuristic used,
......@@ -170,7 +170,7 @@ class AbstractContactGenerator:
self.v(self.q_init)
print("Generate contact plan ...")
t_start = time.time()
self.configs = self.fullbody.interpolate(0.01,
self.configs = self.fullbody.interpolate(self.dt,
pathId=self.pid,
robustnessTreshold=self.robustness,
filterStates=self.filter_states,
......
......@@ -109,27 +109,11 @@ class AbstractPathPlanner:
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel(env_package, env_name, "planning", vf, reduceSizes=reduce_sizes)
try:
self.v = vf.createViewer(displayArrows=True)
except Exception:
print("No viewer started !")
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
self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
"planning", vf, reduceSizes=reduce_sizes)
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)
......
......@@ -5,10 +5,12 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.robustness = 2.
def set_reference(self):
super().set_reference()
self.q_ref = self.fullbody.referenceConfig_elbowsUp[::] + [0] * self.path_planner.extra_dof
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__":
......
......@@ -5,10 +5,13 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
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
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):
super().load_limbs(heuristic)
......
......@@ -4,7 +4,8 @@ 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 += "_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()
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
class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
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()
def init_problem(self):
......
......@@ -5,7 +5,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
# 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']
self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot()
......
......@@ -13,6 +13,8 @@ class HRP2ContactGenerator(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]
def set_joints_bounds(self):
super().set_joints_bounds()
......
......@@ -23,7 +23,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
# 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
Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
self.rbprmBuilder = Robot()
......
......@@ -13,7 +13,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
# 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()
def init_problem(self):
......
......@@ -8,7 +8,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
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()
def init_problem(self):
......
......@@ -35,7 +35,8 @@ class PathPlanner(TalosPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
# 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']
self.used_limbs = Robot.urdfNameRom
self.rbprmBuilder = Robot()
......
......@@ -13,6 +13,7 @@ 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]
def init_viewer(self):
super().init_viewer()
......
......@@ -51,10 +51,11 @@ def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
if len(p_list) < 2:
return
gui = viewer.client.gui
if name is None:
node_list = gui.getNodeList()
if name is None and node_list is not None:
i = 0
name = 'surface_' + str(i)
while name in gui.getNodeList():
while name in node_list:
i = i + 1
name = 'surface_' + str(i)
gui.addCurve(name, p_list, color)
......
......@@ -975,7 +975,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
if (rep.success_) {
lastStatesComputed_.push_back(rep.result_);
lastStatesComputedTime_.push_back(std::make_pair(-1., rep.result_));
return lastStatesComputed_.size() - 1;
return (CORBA::Short)(lastStatesComputed_.size() - 1);
}
}
return -1;
......@@ -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,
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