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
def setUp(self): self.x = self.ROBOT_STATE.rand() self.robot_data = self.ROBOT_MODEL.createData() self.impulseSum = crocoddyl.ImpulseModelMultiple(self.ROBOT_STATE) self.datas = collections.OrderedDict([[name, impulse.createData(self.robot_data)] for name, impulse in self.IMPULSES.items()]) for name, impulse in self.IMPULSES.items(): self.impulseSum.addImpulse(name, impulse) self.dataSum = self.impulseSum.createData(self.robot_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv))
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: frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation( self.state, i[0], i[1].translation, 0) footTrack = crocoddyl.CostModelResidual( self.state, frameTranslationResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e8) stateWeights = np.array([1.] * 6 + [0.1] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, 0) stateActivation = crocoddyl.ActivationModelWeightedQuad( stateWeights**2) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) 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()