Commit 19782f52 authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

[Tests][Python] add test-cases for ContactSequence

parent 864eedf7
......@@ -14,7 +14,7 @@ from curves import SE3Curve,polynomial,bezier,piecewise,piecewise_SE3
pin.switchToNumpyArray()
import multicontact_api
from multicontact_api import ContactModelPlanar,ContactPatch,ContactPhase
from multicontact_api import ContactModelPlanar,ContactPatch,ContactPhase,ContactSequence
def randomQuaternion():
u1 = uniform(0.,1.)
......@@ -132,10 +132,10 @@ def addRandomEffectorTrajectories(cp):
cp.addEffectorTrajectory("left-hand",fL)
def buildRandomContactPhase():
def buildRandomContactPhase(min=0.,max=2.):
cp = ContactPhase()
cp.timeInitial = 1.
cp.timeFinal =3.5
cp.timeInitial = min
cp.timeFinal =max
addRandomPointsValues(cp)
addRandomCurvesValues(cp)
addRandomContacts(cp)
......@@ -1177,6 +1177,174 @@ class ContactPhaseTest(unittest.TestCase):
cp_xml.loadFromXML("cp_test.xml",'ContactPhase')
self.assertEqual(cp1,cp_xml)
class ContactSequenceTest(unittest.TestCase):
def test_append(self):
cs = ContactSequence(0)
self.assertTrue(cs.size() == 0)
cp0 = buildRandomContactPhase(0,2)
cp1 = buildRandomContactPhase(2,4.)
id = cs.append(cp0)
self.assertTrue(cs.size() == 1)
self.assertTrue(id == 0)
self.assertTrue(cs.contactPhases[0] == cp0)
id = cs.append(cp1)
self.assertTrue(cs.size() == 2)
self.assertTrue(id == 1)
self.assertTrue(cs.contactPhases[0] == cp0)
self.assertTrue(cs.contactPhases[1] == cp1)
""" # test copied from c++, but the same behaviour cannot be obtained in python
def test_accessor_phase_vector(self):
cs = ContactSequence(0)
cp0 = buildRandomContactPhase(0,2)
cp1 = buildRandomContactPhase(2,4.)
cs.append(cp0)
cs.append(cp1)
phases = cs.contactPhases()
self.assertTrue(type(phases) is list)
self.assertTrue(len(phases) == 2)
self.assertTrue(phases[0] == cp0)
self.assertTrue(phases[1] == cp1)
# check that the accessor to contactPhases() create a copy :
cp2 = buildRandomContactPhase(0,2)
phases += [cp2]
self.assertTrue(len(phases) == 3)
self.assertTrue(cs.size() == 2 ) # original contact sequence should not be modified
phases[1].duration = 3.
self.assertTrue(cs.contactPhase(1) == cp1) # original contact sequence should not be modified
"""
def test_accessor_phase_reference(self):
cs = ContactSequence(0)
cp0 = buildRandomContactPhase(0,2)
cp1 = buildRandomContactPhase(2,4.)
cp2 = buildRandomContactPhase(2,4.)
cs.append(cp0)
cs.append(cp1)
cs.contactPhases[1].timeFinal = 10.
self.assertTrue(cs.contactPhases[1] != cp1)
self.assertTrue(cs.contactPhases[1].timeFinal == 10.)
cs.contactPhases[0] = cp2
self.assertTrue(cs.contactPhases[0] == cp2)
# try with a variable :
cp_ref = cs.contactPhases[0]
c_init= np.random.rand(3)
cp_ref.c_init = c_init
cp_ref.duration = 10
self.assertTrue(cs.contactPhases[0].duration == 10)
self.assertTrue(array_equal(cs.contactPhases[0].c_init, c_init))
def test_constructor_with_size(self):
cp_default = ContactPhase()
cs = ContactSequence(3)
self.assertTrue(cs.size() == 3)
for i in range(3):
self.assertTrue(cs.contactPhases[i] == cp_default)
# try to modify the uninitialized contact phase inside the sequence from the reference
cp_0 = cs.contactPhases[0]
c_init= np.random.rand(3)
cp_0.c_init = c_init
cp_0.duration = 10
self.assertTrue(cs.contactPhases[0] != cp_default)
self.assertTrue(cs.contactPhases[0].duration == 10)
self.assertTrue(array_equal(cs.contactPhases[0].c_init,c_init))
cp1 = buildRandomContactPhase(2,4.)
cs.contactPhases[1] = cp1
self.assertTrue(cs.contactPhases[1] == cp1)
def test_resize(self):
cp_default = ContactPhase()
cs = ContactSequence(3)
cp_0 = cs.contactPhases[0]
c_init= np.random.rand(3)
cp_0.c_init = c_init
cp_0.duration = 10
cp1 = buildRandomContactPhase(2,4.)
cs.contactPhases[1] = cp1
# with smaller value than current :
cs.resize(1)
self.assertTrue(cs.size() == 1)
self.assertTrue(cs.contactPhases[0].duration == 10)
self.assertTrue(array_equal(cs.contactPhases[0].c_init, c_init))
# check with greater size than current :
cs.resize(4)
self.assertTrue(cs.size() == 4)
self.assertTrue(cs.contactPhases[0].duration == 10)
self.assertTrue(array_equal(cs.contactPhases[0].c_init, c_init))
for i in range(1,4):
self.assertTrue(cs.contactPhases[i] == cp_default)
def test_operator_equal(self):
cs3 = ContactSequence()
cs4 = ContactSequence()
self.assertTrue(cs3 == cs4)
cp3_0 = buildRandomContactPhase()
cs3.append(cp3_0)
self.assertTrue(cs3 != cs4)
self.assertFalse(cs3 == cs4)
cs4.append(cp3_0)
self.assertTrue(cs3 == cs4)
cp3_1 = buildRandomContactPhase()
cs3.append(cp3_1)
self.assertTrue(cs3 != cs4)
cs4.append(cp3_1)
self.assertTrue(cs3 == cs4)
cs4.contactPhases[1].duration = 10
self.assertTrue(cs4.contactPhases[1] != cp3_1)
self.assertTrue(cs3 != cs4)
cs5 = ContactSequence(2)
cs5.contactPhases[0] = cp3_0
self.assertTrue(cs3 != cs5)
cs5.contactPhases[1] = cp3_1
self.assertTrue(cs3 == cs5)
def test_copy_constructor(self):
cs = ContactSequence()
for i in range(10):
cp = buildRandomContactPhase()
cs.append(cp)
self.assertTrue(cs.size() == 10)
cs1 = ContactSequence(cs)
self.assertTrue(cs == cs1)
for i in range(10):
self.assertTrue(cs.contactPhases[i] == cs1.contactPhases[i])
# check that it's a copy and not the same object :
cs.contactPhases[0].duration = 15.
self.assertFalse(cs == cs1)
self.assertFalse(cs.contactPhases[0] == cs1.contactPhases[0])
def test_serialization(self):
cs = ContactSequence()
for i in range(10):
cp = buildRandomContactPhase()
cs.append(cp)
cs.saveAsText("cs_test.txt")
cs_txt = ContactSequence()
cs_txt.loadFromText("cs_test.txt")
self.assertEqual(cs,cs_txt)
cs.saveAsBinary("cs_test")
cs_bin = ContactSequence()
cs_bin.loadFromBinary("cs_test")
self.assertEqual(cs,cs_bin)
cs.saveAsXML("cs_test.xml",'ContactSequence')
cs_xml = ContactSequence()
cs_xml.loadFromXML("cs_test.xml",'ContactPatch')
self.assertEqual(cs,cs_xml)
if __name__ == '__main__':
unittest.main()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment