class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('icub_reduced').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class CoMPositionCostTest(CostModelAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref)
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
actuation.nu)
footTrackingWeights = np.array([1, 1, 0.1] + [1.] * 3)
Pref = crocoddyl.FramePlacement(
leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.])))
footTrackingCost1 = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref,
actuation.nu)
Pref = crocoddyl.FramePlacement(
leftFootId, pinocchio.SE3(np.eye(3), np.array([0.3, 0.15, 0.35])))
footTrackingCost2 = crocoddyl.CostModelFramePlacement(
state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref,
actuation.nu)
# Cost for CoM reference
comTrack = crocoddyl.CostModelCoMPosition(state, comRef, actuation.nu)
# Create cost model per each action model. We divide the motion in 3 phases plus its terminal model
runningCostModel1 = crocoddyl.CostModelSum(state, actuation.nu)
runningCostModel2 = crocoddyl.CostModelSum(state, actuation.nu)
runningCostModel3 = crocoddyl.CostModelSum(state, actuation.nu)
terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu)
# Then let's added the running and terminal cost functions
runningCostModel1.addCost("gripperPose", handTrackingCost, 1e2)
runningCostModel1.addCost("stateReg", xRegCost, 1e-3)
runningCostModel1.addCost("ctrlReg", uRegCost, 1e-4)
runningCostModel1.addCost("limitCost", limitCost, 1e3)
runningCostModel2.addCost("gripperPose", handTrackingCost, 1e2)
runningCostModel2.addCost("footPose", footTrackingCost1, 1e1)
class CoMPositionCostSumTest(CostModelSumTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
cref = pinocchio.utils.rand(3)
COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)
