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.CostModelResidual(ROBOT_STATE, crocoddyl.ResidualModelCoMPosition(ROBOT_STATE, cref))
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) +
[100] * state.nv)**2)
xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual)
# Cost for target reaching
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu)
framePlacementActivation = crocoddyl.ActivationModelWeightedQuad(
np.array([1] * 3 + [0.0001] * 3)**2)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementActivation,
framePlacementResidual)
# Cost for CoM reference
comResidual = crocoddyl.ResidualModelCoMPosition(state, comRef, actuation.nu)
comTrack = crocoddyl.CostModelResidual(state, comResidual)
# Create cost model per each action model
runningCostModel = crocoddyl.CostModelSum(state, actuation.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)
# Then let's added the running and terminal cost functions
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
runningCostModel.addCost("stateReg", xRegCost, 1e-3)
runningCostModel.addCost("ctrlReg", uRegCost, 1e-4)
runningCostModel.addCost("limitCost", limitCost, 1e3)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
terminalCostModel.addCost("stateReg", xRegTermCost, 1e-3)
terminalCostModel.addCost("limitCost", limitCost, 1e3)
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
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = \
crocoddyl.ContactModel6D(self.state, i,
pinocchio.SE3.Identity(),
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, nu)
if isinstance(comTask, np.ndarray):
comResidual = crocoddyl.ResidualModelCoMPosition(
self.state, comTask, nu)
comTrack = crocoddyl.CostModelResidual(self.state, comResidual)
costModel.addCost("comTrack", comTrack, 1e6)
for i in supportFootIds:
cone = crocoddyl.WrenchCone(self.Rsurf, self.mu,
np.array([0.1, 0.05]))
wrenchResidual = crocoddyl.ResidualModelContactWrenchCone(
self.state, i, cone, nu)
wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub))
wrenchCone = crocoddyl.CostModelResidual(self.state,
wrenchActivation,
wrenchResidual)
costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone",
wrenchCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
self.state, i[0], i[1], nu)
footTrack = crocoddyl.CostModelResidual(
self.state, framePlacementResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e6)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.state.nv - 6) + [10] * self.state.nv)
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
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, 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 3D multi-contact model, and then including the supporting
# foot
nu = self.actuation.nu
contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
for i in supportFootIds:
supportContactModel = crocoddyl.ContactModel3D(
self.state, i, np.array([0., 0., 0.]), 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, nu)
if isinstance(comTask, np.ndarray):
comResidual = crocoddyl.ResidualModelCoMPosition(
self.state, comTask, nu)
comTrack = crocoddyl.CostModelResidual(self.state, comResidual)
costModel.addCost("comTrack", comTrack, 1e6)
for i in supportFootIds:
cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False)
coneResidual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu)
coneActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(cone.lb, cone.ub))
frictionCone = crocoddyl.CostModelResidual(self.state,
coneActivation,
coneResidual)
costModel.addCost(self.rmodel.frames[i].name + "_frictionCone",
frictionCone, 1e1)
if swingFootTask is not None:
for i in swingFootTask:
frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
self.state, i[0], i[1].translation, nu)
footTrack = crocoddyl.CostModelResidual(
self.state, frameTranslationResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10.] * 6 + [1.] *
(self.rmodel.nv - 6))
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
nu)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
costModel.addCost("stateReg", stateReg, 1e1)
costModel.addCost("ctrlReg", ctrlReg, 1e-1)
lb = np.concatenate([
self.state.lb[1:self.state.nv + 1], self.state.lb[-self.state.nv:]
])
ub = np.concatenate([
self.state.ub[1:self.state.nv + 1], self.state.ub[-self.state.nv:]
])
stateBoundsResidual = crocoddyl.ResidualModelState(self.state, nu)
stateBoundsActivation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(lb, ub))
stateBounds = crocoddyl.CostModelResidual(self.state,
stateBoundsActivation,
stateBoundsResidual)
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
