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 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.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)
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.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, 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-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 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
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., 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.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)
frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
self.state, i[0], pinocchio.Motion.Zero(), pinocchio.LOCAL,
nu)
footTrack = crocoddyl.CostModelResidual(
self.state, framePlacementResidual)
impulseFootVelCost = crocoddyl.CostModelResidual(
self.state, frameVelocityResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e8)
costModel.addCost(
self.rmodel.frames[i[0]].name + "_impulseVel",
impulseFootVelCost, 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-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
import crocoddyl
import pinocchio
import example_robot_data
import numpy as np
from test_utils import NUMDIFF_MODIFIER, assertNumDiff
# Create robot model, data, state and actuation
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_DATA = ROBOT_MODEL.createData()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE)
# Create wrench cone and its activation
wrenchCone = crocoddyl.WrenchCone(np.identity(3), 0.7, np.array([0.1, 0.05]))
activation = crocoddyl.ActivationModelQuadraticBarrier(
crocoddyl.ActivationBounds(wrenchCone.lb, wrenchCone.ub))
# Contact wrench-cone cost unittest
CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu)
CONTACT_6D_r = crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random()), ACTUATION.nu,
pinocchio.utils.rand(2))
CONTACT_6D_l = crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random()), ACTUATION.nu,
pinocchio.utils.rand(2))
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., 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.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)
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