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_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_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()
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))