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 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_contacts(self):
with ServerManager('hpp-rbprm-server'):
module_scenario = import_module(PATH + ".talos_flatGround")
self.assertTrue(hasattr(module_scenario, 'ContactGenerator'))
ContactGenerator = getattr(module_scenario, 'ContactGenerator')
cg = ContactGenerator()
cg.run()
self.assertGreater(len(cg.configs), 5)
self.assertLess(len(cg.configs), 10)
self.assertEqual(cg.q_init, cg.configs[0])
self.assertEqual(cg.q_goal, cg.configs[-1])
def test_talos_walk_path(self):
with ServerManager('hpp-rbprm-server'):
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(), 1)
self.assertTrue(ps.pathLength(0) > 6.)
self.assertTrue(ps.pathLength(0) < 7.)
self.assertEqual(planner.q_init, ps.configAtParam(0, 0))
self.assertEqual(planner.q_goal, ps.configAtParam(0, ps.pathLength(0)))
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_contacts_3d(self):
with ServerManager('hpp-rbprm-server'):
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())
p_real, n_real = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n_real)
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)
demo_name = "walk_20cm"
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()
if __name__ == "__main__":
with ServerManager('hpp-rbprm-server'):
gen_com_motion()
gen_step_in_place_quasistatic()
gen_step_in_place()
gen_walk_20cm_cs()
gen_walk_20cm()
gen_walk_20cm_quasistatic()
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_contacts_6d(self):
with ServerManager('hpp-rbprm-server'):
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)
