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
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)
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)
frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(self.state, i[0], pinocchio.Motion.Zero(),
pinocchio.LOCAL, nu)
footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual)
impulseFootVelCost = crocoddyl.CostModelResidual(self.state, frameVelocityResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e7)
costModel.addCost(self.rmodel.frames[i[0]].name + "_impulseVel", impulseFootVelCost, 1e6)
stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.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-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)
if self.integrator == 'euler':
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
elif self.integrator == 'rk4':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.four, 0.)
elif self.integrator == 'rk3':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.three, 0.)
elif self.integrator == 'rk2':
model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.)
else:
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
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 AnymalIntegratedRK4Test(ActionModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('anymal').model
STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(STATE)
COST_SUM = crocoddyl.CostModelSum(STATE, ACTUATION.nu)
COST_SUM.addCost(
"xReg",
crocoddyl.CostModelResidual(
STATE, crocoddyl.ResidualModelState(STATE, ACTUATION.nu)), 1e-7)
COST_SUM.addCost(
"uReg",
crocoddyl.CostModelResidual(
STATE, crocoddyl.ResidualModelControl(STATE, ACTUATION.nu)), 1e-7)
DIFFERENTIAL = crocoddyl.DifferentialActionModelFreeFwdDynamics(
STATE, ACTUATION, COST_SUM)
MODEL = crocoddyl.IntegratedActionModelRK(DIFFERENTIAL,
crocoddyl.RKType.four, 1e-3)
MODEL_DER = IntegratedActionModelRK4Derived(DIFFERENTIAL, 1e-3)
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)
if self.integrator == 'euler':
model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep)
elif self.integrator == 'rk4':
model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.four, timeStep)
elif self.integrator == 'rk3':
model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.three, timeStep)
elif self.integrator == 'rk2':
model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.two, timeStep)
else:
model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep)
return model