def createImpulseModel(self, supportFootIds, swingFootTask, JMinvJt_damping=1e-12, r_coeff=0.0):
""" Action model for impulse models.
An impulse model consists of describing the impulse dynamics against a set of contacts.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return impulse action model
"""
# Creating a 3D multi-contact model, and then including the supporting foot
impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
for i in supportFootIds:
supportContactModel = crocoddyl.ImpulseModel3D(self.state, i)
impulseModel.addImpulse(self.rmodel.frames[i].name + "_impulse", supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, 0)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.id, i.placement.translation)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0)
costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e7)
stateWeights = np.array([1.] * 6 + [10.] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv)
stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2),
self.rmodel.defaultState, 0)
costModel.addCost("stateReg", stateReg, 1e1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = crocoddyl.ActionModelImpulseFwdDynamics(self.state, impulseModel, costModel)
model.JMinvJt_damping = JMinvJt_damping
model.r_coeff = r_coeff
return model
def createImpulseModel(self, supportFootIds, swingFootTask):
""" Action model for impulse models.
An impulse model consists of describing the impulse dynamics against a set of contacts.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return impulse action model
"""
# Creating a 6D multi-contact model, and then including the supporting foot
impulseModel = crocoddyl.ImpulseModelMultiple(self.state)
for i in supportFootIds:
supportContactModel = crocoddyl.ImpulseModel6D(self.state, i)
impulseModel.addImpulse(self.rmodel.frames[i].name + "_impulse",
supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, 0)
if swingFootTask is not None:
for i in swingFootTask:
frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
self.state, i[0], i[1].translation, 0)
footTrack = crocoddyl.CostModelResidual(
self.state, frameTranslationResidual)
costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack",
footTrack, 1e8)
stateWeights = np.array([1.] * 6 + [0.1] * (self.rmodel.nv - 6) +
[10] * self.rmodel.nv)
stateResidual = crocoddyl.ResidualModelState(self.state,
self.rmodel.defaultState,
0)
stateActivation = crocoddyl.ActivationModelWeightedQuad(
stateWeights**2)
stateReg = crocoddyl.CostModelResidual(self.state, stateActivation,
stateResidual)
costModel.addCost("stateReg", stateReg, 1e1)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
model = crocoddyl.ActionModelImpulseFwdDynamics(
self.state, impulseModel, costModel)
return model
ROBOT_DATA = ROBOT_MODEL.createData()
# Create differential action model and data; link the contact data
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE)
IMPULSES = crocoddyl.ImpulseModelMultiple(ROBOT_STATE)
IMPULSE_6D = crocoddyl.ImpulseModel6D(ROBOT_STATE,
ROBOT_MODEL.getFrameId('r_sole'))
IMPULSE_3D = crocoddyl.ImpulseModel3D(ROBOT_STATE,
ROBOT_MODEL.getFrameId('l_sole'))
IMPULSES.addImpulse("r_sole_impulse", IMPULSE_6D)
IMPULSES.addImpulse("l_sole_impulse", IMPULSE_3D)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, 0)
COSTS.addCost("impulse_com", crocoddyl.CostModelImpulseCoM(ROBOT_STATE), 1.)
MODEL = crocoddyl.ActionModelImpulseFwdDynamics(ROBOT_STATE, IMPULSES, COSTS,
0., 0., True)
DATA = MODEL.createData()
# Created DAM numdiff
MODEL_ND = crocoddyl.ActionModelNumDiff(MODEL)
MODEL_ND.disturbance *= 10
dnum = MODEL_ND.createData()
x = ROBOT_STATE.rand()
u = pinocchio.utils.rand(0) # (ACTUATION.nu)
MODEL.calc(DATA, x, u)
MODEL.calcDiff(DATA, x, u)
MODEL_ND.calc(dnum, x, u)
MODEL_ND.calcDiff(dnum, x, u)
assertNumDiff(
DATA.Fx, dnum.Fx, NUMDIFF_MODIFIER * MODEL_ND.disturbance
