diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index d309c126a8c431b9324304f2d8d9464f818666d8..88be29bb393bca4bc197febc185b6605228ea9fc 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -8,4 +8,19 @@ FOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK}) ADD_EXECUTABLE(${BENCHMARK_NAME} ${BENCHMARK_NAME}.cpp) PKG_CONFIG_USE_DEPENDENCY(${BENCHMARK_NAME} eigen3) TARGET_LINK_LIBRARIES(${BENCHMARK_NAME} ${PROJECT_NAME}) -ENDFOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK}) \ No newline at end of file +ENDFOREACH(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK}) + +SET(${PROJECT_NAME}_BENCHMARK_PYTHON + hyq + lqr + talos_arm + talos_legs + unicycle + ) + +FOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON}) + PYTHON_BUILD(. "${benchmark}.py") + ADD_CUSTOM_TARGET("benchmark-${benchmark}" + ${CMAKE_COMMAND} -E env PYTHONPATH=${PROJECT_BINARY_DIR}/bindings/python + ${PYTHON_EXECUTABLE} -c "from ${benchmark} import *") +ENDFOREACH(benchmark ${${PROJECT_NAME}_BENCHMARK_PYTHON}) diff --git a/benchmark/hyq.py b/benchmark/hyq.py index 393720a18212bdbe004191254d0f3bdd782c43ea..1f6a3e1fa35b689f8e35ea95f14b06f8a6b850cf 100644 --- a/benchmark/hyq.py +++ b/benchmark/hyq.py @@ -1,9 +1,11 @@ +import time + +import example_robot_data import numpy as np -import pinocchio + import crocoddyl -import quadruped_utils -import example_robot_data -import time +import pinocchio +from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem T = int(5e3) # number of trials MAXITER = 1 @@ -17,12 +19,12 @@ GAIT = "walking" # 115 nodes def runBenchmark(gait_phase): robot_model = example_robot_data.loadHyQ().model lfFoot, rfFoot, lhFoot, rhFoot = 'lf_foot', 'rf_foot', 'lh_foot', 'rh_foot' - gait = quadruped_utils.SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot) + gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot) q0 = robot_model.referenceConfigurations['half_sitting'].copy() v0 = pinocchio.utils.zero(robot_model.nv) x0 = np.concatenate([q0, v0]) - type_of_gait = gait_phase.keys()[0] + type_of_gait = list(gait_phase.keys())[0] value = gait_phase[type_of_gait] if type_of_gait == 'walking': # Creating a walking problem diff --git a/benchmark/lqr.py b/benchmark/lqr.py index 4862bd4a07370021a02ccd7210a39353890d92a3..224a676321626a7a1c83ae9d4f110baebcd1b94f 100644 --- a/benchmark/lqr.py +++ b/benchmark/lqr.py @@ -1,8 +1,10 @@ -import crocoddyl -import utils -import numpy as np import time +import numpy as np + +import crocoddyl +from crocoddyl.utils import LQRDerived + NX = 37 NU = 12 N = 100 # number of nodes @@ -38,5 +40,5 @@ avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.ActionModelLQ print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration)) print('Python-derived lqr:') -avrg_duration, min_duration, max_duration = runBenchmark(utils.LQRDerived) +avrg_duration, min_duration, max_duration = runBenchmark(LQRDerived) print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration)) diff --git a/benchmark/talos_legs.py b/benchmark/talos_legs.py index 58017840784fc67fac8a8b9992e3f0645e38da53..bc67b76afe117ac1de5fffe0c5fb33d16d178de3 100644 --- a/benchmark/talos_legs.py +++ b/benchmark/talos_legs.py @@ -1,9 +1,11 @@ +import time + import numpy as np -import pinocchio + import crocoddyl -import biped_utils import example_robot_data -import time +import pinocchio +from crocoddyl.utils.biped import SimpleBipedGaitProblem T = int(5e3) # number of trials MAXITER = 1 @@ -13,12 +15,12 @@ GAIT = "walking" # 55 nodes def runBenchmark(gait_phase): robot_model = example_robot_data.loadTalosLegs().model rightFoot, leftFoot = 'right_sole_link', 'left_sole_link' - gait = biped_utils.SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot) + gait = SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot) q0 = robot_model.referenceConfigurations['half_sitting'].copy() v0 = pinocchio.utils.zero(robot_model.nv) x0 = np.concatenate([q0, v0]) - type_of_gait = gait_phase.keys()[0] + type_of_gait = list(gait_phase.keys())[0] value = gait_phase[type_of_gait] if type_of_gait == 'walking': # Creating a walking problem diff --git a/benchmark/unicycle.py b/benchmark/unicycle.py index 1c30fae9521d090e03dc1ba87a61624638cde7d1..2354b3dcf512a9829c73ad8819879dfcaf02a2c1 100644 --- a/benchmark/unicycle.py +++ b/benchmark/unicycle.py @@ -1,7 +1,7 @@ import crocoddyl -import utils import numpy as np import time +from crocoddyl.utils import UnicycleDerived N = 200 # number of nodes T = int(5e3) # number of trials @@ -36,5 +36,5 @@ avrg_duration, min_duration, max_duration = runBenchmark(crocoddyl.ActionModelUn print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration)) print('Python-derived unicycle:') -avrg_duration, min_duration, max_duration = runBenchmark(utils.UnicycleDerived) +avrg_duration, min_duration, max_duration = runBenchmark(UnicycleDerived) print(' CPU time [ms]: {0} ({1}, {2})'.format(avrg_duration, min_duration, max_duration)) diff --git a/bindings/python/crocoddyl/CMakeLists.txt b/bindings/python/crocoddyl/CMakeLists.txt index c463dcbb119b7509200124b726d14bd6f8306fb3..75254719c5ca111f18238053530741b7feeb443f 100644 --- a/bindings/python/crocoddyl/CMakeLists.txt +++ b/bindings/python/crocoddyl/CMakeLists.txt @@ -23,4 +23,15 @@ FOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES}) INSTALL(FILES "${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/crocoddyl/${python}" DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}) -ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES}) \ No newline at end of file +ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_BINDINGS_FILES}) + + +SET(${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES + __init__.py + biped.py + quadruped.py + ) + +FOREACH(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES}) + PYTHON_BUILD(utils ${python}) +ENDFOREACH(python ${${PROJECT_NAME}_BINDINGS_UTILS_PYTHON_FILES}) diff --git a/unittest/bindings/utils.py b/bindings/python/crocoddyl/utils/__init__.py similarity index 100% rename from unittest/bindings/utils.py rename to bindings/python/crocoddyl/utils/__init__.py diff --git a/examples/biped_utils.py b/bindings/python/crocoddyl/utils/biped.py similarity index 100% rename from examples/biped_utils.py rename to bindings/python/crocoddyl/utils/biped.py diff --git a/examples/quadruped_utils.py b/bindings/python/crocoddyl/utils/quadruped.py similarity index 100% rename from examples/quadruped_utils.py rename to bindings/python/crocoddyl/utils/quadruped.py diff --git a/examples/bipedal_walking_from_foot_traj.py b/examples/bipedal_walking_from_foot_traj.py index b93693113a1f1881bf2364ccfe139141cf67dd13..5dc018defba3ed3e7293b8caa172752d39921c54 100644 --- a/examples/bipedal_walking_from_foot_traj.py +++ b/examples/bipedal_walking_from_foot_traj.py @@ -1,10 +1,11 @@ import sys import numpy as np -import pinocchio + import crocoddyl -import biped_utils import example_robot_data +import pinocchio +from crocoddyl.utils.biped import SimpleBipedGaitProblem, plotSolution WITHDISPLAY = 'display' in sys.argv WITHPLOT = 'plot' in sys.argv @@ -20,7 +21,7 @@ x0 = np.concatenate([q0, v0]) # Setting up the 3d walking problem rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' -gait = biped_utils.SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot) +gait = SimpleBipedGaitProblem(talos_legs.model, rightFoot, leftFoot) # Setting up all tasks GAITPHASES = \ @@ -71,4 +72,4 @@ if WITHPLOT: for i, phase in enumerate(GAITPHASES): xs.extend(ddp[i].xs) us.extend(ddp[i].us) - biped_utils.plotSolution(talos_legs.model, xs, us) + plotSolution(talos_legs.model, xs, us) diff --git a/examples/quadrupedal_walking_balancing.py b/examples/quadrupedal_walking_balancing.py index cb78a3a141f520a4713c72c8822674712ba2408b..0274bd02634b02003f27849212a839fa52b02acf 100644 --- a/examples/quadrupedal_walking_balancing.py +++ b/examples/quadrupedal_walking_balancing.py @@ -1,9 +1,11 @@ import sys + import numpy as np -import pinocchio + import crocoddyl -import quadruped_utils import example_robot_data +import pinocchio +from crocoddyl.utils.quadruped import SimpleQuadrupedalGaitProblem, plotSolution WITHDISPLAY = 'display' in sys.argv WITHPLOT = 'plot' in sys.argv @@ -21,7 +23,7 @@ lfFoot = 'lf_foot' rfFoot = 'rf_foot' lhFoot = 'lh_foot' rhFoot = 'rh_foot' -gait = quadruped_utils.SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot) +gait = SimpleQuadrupedalGaitProblem(hyq.model, lfFoot, rfFoot, lhFoot, rhFoot) # Setting up all tasks GAITPHASES = [{ @@ -119,4 +121,4 @@ if WITHPLOT: for i, phase in enumerate(GAITPHASES): xs.extend(ddp[i].xs) us.extend(ddp[i].us) - quadruped_utils.plotSolution(hyq.model, xs, us) + plotSolution(hyq.model, xs, us) diff --git a/unittest/bindings/test_actions.py b/unittest/bindings/test_actions.py index ea4fb2931211375839459cb16e722d2e5e7f1087..785fbf287aff4533f1fe07a367eaf4729904b0ca 100644 --- a/unittest/bindings/test_actions.py +++ b/unittest/bindings/test_actions.py @@ -1,10 +1,12 @@ -import crocoddyl -import utils -import pinocchio +import sys +import unittest from random import randint + import numpy as np -import unittest -import sys + +import crocoddyl +import pinocchio +from crocoddyl.utils import DifferentialFreeFwdDynamicsDerived, DifferentialLQRDerived, LQRDerived, UnicycleDerived class ActionModelAbstractTestCase(unittest.TestCase): @@ -59,21 +61,21 @@ class ActionModelAbstractTestCase(unittest.TestCase): class UnicycleTest(ActionModelAbstractTestCase): MODEL = crocoddyl.ActionModelUnicycle() - MODEL_DER = utils.UnicycleDerived() + MODEL_DER = UnicycleDerived() class LQRTest(ActionModelAbstractTestCase): NX = randint(1, 21) NU = randint(1, NX) MODEL = crocoddyl.ActionModelLQR(NX, NU) - MODEL_DER = utils.LQRDerived(NX, NU) + MODEL_DER = LQRDerived(NX, NU) class DifferentialLQRTest(ActionModelAbstractTestCase): NX = randint(1, 21) NU = randint(1, NX) MODEL = crocoddyl.DifferentialActionModelLQR(NX, NU) - MODEL_DER = utils.DifferentialLQRDerived(NX, NU) + MODEL_DER = DifferentialLQRDerived(NX, NU) class FreeFwdDynamicsTest(ActionModelAbstractTestCase): @@ -86,7 +88,7 @@ class FreeFwdDynamicsTest(ActionModelAbstractTestCase): crocoddyl.CostModelFramePlacement( STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) - MODEL_DER = utils.DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM) + MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM) class FreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase): @@ -100,7 +102,7 @@ class FreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase): STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.) MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM) MODEL.armature = 0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T - MODEL_DER = utils.DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM) + MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, COST_SUM) MODEL_DER.set_armature(0.1 * np.matrix(np.ones(ROBOT_MODEL.nv)).T) diff --git a/unittest/bindings/test_actuations.py b/unittest/bindings/test_actuations.py index 9b9729753b7205356480f9e1386b40d1337510b8..48eeaee211c6b9ceecf04ada8dfa9432e22c6188 100644 --- a/unittest/bindings/test_actuations.py +++ b/unittest/bindings/test_actuations.py @@ -1,9 +1,11 @@ +import sys +import unittest + +import numpy as np + import crocoddyl -import utils import pinocchio -import numpy as np -import unittest -import sys +from crocoddyl.utils import FreeFloatingActuationDerived, FullActuationDerived class ActuationModelAbstractTestCase(unittest.TestCase): @@ -37,14 +39,14 @@ class FloatingBaseActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelHumanoidRandom()) ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE) - ACTUATION_DER = utils.FreeFloatingActuationDerived(STATE) + ACTUATION_DER = FreeFloatingActuationDerived(STATE) class FullActuationTest(ActuationModelAbstractTestCase): STATE = crocoddyl.StateMultibody(pinocchio.buildSampleModelManipulator()) ACTUATION = crocoddyl.ActuationModelFull(STATE) - ACTUATION_DER = utils.FullActuationDerived(STATE) + ACTUATION_DER = FullActuationDerived(STATE) if __name__ == '__main__': diff --git a/unittest/bindings/test_contacts.py b/unittest/bindings/test_contacts.py index 8717d8bec956464456fccf40b320c6b02f939154..b4814aba3af22a1e9c71991356e2676a11cfabab 100644 --- a/unittest/bindings/test_contacts.py +++ b/unittest/bindings/test_contacts.py @@ -1,9 +1,11 @@ +import sys +import unittest + +import numpy as np + import crocoddyl -import utils import pinocchio -import numpy as np -import unittest -import sys +from crocoddyl.utils import Contact3DDerived, Contact6DDerived class ContactModelAbstractTestCase(unittest.TestCase): @@ -96,7 +98,7 @@ class Contact3DTest(ContactModelAbstractTestCase): gains = pinocchio.utils.rand(2) xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation) CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains) - CONTACT_DER = utils.Contact3DDerived(ROBOT_STATE, xref, gains) + CONTACT_DER = Contact3DDerived(ROBOT_STATE, xref, gains) class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase): @@ -115,7 +117,7 @@ class Contact6DTest(ContactModelAbstractTestCase): gains = pinocchio.utils.rand(2) Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains) - CONTACT_DER = utils.Contact6DDerived(ROBOT_STATE, Mref, gains) + CONTACT_DER = Contact6DDerived(ROBOT_STATE, Mref, gains) class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): diff --git a/unittest/bindings/test_costs.py b/unittest/bindings/test_costs.py index df80b96ffaa20606612a0074ca60578ae71825b9..0eb27e739f718485cc7a151aa1750da42ce658d5 100644 --- a/unittest/bindings/test_costs.py +++ b/unittest/bindings/test_costs.py @@ -1,9 +1,12 @@ +import sys +import unittest + +import numpy as np + import crocoddyl -import utils import pinocchio -import numpy as np -import unittest -import sys +from crocoddyl.utils import (CoMPositionCostDerived, ControlCostDerived, FramePlacementCostDerived, + FrameTranslationCostDerived, FrameVelocityCostDerived, StateCostDerived) class CostModelAbstractTestCase(unittest.TestCase): @@ -123,7 +126,7 @@ class StateCostTest(CostModelAbstractTestCase): ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelState(ROBOT_STATE) - COST_DER = utils.StateCostDerived(ROBOT_STATE) + COST_DER = StateCostDerived(ROBOT_STATE) class StateCostSumTest(CostModelSumTestCase): @@ -138,7 +141,7 @@ class ControlCostTest(CostModelAbstractTestCase): ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) COST = crocoddyl.CostModelControl(ROBOT_STATE) - COST_DER = utils.ControlCostDerived(ROBOT_STATE) + COST_DER = ControlCostDerived(ROBOT_STATE) class ControlCostSumTest(CostModelSumTestCase): @@ -154,7 +157,7 @@ class CoMPositionCostTest(CostModelAbstractTestCase): cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref) - COST_DER = utils.CoMPositionCostDerived(ROBOT_STATE, cref=cref) + COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref) class CoMPositionCostSumTest(CostModelSumTestCase): @@ -171,7 +174,7 @@ class FramePlacementCostTest(CostModelAbstractTestCase): Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random()) COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref) - COST_DER = utils.FramePlacementCostDerived(ROBOT_STATE, Mref=Mref) + COST_DER = FramePlacementCostDerived(ROBOT_STATE, Mref=Mref) class FramePlacementCostSumTest(CostModelSumTestCase): @@ -188,7 +191,7 @@ class FrameTranslationCostTest(CostModelAbstractTestCase): xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.utils.rand(3)) COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref) - COST_DER = utils.FrameTranslationCostDerived(ROBOT_STATE, xref=xref) + COST_DER = FrameTranslationCostDerived(ROBOT_STATE, xref=xref) class FrameTranslationCostSumTest(CostModelSumTestCase): @@ -205,7 +208,7 @@ class FrameVelocityCostTest(CostModelAbstractTestCase): vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref) - COST_DER = utils.FrameVelocityCostDerived(ROBOT_STATE, vref=vref) + COST_DER = FrameVelocityCostDerived(ROBOT_STATE, vref=vref) class FrameVelocityCostSumTest(CostModelSumTestCase): diff --git a/unittest/bindings/test_shooting.py b/unittest/bindings/test_shooting.py index fe5e2468cee1ed791194428197d304b8b6001807..fa394f03e60c6fb60a1dd9c57143ea0d62f39170 100644 --- a/unittest/bindings/test_shooting.py +++ b/unittest/bindings/test_shooting.py @@ -1,9 +1,11 @@ -import crocoddyl -from utils import UnicycleDerived +import sys +import unittest from random import randint + import numpy as np -import unittest -import sys + +import crocoddyl +from crocoddyl.utils import UnicycleDerived class ShootingProblemTestCase(unittest.TestCase): diff --git a/unittest/bindings/test_solvers.py b/unittest/bindings/test_solvers.py index 1bd54596b760628e45f6262757267d01ff975fc3..5dd124032916773054b0c44612a1f0450889d849 100644 --- a/unittest/bindings/test_solvers.py +++ b/unittest/bindings/test_solvers.py @@ -1,10 +1,12 @@ -import crocoddyl -import utils -import pinocchio +import sys +import unittest from random import randint + import numpy as np -import unittest -import sys + +import crocoddyl +import pinocchio +from crocoddyl.utils import DDPDerived class SolverAbstractTestCase(unittest.TestCase): @@ -96,7 +98,7 @@ class SolverAbstractTestCase(unittest.TestCase): class UnicycleDDPTest(SolverAbstractTestCase): MODEL = crocoddyl.ActionModelUnicycle() SOLVER = crocoddyl.SolverDDP - SOLVER_DER = utils.DDPDerived + SOLVER_DER = DDPDerived class ManipulatorDDPTest(SolverAbstractTestCase): @@ -113,7 +115,7 @@ class ManipulatorDDPTest(SolverAbstractTestCase): MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM), 1e-3) SOLVER = crocoddyl.SolverDDP - SOLVER_DER = utils.DDPDerived + SOLVER_DER = DDPDerived if __name__ == '__main__': diff --git a/unittest/bindings/test_states.py b/unittest/bindings/test_states.py index 732954b9482431eb9842fde330521af92a897fec..ac59912c0c7ffe1d2b6958a9a019ef920d7dd319 100644 --- a/unittest/bindings/test_states.py +++ b/unittest/bindings/test_states.py @@ -1,10 +1,12 @@ -import crocoddyl -from utils import StateVectorDerived, StateMultibodyDerived -import pinocchio +import sys +import unittest from random import randint + import numpy as np -import unittest -import sys + +import crocoddyl +import pinocchio +from crocoddyl.utils import StateMultibodyDerived, StateVectorDerived class StateAbstractTestCase(unittest.TestCase):