class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom()
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.SE3.Random())
CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
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
class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
CONTACTS = collections.OrderedDict(
sorted({
'l_foot':
crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'),
pinocchio.SE3.Random()), gains),
'r_foot':
crocoddyl.ContactModel6D(
ROBOT_STATE,
crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random()), gains)
}.items()))
class Contact6DTest(ContactModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.loadICub().model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
Mref = crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'),
pinocchio.SE3.Random())
CONTACT = crocoddyl.ContactModel6D(ROBOT_STATE, Mref, gains)
CONTACT_DER = Contact6DDerived(ROBOT_STATE, Mref, gains)
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 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('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:
footTrack = crocoddyl.CostModelFramePlacement(self.state, i, self.actuation.nu)
costModel.addCost("footTrack_" + str(i), footTrack, 1e8)
footVel = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero())
impactFootVelCost = crocoddyl.CostModelFrameVelocity(self.state, footVel, self.actuation.nu)
costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 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-3)
# 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
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",
crocoddyl.CostModelContactForce(
ROBOT_STATE,
crocoddyl.FrameForce(ROBOT_MODEL.getFrameId('r_sole'),
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([
-maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.lowerPositionLimit[7:],
-maxfloat * np.ones(state.nv)
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
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)
# Cost for self-collision
maxfloat = sys.float_info.max
xlb = np.concatenate([
-maxfloat * np.ones(6), # dimension of the SE(3) manifold
rmodel.lowerPositionLimit[7:],
-maxfloat * np.ones(state.nv)
])
# 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)
framePlacementLeft = crocoddyl.FramePlacement(leftFootId,
pinocchio.SE3.Identity())
supportContactModelLeft = crocoddyl.ContactModel6D(state, framePlacementLeft,
actuation.nu,
np.matrix([0, 0]).T)
contactModel.addContact(leftFoot + "_contact", supportContactModelLeft)
framePlacementRight = crocoddyl.FramePlacement(rightFootId,
pinocchio.SE3.Identity())
supportContactModelRight = crocoddyl.ContactModel6D(state, framePlacementRight,
actuation.nu,
np.matrix([0, 0]).T)
contactModel.addContact(rightFoot + "_contact", supportContactModelRight)
contactData = contactModel.createData(rdata)
# Cost for self-collision
maxfloat = sys.float_info.max
xlb = np.vstack([
-maxfloat * np.matrix(np.ones((6, 1))), # dimension of the SE(3) manifold
rmodel.lowerPositionLimit[7:],
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
