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 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.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)
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)),
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.id, i.placement.translation)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i.id].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(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-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:]])
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)
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:
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, 0., True)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
ROBOT_STATE,
crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random().translation),
ACTUATION.nu, pinocchio.utils.rand(2))
CONTACTS.addContact("r_sole_contact", CONTACT_6D)
CONTACTS.addContact("l_sole_contact", CONTACT_3D)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu)
frictionCone = crocoddyl.FrictionCone(np.matrix([0., 0., 1.]).T, 0.7, 4, False)
activation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(frictionCone.lb, frictionCone.ub))
COSTS.addCost(
"r_sole_friction_cone",
crocoddyl.CostModelContactFrictionCone(
ROBOT_STATE, activation,
crocoddyl.FrameFrictionCone(ROBOT_MODEL.getFrameId('r_sole'),
frictionCone), ACTUATION.nu), 1.)
COSTS.addCost(
"l_sole_friction_cone",
crocoddyl.CostModelContactFrictionCone(
ROBOT_STATE, activation,
crocoddyl.FrameFrictionCone(ROBOT_MODEL.getFrameId('l_sole'),
frictionCone), ACTUATION.nu), 1.)
MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics(
ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True)
DATA = MODEL.createData()
# Created DAM numdiff
MODEL_ND = crocoddyl.DifferentialActionModelNumDiff(MODEL)
MODEL_ND.disturbance *= 10
dnum = MODEL_ND.createData()
np.array([0.,
1 / dt])) # makes the velocity drift disappear in one timestep
contactModel.addContact("foot_contact", supportContactModel)
# FRICTION CONE
# the friction cone can also have the [min, maximum] force parameters
# 4 is the number of faces for the approximation
mu = 0.7
normalDirection = np.array([0, 0, 1])
minForce = 0
maxForce = 200
cone = crocoddyl.FrictionCone(normalDirection, mu, 4, True, minForce, maxForce)
coneBounds = crocoddyl.ActivationBounds(cone.lb, cone.ub)
coneActivation = crocoddyl.ActivationModelWeightedQuadraticBarrier(
coneBounds, np.array([1, 1, 0, 0]))
frameFriction = crocoddyl.FrameFrictionCone(footFrameID, cone)
frictionCone = crocoddyl.CostModelContactFrictionCone(state, coneActivation,
frameFriction,
actuation.nu)
# Creating the action model for the KKT dynamics with simpletic Euler integration scheme
contactCostModel = crocoddyl.CostModelSum(state, actuation.nu)
# contactCostModel.addCost('frictionCone', frictionCone, 1e-6)
contactCostModel.addCost('joule_dissipation', joule_dissipation, 5e-3)
contactCostModel.addCost('joint_friction', joint_friction, 5e-3)
contactCostModel.addCost('velocityRegularization', v2, 1e-1)
contactCostModel.addCost('nonPenetration', nonPenetration, 1e5)
contactDifferentialModel = crocoddyl.DifferentialActionModelContactFwdDynamics(
state,
actuation,
contactModel,
