Merge branch 'cpp_devel' into devel

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})

benchmark/hyq.py

+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

benchmark/lqr.py

-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))

benchmark/talos_legs.py

+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

benchmark/unicycle.py

 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))

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})

examples/bipedal_walking_from_foot_traj.py

 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)

examples/quadrupedal_walking_balancing.py

 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)

unittest/bindings/test_actions.py

-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)
 
 

unittest/bindings/test_actuations.py

+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__':

unittest/bindings/test_contacts.py

+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):

unittest/bindings/test_costs.py

+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):

unittest/bindings/test_shooting.py

-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):

unittest/bindings/test_solvers.py

-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__':

unittest/bindings/test_states.py

-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):