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()