Commit 06e49fdd by Guilhem Saurel

### flake8

parent 41868547
 # Copyright (c) 2019, CNRS # Authors: Pierre Fernbach 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): ... ...
 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,7 +10,6 @@ PATH = (pathlib.Path(__file__).parent.parent.parent / 'examples').absolute() print("PATH : ", PATH) def assertTrajNotNone(testCase, phase, root, wholeBody): testCase.assertIsNotNone(phase.c_t) testCase.assertIsNotNone(phase.dc_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,8 +108,6 @@ 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")) ... ... @@ -128,9 +120,7 @@ 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() ... ... @@ -138,7 +128,6 @@ class ExamplesSerialization(unittest.TestCase): 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")) ... ... @@ -147,8 +136,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -159,9 +147,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -176,7 +162,7 @@ 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() ... ... @@ -184,7 +170,6 @@ class ExamplesSerialization(unittest.TestCase): 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")) ... ... @@ -193,7 +178,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -204,7 +189,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -219,7 +204,7 @@ 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() ... ... @@ -227,7 +212,6 @@ class ExamplesSerialization(unittest.TestCase): 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")) ... ... @@ -236,7 +220,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -247,7 +231,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -262,7 +246,7 @@ 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() ... ... @@ -270,7 +254,6 @@ class ExamplesSerialization(unittest.TestCase): 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")) ... ... @@ -279,7 +262,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -290,7 +273,7 @@ class ExamplesSerialization(unittest.TestCase): 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() ... ... @@ -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!