def test_com_motion_above_feet_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs)
def test_walk_20cm_quasistatic_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertFalse(cs.haveFriction())
self.assertFalse(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_step_in_place_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_com_motion_above_feet_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, wholeBody=True)
def test_walk_20cm_quasistatic_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
self.assertTrue(cs.haveJointsTrajectories())
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
self.assertTrue(cs.haveFriction())
self.assertTrue(cs.haveContactModelDefined())
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
if phase.numContacts() == 2:
phase.timeFinal = t + 2. # DS duration
else:
phase.timeFinal = t + 1.5 ## SS duration
t = phase.timeFinal
# define init/end CoM position :
if phase.numContacts() == 1:
phase.c_init = cs.contactPhases[pid - 1].c_final
phase.c_final = cs.contactPhases[pid - 1].c_final
else:
if pid > 0:
phase.c_init = cs.contactPhases[pid - 1].c_final
if pid < cs.size() - 1:
if cs.contactPhases[pid + 1].isEffectorInContact(fb.lfoot):
phase.c_final = c_l
elif cs.contactPhases[pid + 1].isEffectorInContact(fb.rfoot):
phase.c_final = c_r
else:
phase.c_final = c_mid
for phase in cs.contactPhases:
genCOMTrajFromPhaseStates(phase)
genAMTrajFromPhaseStates(phase)
assert cs.haveConsistentContacts()
assert cs.haveConsistentContacts()
assert cs.haveCentroidalValues()
assert cs.haveCentroidalTrajectories()
cs.saveAsBinary("step_in_place_quasistatic_COM.cs")
