class FramePlacementCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
class FrameVelocityCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.Motion.Random())
COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class TalosArmIntegratedRK4Test(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelRK(DIFFERENTIAL,
crocoddyl.RKType.four, 1e-3)
MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
class Contact3DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random().translation)
CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
def __init__(self, neural_net, robot):
crocoddyl.ActionModelAbstract.__init__(self,
crocoddyl.StateMultibody(robot),
0, 0)
self.net = neural_net
self.net.double()
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
class FramePlacementCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFramePlacement(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()))
class FrameRotationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = crocoddyl.FrameRotation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random().rotation)
COST = crocoddyl.CostModelFrameRotation(ROBOT_STATE, Rref)
class TalosArmShootingTest(ShootingProblemTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
DIFF_MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3)
MODEL_DER = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL_DER, 1e-3)
class TalosArmFreeFwdDynamicsWithArmatureTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('talos_arm').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelResidual(
STATE,
crocoddyl.ResidualModelFramePlacement(
STATE, ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelState(STATE)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(STATE,
crocoddyl.ResidualModelControl(STATE)),
1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsModelDerived(
STATE, ACTUATION, COST_SUM)
MODEL.armature = 0.1 * np.ones(ROBOT_MODEL.nv)
MODEL_DER.set_armature(0.1 * np.ones(ROBOT_MODEL.nv))
class Impulse6DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
frame = ROBOT_MODEL.getFrameId('r_sole')
IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse6DModelDerived(ROBOT_STATE, frame)
class FrameVelocityCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'),
pinocchio.Motion.Random())
COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
class FrameTranslationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.utils.rand(3))
COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
class FrameTranslationCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('rleg5_joint'),
pinocchio.utils.rand(3))
COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
class FramePlacementCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'),
pinocchio.SE3.Random())
COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref)
def cartpole():
# Loading the double pendulum model
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1, 1, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelQuad(state.ndx), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(state,
crocoddyl.ActivationModelQuad(1),
actModel.nu)
xPendCost = CostModelDoublePendulum(
state, crocoddyl.ActivationModelWeightedQuad(np.matrix(weights).T),
actModel.nu)
dt = 1e-2
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
return runningModel, terminalModel
class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class FrameVelocityCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.Motion.Random())
COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, vref=vref)
class FrameRotationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = crocoddyl.FrameRotation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random().rotation)
COST = crocoddyl.CostModelFrameRotation(ROBOT_STATE, Rref)
COST_DER = FrameRotationCostModelDerived(ROBOT_STATE, Rref=Rref)
class Impulse3DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
# gains = pinocchio.utils.rand(2)
frame = ROBOT_MODEL.getFrameId('lf_foot')
IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse3DModelDerived(ROBOT_STATE, frame)
class FrameRotationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Rref = pinocchio.SE3.Random().rotation
COST = crocoddyl.CostModelResidual(
ROBOT_STATE, crocoddyl.ResidualModelFrameRotation(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), Rref))
COST_DER = FrameRotationCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), rotation=Rref)
class FrameTranslationCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.utils.rand(3))
COST = crocoddyl.CostModelFrameTranslation(ROBOT_STATE, xref)
COST_DER = FrameTranslationCostModelDerived(ROBOT_STATE, xref=xref)
class FramePlacementCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
COST = crocoddyl.CostModelFramePlacement(ROBOT_STATE, Mref)
COST_DER = FramePlacementCostModelDerived(ROBOT_STATE, Mref=Mref)
def runBenchmark(model):
robot_model = ROBOT.model
q0 = np.matrix([0.173046, 1., -0.52366, 0., 0., 0.1, -0.005]).T
x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))])
# Note that we need to include a cost model (i.e. set of cost functions) in
# order to fully define the action model for our optimal control problem.
# For this particular example, we formulate three running-cost functions:
# goal-tracking cost, state and control regularization; and one terminal-cost:
# goal cost. First, let's create the common cost functions.
state = crocoddyl.StateMultibody(robot_model)
Mref = crocoddyl.FramePlacement(
robot_model.getFrameId("gripper_left_joint"),
pinocchio.SE3(np.eye(3), np.matrix([[.0], [.0], [.4]])))
goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref)
xRegCost = crocoddyl.CostModelState(state)
uRegCost = crocoddyl.CostModelControl(state)
# Create a cost model per the running and terminal action model.
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e-3)
runningCostModel.addCost("xReg", xRegCost, 1e-7)
runningCostModel.addCost("uReg", uRegCost, 1e-7)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
# Next, we need to create an action model for running and terminal knots. The
# forward dynamics (computed using ABA) are implemented
# inside DifferentialActionModelFullyActuated.
runningModel = crocoddyl.IntegratedActionModelEuler(
model(state, runningCostModel), 1e-3)
runningModel.differential.armature = np.matrix(
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
terminalModel = crocoddyl.IntegratedActionModelEuler(
model(state, terminalCostModel), 1e-3)
terminalModel.differential.armature = np.matrix(
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.]).T
# For this optimal control problem, we define 100 knots (or running action
# models) plus a terminal knot
problem = crocoddyl.ShootingProblem(x0, [runningModel] * N, terminalModel)
# Creating the DDP solver for this OC problem, defining a logger
ddp = crocoddyl.SolverDDP(problem)
duration = []
for i in range(T):
c_start = time.time()
ddp.solve([], [], MAXITER)
c_end = time.time()
duration.append(1e3 * (c_end - c_start))
avrg_duration = sum(duration) / len(duration)
min_duration = min(duration)
max_duration = max(duration)
return avrg_duration, min_duration, max_duration
class FrameVelocityCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
vref = pinocchio.Motion.Random()
COST = crocoddyl.CostModelResidual(
ROBOT_STATE,
crocoddyl.ResidualModelFrameVelocity(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'), vref, pinocchio.LOCAL))
COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, frame_id=ROBOT_MODEL.getFrameId('r_sole'), velocity=vref)
class AnymalFreeFwdDynamicsTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadANYmal().model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu)
COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE, ACTUATION.nu), 1e-7)
COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE, ACTUATION.nu), 1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
class Contact3DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadHyQ().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
xref = crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('lf_foot'),
pinocchio.SE3.Random().translation)
CONTACT = crocoddyl.ContactModel3D(ROBOT_STATE, xref, gains)
CONTACT_DER = Contact3DDerived(ROBOT_STATE, xref, gains)
class Contact6DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
CONTACT_DER = Contact6DDerived(ROBOT_STATE, Mref, gains)
class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
IMPULSES = collections.OrderedDict(
sorted({
'l_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')),
'r_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole'))
}.items()))
