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.CostModelCoMPosition(ROBOT_STATE, cref) COST_DER = CoMPositionCostModelDerived(ROBOT_STATE, cref=cref)
class CoMPositionCostTest(CostModelAbstractTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref) COST_DER = CoMPositionCostDerived(ROBOT_STATE, cref=cref)
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.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).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)), cone, i, 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) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.frame].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(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) lb = self.state.diff(0 * self.state.lb, self.state.lb) ub = self.state.diff(0 * self.state.ub, self.state.ub) 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) 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) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
actuation.nu) footTrackingWeights = np.array([1, 1, 0.1] + [1.] * 3) Pref = crocoddyl.FramePlacement( leftFootId, pinocchio.SE3(np.eye(3), np.array([0., 0.4, 0.]))) footTrackingCost1 = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref, actuation.nu) Pref = crocoddyl.FramePlacement( leftFootId, pinocchio.SE3(np.eye(3), np.array([0.3, 0.15, 0.35]))) footTrackingCost2 = crocoddyl.CostModelFramePlacement( state, crocoddyl.ActivationModelWeightedQuad(footTrackingWeights**2), Pref, actuation.nu) # Cost for CoM reference comTrack = crocoddyl.CostModelCoMPosition(state, comRef, actuation.nu) # Create cost model per each action model. We divide the motion in 3 phases plus its terminal model runningCostModel1 = crocoddyl.CostModelSum(state, actuation.nu) runningCostModel2 = crocoddyl.CostModelSum(state, actuation.nu) runningCostModel3 = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) # Then let's added the running and terminal cost functions runningCostModel1.addCost("gripperPose", handTrackingCost, 1e2) runningCostModel1.addCost("stateReg", xRegCost, 1e-3) runningCostModel1.addCost("ctrlReg", uRegCost, 1e-4) runningCostModel1.addCost("limitCost", limitCost, 1e3) runningCostModel2.addCost("gripperPose", handTrackingCost, 1e2) runningCostModel2.addCost("footPose", footTrackingCost1, 1e1)
class CoMPositionCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) cref = pinocchio.utils.rand(3) COST = crocoddyl.CostModelCoMPosition(ROBOT_STATE, cref)