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

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 {
......
ADD_SUBDIRECTORY(python)
SET(${PROJECT_NAME}_PYTHON_TESTS
test_talos_walk_contacts
test_talos_walk_path
test_rbprm_state_6d
test_rbprm_state_3d
)
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
ADD_PYTHON_UNIT_TEST("py-${TEST}" "tests/python/${TEST}.py")
SET_TESTS_PROPERTIES("py-${TEST}" PROPERTIES RUN_SERIAL "ON")
ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
import subprocess
import time
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
from hpp.corbaserver.rbprm.hyq import Robot
class TestRBPRMstate3D(unittest.TestCase):
def test_contacts_3d(self):
subprocess.run(["killall", "hpp-rbprm-server"])
process = subprocess.Popen("hpp-rbprm-server")
time.sleep(3)
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId, fullbody.rArmId, fullbody.lArmId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.45, -0.2, 0.002]
state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state.isLimbInContact(fullbody.rArmId))
self.assertTrue(state.isLimbInContact(fullbody.lArmId))
self.assertTrue(state2.isBalanced())
process.kill()
if __name__ == '__main__':
unittest.main()
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
import subprocess
import time
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
from talos_rbprm.talos import Robot
class TestRBPRMstate6D(unittest.TestCase):
def test_contacts_6d(self):
subprocess.run(["killall", "hpp-rbprm-server"])
process = subprocess.Popen("hpp-rbprm-server")
time.sleep(3)
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds([-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertFalse(state1.isBalanced())
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.15, -0.085, 0.002]
state2, success = StateHelper.addNewContact(state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state2.isBalanced())
p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n2)
for i in range(3):
self.assertAlmostEqual(p[i], p2[i], 2)
# try to replace a contact :
p = [0.2, 0.085, 0.002]
state3, success = StateHelper.addNewContact(state2, fullbody.lLegId, p, n)
self.assertTrue(success)
self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
self.assertTrue(state3.isBalanced())
## try com projection:
com_target = com[::]
com_target[2] -= 0.08
success = state.projectToCOM(com_target)
self.assertTrue(success)
fullbody.setCurrentConfig(state.q())
com_state = fullbody.getCenterOfMass()
self.assertEqual(com_state, state.getCenterOfMass())
for i in range(3):
self.assertAlmostEqual(com_state[i], com_target[i], 4)
# try lockOtherJoints:
p = [0.1, -0.085, 0.002]
state4, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, lockOtherJoints=True)
self.assertTrue(success)
self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
self.assertTrue(state4.isBalanced())
for i in range(7, 13):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
for i in range(19, len(q)):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
# try with a rotation
rot = [0.259, 0, 0, 0.966]
state5, success = StateHelper.addNewContact(state, fullbody.rLegId, p, n, rotation=rot)
self.assertTrue(success)
self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
self.assertTrue(state5.isBalanced())
fullbody.setCurrentConfig(state5.q())
rf_pose = fullbody.getJointPosition(fullbody.rfoot)
for i in range(4):
self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)
process.kill()
if __name__ == '__main__':
unittest.main()
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import subprocess
from importlib import import_module
import unittest
import time
PATH = "hpp.corbaserver.rbprm.scenarios.demos"
class TestTalosWalkContact(unittest.TestCase):
def test_talos_walk_contacts(self):
subprocess.run(["killall", "hpp-rbprm-server"])
process = subprocess.Popen("hpp-rbprm-server")
time.sleep(3)
module_scenario = import_module(PATH + ".talos_flatGround")
if not hasattr(module_scenario, 'ContactGenerator'):
self.assertTrue(False)
ContactGenerator = getattr(module_scenario, 'ContactGenerator')
cg = ContactGenerator()
cg.run()
self.assertTrue(len(cg.configs) > 5)
self.assertTrue(len(cg.configs) < 10)
self.assertEqual(cg.q_init, cg.configs[0])
self.assertEqual(cg.q_goal, cg.configs[-1])
process.kill()
if __name__ == '__main__':
unittest.main()
# Copyright (c) 2020, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import subprocess
from importlib import import_module
import os
import unittest
import time
PATH = "hpp.corbaserver.rbprm.scenarios.demos"
class TestTalosWalkPath(unittest.TestCase):
def test_talos_walk_path(self):
subprocess.run(["killall", "hpp-rbprm-server"])
process = subprocess.Popen("hpp-rbprm-server")
time.sleep(3)
module_scenario = import_module(PATH + ".talos_flatGround_path")
if not hasattr(module_scenario, 'PathPlanner'):
self.assertTrue(False)
PathPlanner = getattr(module_scenario, 'PathPlanner')
planner = PathPlanner()
planner.run()
ps = planner.ps
self.assertEqual(ps.numberPaths(), 2)
self.assertEqual(ps.pathLength(0), ps.pathLength(1))
self.assertTrue(ps.pathLength(1) > 6.)
self.assertTrue(ps.pathLength(1) < 7.)
self.assertEqual(planner.q_init, ps.configAtParam(1, 0))
self.assertEqual(planner.q_goal, ps.configAtParam(1, ps.pathLength(1)))
process.kill()
if __name__ == '__main__':
unittest.main()
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