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)
c1[2] = c0[2]
c_t_left = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+3)
t = c_t_left.max()
c0 = c1.copy()
# go back to the initial CoM position in 2 seconds
c1 = com
c_t_mid = polynomial(c0, dc0, ddc0, c1, dc1, ddc1, t, t+2)
c_t = piecewise(c_t_right)
c_t.append(c_t_left)
c_t.append(c_t_mid)
phase.c_t = c_t
phase.dc_t = c_t.compute_derivate(1)
phase.ddc_t = c_t.compute_derivate(2)
# set the timings of the phase :
phase.timeInitial = c_t.min()
phase.timeFinal = c_t.max()
# set a 0 AM trajectory
generateZeroAMreference(cs)
assert cs.haveTimings()
assert cs.haveConsistentContacts()
assert cs.haveCentroidalValues()
assert cs.haveCentroidalTrajectories()
cs.saveAsBinary("com_motion_above_feet_COM.cs")
