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
def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
""" Action model for pseudo-impulse models.
A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
:param swingFootTask: swinging foot task
:return pseudo-impulse differential action model
"""
# Creating a 6D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity())
supportContactModel = crocoddyl.ContactModel6D(
self.state, Mref, self.actuation.nu, np.array([0., 0.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
for i in supportFootIds:
cone = crocoddyl.WrenchCone(np.identity(3), self.mu,
np.array([0.1, 0.05]))
wrenchCone = crocoddyl.CostModelContactWrenchCone(
self.state,
crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub)),
crocoddyl.FrameWrenchCone(i, cone), self.actuation.nu)
costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone",
wrenchCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
footTrack = crocoddyl.CostModelFramePlacement(
self.state, i, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack",
footTrack, 1e8)
footVel = crocoddyl.FrameMotion(i.id, pinocchio.Motion.Zero())
impulseFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, footVel, self.actuation.nu)
costModel.addCost(
self.rmodel.frames[i.id].name + "_impulseVel",
impulseFootVelCost, 1e6)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.state.nv - 6) + [10] * self.state.nv)
stateReg = crocoddyl.CostModelState(
self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
self.rmodel.defaultState, self.actuation.nu)
ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
def createPseudoImpulseModel(self, supportFootIds, swingFootTask):
""" Action model for pseudo-impulse models.
A pseudo-impulse model consists of adding high-penalty cost for the contact velocities.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return pseudo-impulse differential action model
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.]))
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, self.actuation.nu, np.array([0., 50.]))
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False)
frictionCone = crocoddyl.CostModelContactFrictionCone(
self.state,
crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub)),
crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone",
frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
impulseFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, vref, self.actuation.nu)
costModel.addCost(
self.rmodel.frames[i.frame].name + "_footTrack", footTrack,
1e7)
costModel.addCost(
self.rmodel.frames[i.frame].name + "_impulseVel",
impulseFootVelCost, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(
self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
self.rmodel.defaultState, self.actuation.nu)
ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
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 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)
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None):
""" Action model for a swing foot phase.
:param timeStep: step duration of the action model
:param supportFootIds: Ids of the constrained feet
:param comTask: CoM task
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T)
contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
if isinstance(comTask, np.ndarray):
comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu)
costModel.addCost("comTrack", comTrack, 1e6)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False)
frictionCone = crocoddyl.CostModelContactFrictionCone(
self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)),
cone, i, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] *
(self.rmodel.nv - 6))
stateReg = crocoddyl.CostModelState(self.state,
crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T),
self.rmodel.defaultState, self.actuation.nu)
ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-1)
lb = self.state.diff(0 * self.state.lb, self.state.lb)
ub = self.state.diff(0 * self.state.ub, self.state.ub)
stateBounds = crocoddyl.CostModelState(
self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)),
0 * self.rmodel.defaultState, self.actuation.nu)
costModel.addCost("stateBounds", stateBounds, 1e3)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel,
costModel, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
def createSwingFootModel(self,
timeStep,
supportFootIds,
comTask=None,
swingFootTask=None):
""" Action model for a swing foot phase.
:param timeStep: step duration of the action model
:param supportFootIds: Ids of the constrained feet
:param comTask: CoM task
:param swingFootTask: swinging foot task
:return action model for a swing foot phase
"""
# Creating a 6D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity())
supportContactModel = \
crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).T)
contactModel.addContact(self.rmodel.frames[i].name + "_contact",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
if isinstance(comTask, np.ndarray):
comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask,
self.actuation.nu)
costModel.addCost("comTrack", comTrack, 1e6)
if swingFootTask is not None:
for i in swingFootTask:
footTrack = crocoddyl.CostModelFramePlacement(
self.state, i, self.actuation.nu)
costModel.addCost(
self.rmodel.frames[i.frame].name + "_footTrack", footTrack,
1e6)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.state.nv - 6) + [10] * self.state.nv)
stateReg = crocoddyl.CostModelState(
self.state,
crocoddyl.ActivationModelWeightedQuad(
np.matrix(stateWeights**2).T), self.rmodel.defaultState,
self.actuation.nu)
ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
class TalosArmFreeFwdDynamicsTest(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadTalosArm().model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFull(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE)
COST_SUM.addCost(
'gripperPose',
crocoddyl.CostModelFramePlacement(
STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("gripper_left_joint"), pinocchio.SE3.Random())),
1e-3)
COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE), 1e-7)
COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE), 1e-7)
MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, ACTUATION, COST_SUM)
MODEL_DER = DifferentialFreeFwdDynamicsDerived(STATE, ACTUATION, COST_SUM)
def createProblem(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, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-4)
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.
actuation = crocoddyl.ActuationModelFull(state)
runningModel = crocoddyl.IntegratedActionModelEuler(
model(state, actuation, 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, actuation, 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)
xs = [x0] * (len(problem.runningModels) + 1)
us = [
m.quasiStatic(d, x0)
for m, d in list(zip(problem.runningModels, problem.runningDatas))
]
return xs, us, problem
def createFootSwitchModel(self, supportFootIds, swingFootTask):
""" Action model for a foot switch phase.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return action model for a foot switch phase
"""
# Creating a 3D multi-contact model, and then including the supporting
# foot
contactModel = crocoddyl.ContactModelMultiple(self.state,
self.actuation.nu)
for i in supportFootIds:
xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, self.actuation.nu,
np.matrix([0., 0.]).T)
contactModel.addContact('contact_' + str(i), supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(
self.state,
crocoddyl.ActivationModelWeightedQuad(
np.matrix(stateWeights**2).T), self.rmodel.defaultState,
self.actuation.nu)
ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-3)
for i in swingFootTask:
vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
impactFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, vref, self.actuation.nu)
costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost,
1e6)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel)
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
class ManipulatorDDPTest(SolverAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelManipulator()
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST_SUM = crocoddyl.CostModelSum(STATE, ROBOT_MODEL.nv)
COST_SUM.addCost('xReg', crocoddyl.CostModelState(STATE), 1e-7)
COST_SUM.addCost('uReg', crocoddyl.CostModelControl(STATE), 1e-7)
COST_SUM.addCost(
'frTrack',
crocoddyl.CostModelFramePlacement(
STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId("effector_body"), pinocchio.SE3.Random())), 1.)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelEuler(crocoddyl.DifferentialActionModelFreeFwdDynamics(STATE, COST_SUM),
1e-3)
SOLVER = crocoddyl.SolverDDP
SOLVER_DER = DDPDerived
class TalosArmFDDPTest(SolverAbstractTestCase):
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.CostModelFramePlacement(
STATE,
crocoddyl.FramePlacement(
ROBOT_MODEL.getFrameId("gripper_left_joint"),
pinocchio.SE3.Random())), 1e-3)
COST_SUM.addCost("xReg", crocoddyl.CostModelState(STATE), 1e-7)
COST_SUM.addCost("uReg", crocoddyl.CostModelControl(STATE), 1e-7)
DIFF_MODEL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelEuler(DIFF_MODEL, 1e-3)
SOLVER = crocoddyl.SolverFDDP
SOLVER_DER = FDDPDerived
def solver(starting_condition, T=30, precision=1e-9, maxiters=1000):
"""Solve one pendulum problem"""
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
dt = 1e-2
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(weights), actModel.nu)
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
problem = crocoddyl.ShootingProblem(starting_condition, [runningModel] * T,
terminal_model)
fddp = crocoddyl.SolverFDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
def solve_problem(terminal_model=None,
initial_configuration=None,
horizon: int = 100,
precision: float = 1e-9,
maxiters: int = 1000):
"""
Solve the problem for a given initial_position.
@params:
1: terminal_model = Terminal model with neural network inside it, for the crocoddyl problem.
If none, then Crocoddyl Integrated Action Model will be used as terminal model.
2: initial_configuration = initial position for the unicycle,
either a list or a numpy array or a tensor.
3: horizon = Time horizon for the unicycle. Defaults to 100
4: stop = ddp.th_stop. Defaults to 1e-9
5: maxiters = maximum iterations allowed for crocoddyl.Defaults to 1000
@returns:
ddp
"""
if isinstance(initial_configuration, list):
initial_configuration = np.array(initial_configuration)
elif isinstance(initial_configuration, torch.Tensor):
initial_configuration = initial_configuration.numpy()
# 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)
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)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
if terminal_model is None:
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
problem = crocoddyl.ShootingProblem(initial_configuration.T,
[runningModel] * horizon,
terminal_model)
fddp = crocoddyl.SolverFDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
return fddp
maxfloat * np.ones(state.nv)
])
bounds = crocoddyl.ActivationBounds(xlb, xub, 1.)
limitCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelQuadraticBarrier(bounds),
rmodel.defaultState, actuation.nu)
# Cost for state and control
stateWeights = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[10] * state.nv)
stateWeightsTerm = np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[100] * state.nv)
xRegCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
rmodel.defaultState, actuation.nu)
uRegCost = crocoddyl.CostModelControl(state, actuation.nu)
xRegTermCost = crocoddyl.CostModelState(
state, crocoddyl.ActivationModelWeightedQuad(stateWeightsTerm**2),
rmodel.defaultState, actuation.nu)
# Cost for target reaching: hand and foot
handTrackingWeights = np.array([1] * 3 + [0.0001] * 3)
Pref = crocoddyl.FramePlacement(endEffectorId,
pinocchio.SE3(np.eye(3), target))
handTrackingCost = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(handTrackingWeights**2), Pref,
actuation.nu)
footTrackingWeights = np.array([1, 1, 0.1] + [1.] * 3)
Pref = crocoddyl.FramePlacement(
leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.])))
class ControlCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
# Create a cost model per the running and terminal action model.
state = crocoddyl.StateMultibody(robot_model)
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)
# 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.
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)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-4)
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.
actuationModel = crocoddyl.ActuationModelFull(state)
dt = 1e-3
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel,
runningCostModel), dt)
class ControlCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
COST_DER = ControlCostDerived(ROBOT_STATE)
class ControlCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
COST_DER = ControlCostModelDerived(ROBOT_STATE)
actModel = crocoddyl.ActuationModelMultiCopterBase(state, 4, tau_f)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
# Needed objects to create the costs
Mref = crocoddyl.FramePlacement(robot_model.getFrameId("base_link"), pinocchio.SE3(target_quat.matrix(), target_pos))
wBasePos, wBaseOri, wBaseVel = 0.1, 1000, 1000
stateWeights = np.array([wBasePos] * 3 + [wBaseOri] * 3 + [wBaseVel] * robot_model.nv)
# Costs
goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref, actModel.nu)
xRegCost = crocoddyl.CostModelState(state, crocoddyl.ActivationModelWeightedQuad(stateWeights), state.zero(),
actModel.nu)
uRegCost = crocoddyl.CostModelControl(state, actModel.nu)
runningCostModel.addCost("xReg", xRegCost, 1e-6)
runningCostModel.addCost("uReg", uRegCost, 1e-6)
runningCostModel.addCost("trackPose", goalTrackingCost, 1e-2)
terminalCostModel.addCost("goalPose", goalTrackingCost, 100)
dt = 3e-2
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actModel, terminalCostModel), dt)
# Creating the shooting problem and the FDDP solver
T = 33
problem = crocoddyl.ShootingProblem(np.concatenate([hector.q0, np.zeros(state.nv)]), [runningModel] * T, terminalModel)
fddp = crocoddyl.SolverFDDP(problem)
class ControlCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
COST = crocoddyl.CostModelControl(ROBOT_STATE)
def optimalSolution(init_states: Union[list, np.ndarray, torch.Tensor],
terminal_model: crocoddyl.ActionModelAbstract = None,
horizon: int = 150,
precision: float = 1e-9,
maxiters: int = 1000,
use_fddp: bool = True):
"""Solve double pendulum problem with the given terminal model for the given position
Parameters
----------
init_states : list or array or tensor
These are the initial, starting configurations for the double pendulum
terminal_model: crocoddyl.ActionModelAbstract
The terminal model to be used to solve the problem
horizon : int
Time horizon for the running model
precision : float
precision for ddp.th_stop
maxiters : int
Maximum iterations allowed for the problem
use_fddp : boolean
Solve using ddp or fddp
Returns
--------
ddp : crocoddyl.Solverddp
the optimal ddp or fddp of the prblem
"""
if isinstance(init_states, torch.Tensor):
init_states = init_states.numpy()
init_states = np.atleast_2d(init_states)
solutions = []
for init_state in init_states:
robot = example_robot_data.loadDoublePendulum()
robot_model = robot.model
state = crocoddyl.StateMultibody(robot_model)
actModel = ActuationModelDoublePendulum(state, actLink=1)
weights = np.array([1.5, 1.5, 1, 1] + [0.1] * 2)
runningCostModel = crocoddyl.CostModelSum(state, actModel.nu)
dt = 1e-2
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(weights),
actModel.nu)
runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, runningCostModel), dt)
if terminal_model is None:
terminalCostModel = crocoddyl.CostModelSum(state, actModel.nu)
terminalCostModel.addCost("xGoal", xPendCost, 1e4)
terminal_model = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actModel, terminalCostModel), dt)
problem = crocoddyl.ShootingProblem(init_state,
[runningModel] * horizon,
terminal_model)
if use_fddp:
fddp = crocoddyl.SolverFDDP(problem)
else:
fddp = crocoddyl.SolverDDP(problem)
fddp.th_stop = precision
fddp.solve([], [], maxiters)
solutions.append(fddp)
return solutions
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ
# 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(weights), 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)
target = np.array(conf.target)
footName = 'foot'
footFrameID = robot_model.getFrameId(footName)
assert (robot_model.existFrame(footName))
Pref = crocoddyl.FrameTranslation(footFrameID, target)
# If also the orientation is useful for the task use
# Mref = crocoddyl.FramePlacement(footFrameID,
# pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]])))
footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref,
actuation.nu)
Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6)))
footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu)
# simulating the cost on the power with a cost on the control
power_act = crocoddyl.ActivationModelQuad(conf.n_links)
u2 = crocoddyl.CostModelControl(
state, power_act,
actuation.nu) # joule dissipation cost without friction, for benchmarking
stateAct = crocoddyl.ActivationModelWeightedQuad(
np.concatenate([np.zeros(state.nq + 1),
np.ones(state.nv - 1)]))
v2 = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx),
actuation.nu)
joint_friction = CostModelJointFrictionSmooth(state, power_act, actuation.nu)
joule_dissipation = CostModelJouleDissipation(state, power_act, actuation.nu)
# PENALIZATIONS
bounds = crocoddyl.ActivationBounds(
np.concatenate([np.zeros(1), -1e3 * np.ones(state.nx - 1)]),
1e3 * np.ones(state.nx))
stateAct = crocoddyl.ActivationModelWeightedQuadraticBarrier(
bounds, np.concatenate([np.ones(1), np.zeros(state.nx - 1)]))