class Impulse6DMultipleTest(ImpulseModelMultipleAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) gains = pinocchio.utils.rand(2) IMPULSES = collections.OrderedDict( sorted({ 'l_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('l_sole')), 'r_sole': crocoddyl.ImpulseModel6D(ROBOT_STATE, ROBOT_MODEL.getFrameId('r_sole')) }.items()))
class Impulse6DTest(ImpulseModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) frame = ROBOT_MODEL.getFrameId('r_sole') IMPULSE = crocoddyl.ImpulseModel6D(ROBOT_STATE, frame) IMPULSE_DER = Impulse6DModelDerived(ROBOT_STATE, frame)
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: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, 0) costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e8) stateWeights = np.array([1.] * 6 + [0.1] * (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) return model
import crocoddyl 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()