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 supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.])) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.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, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e7) costModel.addCost( self.rmodel.frames[i.frame].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.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
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 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.])) supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.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, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.id, i.placement.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) 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-1) lb = np.concatenate([self.state.lb[1:self.state.nv + 1], self.state.lb[-self.state.nv:]]) ub = np.concatenate([self.state.ub[1:self.state.nv + 1], self.state.ub[-self.state.nv:]]) stateBounds = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)), 0 * self.rmodel.defaultState, self.actuation.nu) costModel.addCost("stateBounds", stateBounds, 1e3) # 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
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) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 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.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, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
ROBOT_STATE, crocoddyl.FrameTranslation(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random().translation), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D) CONTACTS.addContact("l_sole_contact", CONTACT_3D) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu) frictionCone = crocoddyl.FrictionCone(np.matrix([0., 0., 1.]).T, 0.7, 4, False) activation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(frictionCone.lb, frictionCone.ub)) COSTS.addCost( "r_sole_friction_cone", crocoddyl.CostModelContactFrictionCone( ROBOT_STATE, activation, crocoddyl.FrameFrictionCone(ROBOT_MODEL.getFrameId('r_sole'), frictionCone), ACTUATION.nu), 1.) COSTS.addCost( "l_sole_friction_cone", crocoddyl.CostModelContactFrictionCone( ROBOT_STATE, activation, crocoddyl.FrameFrictionCone(ROBOT_MODEL.getFrameId('l_sole'), frictionCone), ACTUATION.nu), 1.) MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics( ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True) DATA = MODEL.createData() # Created DAM numdiff MODEL_ND = crocoddyl.DifferentialActionModelNumDiff(MODEL) MODEL_ND.disturbance *= 10 dnum = MODEL_ND.createData()
np.array([0., 1 / dt])) # makes the velocity drift disappear in one timestep contactModel.addContact("foot_contact", supportContactModel) # FRICTION CONE # the friction cone can also have the [min, maximum] force parameters # 4 is the number of faces for the approximation mu = 0.7 normalDirection = np.array([0, 0, 1]) minForce = 0 maxForce = 200 cone = crocoddyl.FrictionCone(normalDirection, mu, 4, True, minForce, maxForce) coneBounds = crocoddyl.ActivationBounds(cone.lb, cone.ub) coneActivation = crocoddyl.ActivationModelWeightedQuadraticBarrier( coneBounds, np.array([1, 1, 0, 0])) frameFriction = crocoddyl.FrameFrictionCone(footFrameID, cone) frictionCone = crocoddyl.CostModelContactFrictionCone(state, coneActivation, frameFriction, actuation.nu) # Creating the action model for the KKT dynamics with simpletic Euler integration scheme contactCostModel = crocoddyl.CostModelSum(state, actuation.nu) # contactCostModel.addCost('frictionCone', frictionCone, 1e-6) contactCostModel.addCost('joule_dissipation', joule_dissipation, 5e-3) contactCostModel.addCost('joint_friction', joint_friction, 5e-3) contactCostModel.addCost('velocityRegularization', v2, 1e-1) contactCostModel.addCost('nonPenetration', nonPenetration, 1e5) contactDifferentialModel = crocoddyl.DifferentialActionModelContactFwdDynamics( state, actuation, contactModel,