def test_talos_stairs_manual_limb_rrt(self):
cfg = Config()
cfg.load_scenario_config("talos_stairs10")
cfg.contact_generation_method = "load"
cfg.centroidal_method = "momentumopt"
cfg.IK_store_centroidal = True
cfg.IK_store_zmp = True
cfg.IK_store_effector = True
cfg.IK_store_contact_forces = True
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = True
cfg.ITER_DYNAMIC_FILTER = 0
with ServerManager('hpp-rbprm-server'):
cs = create_stairs_cs()
if not os.path.exists(cfg.CONTACT_SEQUENCE_PATH):
os.makedirs(cfg.CONTACT_SEQUENCE_PATH)
filename = cfg.CS_FILENAME
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
check_motion(self, loco_planner)
def test_talos_stairs_sl1m_mopt(self):
cfg = Config()
cfg.IK_store_centroidal = True
cfg.IK_store_zmp = True
cfg.IK_store_effector = True
cfg.IK_store_contact_forces = True
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = True
cfg.load_scenario_config("talos_stairs10")
cfg.contact_generation_method = "sl1m"
cfg.centroidal_method = "momentumopt"
cfg.wholebody_method = "none"
cfg.ITER_DYNAMIC_FILTER = 0
with ServerManager('hpp-rbprm-server'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
self.assertTrue(loco_planner.cs.size() > 2)
self.assertTrue(loco_planner.cs.size() < 50)
self.assertEqual(loco_planner.cs.size(),
loco_planner.cs_com.size())
self.assertEqual(loco_planner.cs.size(),
loco_planner.cs_ref.size())
self.assertIsNone(loco_planner.cs_wb)
# check that the sequence contains the expected data:
self.assertTrue(loco_planner.cs.haveConsistentContacts())
self.assertTrue(loco_planner.cs_ref.haveConsistentContacts())
self.assertTrue(loco_planner.cs_ref.haveTimings())
self.assertTrue(loco_planner.cs_ref.haveCentroidalTrajectories())
self.assertTrue(loco_planner.cs_ref.haveZMPtrajectories())
self.assertTrue(
loco_planner.cs_ref.haveEffectorsTrajectories(1e-2))
def gen_step_in_place_quasistatic():
import mlp.contact_sequence.manually_defined.step_in_place_quasiStatic
cfg = Config()
cfg.load_scenario_config("talos_flatGround_quasiStatic")
cfg.centroidal_method = "load"
demo_name = "step_in_place_quasistatic"
set_cs_names(cfg, demo_name)
store_all(cfg)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
def gen_com_motion():
import mlp.contact_sequence.manually_defined.com_motion_above_feet
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.centroidal_method = "load"
demo_name = "com_motion_above_feet"
set_cs_names(cfg, demo_name)
store_all(cfg)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
def gen_walk_20cm_quasistatic():
cfg = Config()
cfg.load_scenario_config("talos_flatGround_quasiStatic")
cfg.contact_generation_method = "load"
cfg.centroidal_method = "quasistatic"
cfg.ITER_DYNAMIC_FILTER = 0
demo_name = "walk_20cm_quasistatic"
set_cs_names(cfg, demo_name)
store_all(cfg)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
def gen_walk_20cm():
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.contact_generation_method = "load"
cfg.centroidal_method = "momentumopt"
cfg.ITER_DYNAMIC_FILTER = 2
demo_name = "walk_20cm"
set_cs_names(cfg, demo_name)
store_all(cfg)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
def gen_step_in_place():
import mlp.contact_sequence.manually_defined.step_in_place
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.contact_generation_method = "load"
cfg.centroidal_method = "momentumopt"
cfg.ITER_DYNAMIC_FILTER = 0
demo_name = "step_in_place"
set_cs_names(cfg, demo_name)
store_all(cfg)
loco_planner = LocoPlanner(cfg)
loco_planner.run()
def test_talos_walk_rbprm_mopt(self):
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.contact_generation_method = "rbprm"
cfg.centroidal_method = "momentumopt"
cfg.IK_store_centroidal = True
cfg.IK_store_zmp = True
cfg.IK_store_effector = True
cfg.IK_store_contact_forces = True
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = True
cfg.ITER_DYNAMIC_FILTER = 0
with ServerManager('hpp-rbprm-server'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
check_motion(self, loco_planner)
def test_talos_walk_sl1m_quasistatic_no_store(self):
cfg = Config()
cfg.load_scenario_config("talos_flatGround_quasiStatic")
cfg.contact_generation_method = "sl1m"
cfg.centroidal_method = "quasistatic"
cfg.IK_store_centroidal = False
cfg.IK_store_zmp = False
cfg.IK_store_effector = False
cfg.IK_store_contact_forces = False
cfg.IK_store_joints_derivatives = False
cfg.IK_store_joints_torque = False
cfg.ITER_DYNAMIC_FILTER = 0
with ServerManager('hpp-rbprm-server'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
check_motion(self, loco_planner, False)
def test_talos_walk_sl1m_mopt_dyn_filter(self):
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.contact_generation_method = "sl1m"
cfg.centroidal_method = "momentumopt"
cfg.IK_store_centroidal = True
cfg.IK_store_zmp = True
cfg.IK_store_effector = True
cfg.IK_store_contact_forces = True
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = True
cfg.ITER_DYNAMIC_FILTER = 2
with ServerManager('hpp-rbprm-server'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
check_motion(self, loco_planner)
self.assertEqual(len(loco_planner.cs_com_iters), 3)
self.assertEqual(len(loco_planner.cs_ref_iters), 3)
self.assertEqual(len(loco_planner.cs_wb_iters), 3)
def test_talos_walk_sl1m_topt(self):
cfg = Config()
cfg.load_scenario_config("talos_flatGround")
cfg.contact_generation_method = "sl1m"
cfg.centroidal_method = "momentumopt"
cfg.IK_store_centroidal = True
cfg.IK_store_zmp = True
cfg.IK_store_effector = True
cfg.IK_store_contact_forces = True
cfg.IK_store_joints_derivatives = True
cfg.IK_store_joints_torque = True
cfg.ITER_DYNAMIC_FILTER = 0
cfg.TIMEOPT_CONFIG_FILE = "cfg_softConstraints_timeopt_talos.yaml"
with ServerManager('hpp-rbprm-server'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
check_motion(self, loco_planner)
self.assertNotEqual(
loco_planner.cs.contactPhases[-1].timeFinal,
loco_planner.cs_com.contactPhases[-1].timeFinal)
self.assertEqual(loco_planner.cs_com.contactPhases[-1].timeFinal,
loco_planner.cs_wb.contactPhases[-1].timeFinal)
import atexit
import argparse
import os
from mlp import LocoPlanner, Config
from hpp.corbaserver.rbprm.utils import ServerManager
# init argument parser
parser = argparse.ArgumentParser(description="Run a multicontact-locomotion-planning scenario")
parser.add_argument('demo_name', type=str, help="The name of the demo configuration file to load. "
"Must be a valid python file, either inside the PYTHONPATH"
"or inside the mlp.demo_configs folder. ")
parser.add_argument("-n", "--no_viewer", help="Run mlp without visualization.",action="store_true")
args = parser.parse_args()
demo_name = args.demo_name
cfg = Config()
cfg.load_scenario_config(demo_name)
with ServerManager('hpp-rbprm-server'):
if not args.no_viewer:
with ServerManager('gepetto-gui'):
loco_planner = LocoPlanner(cfg)
loco_planner.run()
else:
loco_planner = LocoPlanner(cfg)
loco_planner.run()