Commit b5070f55 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'cpp_devel' into devel

parents 0ce3d632 b50c2f86
Pipeline #5432 passed with stage
in 18 minutes and 52 seconds
......@@ -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})
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
......
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))
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
......
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))
......@@ -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})
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)
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)
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)
......
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__':
......
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):
......
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):
......
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):
......
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__':
......
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):
......
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