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 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
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.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).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)),
cone, i, 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)
footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu)
costModel.addCost(self.rmodel.frames[i.frame].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(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)
lb = self.state.diff(0 * self.state.lb, self.state.lb)
ub = self.state.diff(0 * self.state.ub, self.state.ub)
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)
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)
model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep)
return model
def createFootSwitchModel(self, supportFootIds, swingFootTask):
""" Action model for a foot switch phase.
:param supportFootIds: Ids of the constrained feet
:param swingFootTask: swinging foot task
:return action model for a foot switch 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.matrix([0., 0., 0.]).T)
supportContactModel = crocoddyl.ContactModel3D(
self.state, xref, self.actuation.nu,
np.matrix([0., 0.]).T)
contactModel.addContact('contact_' + str(i), supportContactModel)
# Creating the cost model for a contact phase
costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu)
if swingFootTask is not None:
for i in swingFootTask:
xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation)
footTrack = crocoddyl.CostModelFrameTranslation(
self.state, xref, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e7)
stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] *
(self.rmodel.nv - 6) + [10] * self.rmodel.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-3)
for i in swingFootTask:
vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
impactFootVelCost = crocoddyl.CostModelFrameVelocity(
self.state, vref, self.actuation.nu)
costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost,
1e6)
# Creating the action model for the KKT dynamics with simpletic Euler
# integration scheme
dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, self.actuation, contactModel, costModel)
model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.)
return model
def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.contacts = crocoddyl.ContactModelMultiple(self.ROBOT_STATE)
self.contacts.addContact("myContact", self.CONTACT)
self.data = self.CONTACT.createData(self.robot_data)
self.data_multiple = self.contacts.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
def setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.contactSum = crocoddyl.ContactModelMultiple(self.ROBOT_STATE)
self.datas = collections.OrderedDict(
[[name, contact.createData(self.robot_data)]
for name, contact in self.CONTACTS.items()])
for name, contact in self.CONTACTS.items():
self.contactSum.addContact(name, contact)
self.dataSum = self.contactSum.createData(self.robot_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data,
self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data)
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL,
self.robot_data,
self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
import pinocchio
import example_robot_data
import numpy as np
from test_utils import NUMDIFF_MODIFIER, assertNumDiff
crocoddyl.switchToNumpyMatrix()
# Create robot model and data
ROBOT_MODEL = example_robot_data.loadICub().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)
CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu)
CONTACT_6D_1 = crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random()), ACTUATION.nu,
pinocchio.utils.rand(2))
CONTACT_6D_2 = crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random()), ACTUATION.nu,
pinocchio.utils.rand(2))
CONTACTS.addContact("r_sole_contact", CONTACT_6D_1)
CONTACTS.addContact("l_sole_contact", CONTACT_6D_2)
COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, False)
COSTS.addCost(
"force",
lfPos0 = rdata.oMf[leftFootId].translation
refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation
comRef = (rfPos0 + lfPos0) / 2
comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item()
# Initialize Gepetto viewer
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
display.robot.viewer.gui.addSphere(
'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001
display.robot.viewer.gui.applyConfiguration(
'world/point',
target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion
# Add contact to the model
contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu)
supportContactModelLeft = crocoddyl.ContactModel6D(state, leftFootId,
pinocchio.SE3.Identity(),
actuation.nu,
np.array([0, 0]))
contactModel.addContact(leftFoot + "_contact", supportContactModelLeft)
supportContactModelRight = crocoddyl.ContactModel6D(state, rightFootId,
pinocchio.SE3.Identity(),
actuation.nu,
np.array([0, 0]))
contactModel.addContact(rightFoot + "_contact", supportContactModelRight)
contactData = contactModel.createData(rdata)
# Cost for self-collision
maxfloat = sys.float_info.max
xlb = np.concatenate([
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
lfPos0 = rdata.oMf[leftFootId].translation
refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation
comRef = (rfPos0 + lfPos0) / 2
comRef[2] = np.asscalar(pinocchio.centerOfMass(rmodel, rdata, q0)[2])
# Initialize Gepetto viewer
if WITHDISPLAY:
display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot])
display.robot.viewer.gui.addSphere(
'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001
display.robot.viewer.gui.applyConfiguration(
'world/point',
target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion
# Create two contact models used along the motion
contactModel1Foot = crocoddyl.ContactModelMultiple(state, actuation.nu)
contactModel2Feet = crocoddyl.ContactModelMultiple(state, actuation.nu)
framePlacementLeft = crocoddyl.FramePlacement(leftFootId,
pinocchio.SE3.Identity())
framePlacementRight = crocoddyl.FramePlacement(rightFootId,
pinocchio.SE3.Identity())
supportContactModelLeft = crocoddyl.ContactModel6D(state, framePlacementLeft,
actuation.nu,
np.array([0, 40]))
supportContactModelRight = crocoddyl.ContactModel6D(state, framePlacementRight,
actuation.nu,
np.array([0, 40]))
contactModel1Foot.addContact(rightFoot + "_contact", supportContactModelRight)
contactModel2Feet.addContact(leftFoot + "_contact", supportContactModelLeft)
contactModel2Feet.addContact(rightFoot + "_contact", supportContactModelRight)
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
