class Impulse3DMultipleTest(ImpulseModelMultipleAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
gains = pinocchio.utils.rand(2)
IMPULSES = collections.OrderedDict(
sorted({
'lf_foot': crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('lf_foot')),
'rh_foot': crocoddyl.ImpulseModel3D(ROBOT_STATE, ROBOT_MODEL.getFrameId('rh_foot'))
}.items()))
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
class Impulse3DTest(ImpulseModelAbstractTestCase):
ROBOT_MODEL = example_robot_data.load('hyq').model
ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL)
# gains = pinocchio.utils.rand(2)
frame = ROBOT_MODEL.getFrameId('lf_foot')
IMPULSE = crocoddyl.ImpulseModel3D(ROBOT_STATE, frame)
IMPULSE_DER = Impulse3DModelDerived(ROBOT_STATE, frame)
import pinocchio
import example_robot_data
from test_utils import NUMDIFF_MODIFIER, assertNumDiff
# 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)
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()
