class CoMPositionCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelResidual(ROBOT_STATE, crocoddyl.ResidualModelCoMPosition(ROBOT_STATE, cref)) COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv)**2) xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual) uRegCost = crocoddyl.CostModelResidual(state, uResidual) xRegTermCost = crocoddyl.CostModelResidual(state, xTActivation, xResidual) # Cost for target reaching framePlacementResidual = crocoddyl.ResidualModelFramePlacement( state, endEffectorId, pinocchio.SE3(np.eye(3), target), actuation.nu) framePlacementActivation = crocoddyl.ActivationModelWeightedQuad( np.array([1] * 3 + [0.0001] * 3)**2) goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementActivation, framePlacementResidual) # Cost for CoM reference comResidual = crocoddyl.ResidualModelCoMPosition(state, comRef, actuation.nu) comTrack = crocoddyl.CostModelResidual(state, comResidual) # Create cost model per each action model runningCostModel = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) # Then let's added the running and terminal cost functions runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2) runningCostModel.addCost("stateReg", xRegCost, 1e-3) runningCostModel.addCost("ctrlReg", uRegCost, 1e-4) runningCostModel.addCost("limitCost", limitCost, 1e3) terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2) terminalCostModel.addCost("stateReg", xRegTermCost, 1e-3) terminalCostModel.addCost("limitCost", limitCost, 1e3)
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 nu = self.actuation.nu contactModel = crocoddyl.ContactModelMultiple(self.state, nu) for i in supportFootIds: supportContactModel = \ crocoddyl.ContactModel6D(self.state, i, pinocchio.SE3.Identity(), nu, np.array([0., 0.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, nu) if isinstance(comTask, np.ndarray): comResidual = crocoddyl.ResidualModelCoMPosition( self.state, comTask, nu) comTrack = crocoddyl.CostModelResidual(self.state, comResidual) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.WrenchCone(self.Rsurf, self.mu, np.array([0.1, 0.05])) wrenchResidual = crocoddyl.ResidualModelContactWrenchCone( self.state, i, cone, nu) wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)) wrenchCone = crocoddyl.CostModelResidual(self.state, wrenchActivation, wrenchResidual) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1) if swingFootTask is not None: for i in swingFootTask: framePlacementResidual = crocoddyl.ResidualModelFramePlacement( self.state, i[0], i[1], nu) footTrack = crocoddyl.CostModelResidual( self.state, framePlacementResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu) stateActivation = crocoddyl.ActivationModelWeightedQuad( stateWeights**2) ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual) 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
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 nu = self.actuation.nu contactModel = crocoddyl.ContactModelMultiple(self.state, nu) for i in supportFootIds: supportContactModel = crocoddyl.ContactModel3D( self.state, i, np.array([0., 0., 0.]), 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, nu) if isinstance(comTask, np.ndarray): comResidual = crocoddyl.ResidualModelCoMPosition( self.state, comTask, nu) comTrack = crocoddyl.CostModelResidual(self.state, comResidual) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False) coneResidual = crocoddyl.ResidualModelContactFrictionCone( self.state, i, cone, nu) coneActivation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)) frictionCone = crocoddyl.CostModelResidual(self.state, coneActivation, coneResidual) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation( self.state, i[0], i[1].translation, nu) footTrack = crocoddyl.CostModelResidual( self.state, frameTranslationResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu) stateActivation = crocoddyl.ActivationModelWeightedQuad( stateWeights**2) ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual) 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:] ]) stateBoundsResidual = crocoddyl.ResidualModelState(self.state, nu) stateBoundsActivation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(lb, ub)) stateBounds = crocoddyl.CostModelResidual(self.state, stateBoundsActivation, stateBoundsResidual) 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