Skip to content

GitLab

Commit 06e49fdd authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

flake8

parent 41868547
Hide whitespace changes
Inline Side-by-side
unittest/python/scenario.py
View file @ 06e49fdd
# Copyright (c) 2019, CNRS
# Authors: Pierre Fernbach <pfernbac@laas.fr>
import unittest
from math import cos, sin, sqrt
from random import uniform
import numpy as np
from numpy import array, array_equal, random, isclose
from random import uniform
from math import sqrt, sin, cos
from curves import SE3Curve, bezier, piecewise, piecewise_SE3, polynomial
from numpy import array, array_equal, isclose, random
import pinocchio as pin
from pinocchio import SE3, Quaternion
from curves import SE3Curve, polynomial, bezier, piecewise, piecewise_SE3
from multicontact_api import ContactModelPlanar, ContactPatch, ContactPhase, ContactSequence
from pinocchio import SE3, Quaternion
pin.switchToNumpyArray()
......@@ -26,9 +26,11 @@ def randomQuaternion():
return q
# build random piecewise polynomial with 2 polynomial of degree 3
# between 01 and 12
def createRandomPiecewisePolynomial(dim, t_min=0, t_max=2):
"""
Build random piecewise polynomial with 2 polynomial of degree 3
between 01 and 12
"""
t_mid = (t_min + t_max) / 2.
coefs0 = np.random.rand(dim, 4) # degree 3
pol0 = polynomial(coefs0, t_min, t_mid)
......@@ -117,7 +119,7 @@ def addRandomContacts(cp):
def addRandomForcesTrajs(cp):
fR = createRandomPiecewisePolynomial(12)
fL = createRandomPiecewisePolynomial(12)
fL2 = createRandomPiecewisePolynomial(12)
# fL2 = createRandomPiecewisePolynomial(12)
cp.addContactForceTrajectory("right-leg", fR)
cp.addContactForceTrajectory("left-leg", fL)
fR = createRandomPiecewisePolynomial(1)
......@@ -1213,25 +1215,25 @@ class ContactPhaseTest(unittest.TestCase):
time_points = array(random.rand(1, N)).T
time_points.sort(0)
cp = ContactPhase()
cp.setCOMtrajectoryFromPoints(points,points_derivative,points_second_derivative,time_points)
cp.setCOMtrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points)
self.assertEqual(cp.c_t.min(), time_points[0])
self.assertEqual(cp.c_t.max(), time_points[-1])
self.assertEqual(cp.dc_t.dim(), 3)
for i in range(N):
self.assertTrue(isclose(cp.c_t(time_points[i, 0]), points[:,i]).all())
self.assertTrue(isclose(cp.dc_t(time_points[i, 0]), points_derivative[:,i]).all())
self.assertTrue(isclose(cp.ddc_t(time_points[i, 0]), points_second_derivative[:,i]).all())
self.assertTrue(isclose(cp.c_t(time_points[i, 0]), points[:, i]).all())
self.assertTrue(isclose(cp.dc_t(time_points[i, 0]), points_derivative[:, i]).all())
self.assertTrue(isclose(cp.ddc_t(time_points[i, 0]), points_second_derivative[:, i]).all())
cp.setAMtrajectoryFromPoints(points,points_derivative,time_points)
cp.setAMtrajectoryFromPoints(points, points_derivative, time_points)
for i in range(N):
self.assertTrue(isclose(cp.L_t(time_points[i, 0]), points[:,i]).all())
self.assertTrue(isclose(cp.dL_t(time_points[i, 0]), points_derivative[:,i]).all())
self.assertTrue(isclose(cp.L_t(time_points[i, 0]), points[:, i]).all())
self.assertTrue(isclose(cp.dL_t(time_points[i, 0]), points_derivative[:, i]).all())
cp.setJointsTrajectoryFromPoints(points,points_derivative,points_second_derivative,time_points)
cp.setJointsTrajectoryFromPoints(points, points_derivative, points_second_derivative, time_points)
for i in range(N):
self.assertTrue(isclose(cp.q_t(time_points[i, 0]), points[:,i]).all())
self.assertTrue(isclose(cp.dq_t(time_points[i, 0]), points_derivative[:,i]).all())
self.assertTrue(isclose(cp.ddq_t(time_points[i, 0]), points_second_derivative[:,i]).all())
self.assertTrue(isclose(cp.q_t(time_points[i, 0]), points[:, i]).all())
self.assertTrue(isclose(cp.dq_t(time_points[i, 0]), points_derivative[:, i]).all())
self.assertTrue(isclose(cp.ddq_t(time_points[i, 0]), points_second_derivative[:, i]).all())
class ContactSequenceTest(unittest.TestCase):
......
unittest/python/serialization_examples.py
View file @ 06e49fdd
import pathlib
import unittest
import numpy as np
from numpy import array, array_equal
from random import uniform
from math import sqrt, sin, cos
import pathlib
import pinocchio as pin
from pinocchio import SE3, Quaternion
import curves
from curves import SE3Curve, polynomial, bezier, piecewise, piecewise_SE3
from multicontact_api import ContactSequence
pin.switchToNumpyArray()
......@@ -17,8 +10,7 @@ PATH = (pathlib.Path(__file__).parent.parent.parent / 'examples').absolute()
print("PATH : ", PATH)
def assertTrajNotNone(testCase, phase, root, wholeBody):
def assertTrajNotNone(testCase, phase, root, wholeBody):
testCase.assertIsNotNone(phase.c_t)
testCase.assertIsNotNone(phase.dc_t)
testCase.assertIsNotNone(phase.ddc_t)
......@@ -33,7 +25,7 @@ def assertTrajNotNone(testCase, phase, root, wholeBody):
testCase.assertIsNotNone(phase.tau_t)
def testTrajMinMax(testCase, phase,root, wholeBody):
def testTrajMinMax(testCase, phase, root, wholeBody):
testCase.assertTrue(phase.c_t.min() >= 0.)
testCase.assertTrue(phase.dc_t.min() >= 0.)
testCase.assertTrue(phase.ddc_t.min() >= 0.)
......@@ -57,13 +49,14 @@ def testTrajMinMax(testCase, phase,root, wholeBody):
testCase.assertTrue(phase.ddq_t.min() >= 0.)
testCase.assertTrue(phase.tau_t.min() >= 0.)
def testCallTraj(testCase, phase, root, quasistatic, wholeBody):
testCase.assertTrue(phase.c_t((phase.c_t.max() + phase.c_t.min()) / 2.).any())
if not quasistatic:
testCase.assertTrue(phase.dc_t((phase.dc_t.max() + phase.dc_t.min()) / 2.).any())
testCase.assertTrue(phase.ddc_t((phase.ddc_t.max() + phase.ddc_t.min()) / 2.).any())
#testCase.assertTrue(phase.L_t((phase.L_t.max() + phase.L_t.min()) / 2.).any())
#testCase.assertTrue(phase.dL_t((phase.dL_t.max() + phase.dL_t.min()) / 2.).any())
# testCase.assertTrue(phase.L_t((phase.L_t.max() + phase.L_t.min()) / 2.).any())
# testCase.assertTrue(phase.dL_t((phase.dL_t.max() + phase.dL_t.min()) / 2.).any())
if root:
testCase.assertTrue(phase.root_t.max() >= 0.)
if wholeBody:
......@@ -89,7 +82,7 @@ def testContactForce(testCase, phase):
testCase.assertTrue(traj((traj.min() + traj.max()) / 2.).any())
def checkPhase(testCase, phase, root = False, quasistatic = False, effector = False, wholeBody = False):
def checkPhase(testCase, phase, root=False, quasistatic=False, effector=False, wholeBody=False):
assertTrajNotNone(testCase, phase, root, wholeBody)
testTrajMinMax(testCase, phase, root, wholeBody)
testCallTraj(testCase, phase, root, quasistatic, wholeBody)
......@@ -98,12 +91,13 @@ def checkPhase(testCase, phase, root = False, quasistatic = False, effector = Fa
if wholeBody:
testContactForce(testCase, phase)
def checkCS(testCase, cs, root = False, quasistatic = False, effector = False, wholeBody = False):
def checkCS(testCase, cs, root=False, quasistatic=False, effector=False, wholeBody=False):
for phase in cs.contactPhases:
checkPhase(testCase, phase, root, quasistatic, effector, wholeBody)
class ExamplesSerialization(unittest.TestCase):
class ExamplesSerialization(unittest.TestCase):
def test_com_motion_above_feet_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
......@@ -114,11 +108,9 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs)
def test_com_motion_above_feet_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
self.assertEqual(cs.size(), 1)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
......@@ -128,44 +120,38 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, wholeBody = True)
checkCS(self, cs, wholeBody=True)
def test_step_in_place(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place.cs"))
cs.loadFromBinary(str(PATH / "step_in_place.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
def test_step_in_place_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_COM.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_COM.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs, effector = False, wholeBody = False)
checkCS(self, cs, effector=False, wholeBody=False)
def test_step_in_place_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
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())
checkCS(self, cs, root = True, effector = True, wholeBody = False)
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_step_in_place_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_WB.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_WB.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
......@@ -176,39 +162,38 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, effector = True, wholeBody = True)
checkCS(self, cs, effector=True, wholeBody=True)
def test_step_in_place_quasistatic(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
def test_step_in_place_quasistatic_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_COM.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_COM.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs, quasistatic = True, effector = False, wholeBody = False)
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_step_in_place_quasistatic_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_REF.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_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())
checkCS(self, cs, root = True, quasistatic = True, effector = True, wholeBody = False)
checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
def test_step_in_place_quasistatic_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_WB.cs"))
cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_WB.cs"))
self.assertEqual(cs.size(), 9)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
......@@ -219,39 +204,38 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, quasistatic = True, effector = True, wholeBody = True)
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
def test_walk_20cm(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
def test_walk_20cm_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_COM.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_COM.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
checkCS(self, cs, effector = False, wholeBody = False)
checkCS(self, cs, effector=False, wholeBody=False)
def test_walk_20cm_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_REF.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_REF.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())
checkCS(self, cs, root = True, effector = True, wholeBody = False)
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_walk_20cm_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_WB.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
......@@ -262,39 +246,38 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, effector = True, wholeBody = True)
checkCS(self, cs, effector=True, wholeBody=True)
def test_walk_20cm_quasistatic(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
def test_walk_20cm_quasistatic_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
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())
checkCS(self, cs, quasistatic = True, effector = False, wholeBody = False)
checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
def test_walk_20cm_quasistatic_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_REF.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_REF.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())
checkCS(self, cs, root = True, quasistatic = True, effector = True, wholeBody = False)
checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
def test_walk_20cm_quasistatic_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
......@@ -305,7 +288,8 @@ class ExamplesSerialization(unittest.TestCase):
self.assertTrue(cs.haveJointsDerivativesTrajectories())
self.assertTrue(cs.haveContactForcesTrajectories())
self.assertTrue(cs.haveZMPtrajectories())
checkCS(self, cs, quasistatic = True, effector = True, wholeBody = True)
checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
if __name__ == '__main__':
unittest.main()
Markdown is supported
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