class FrameVelocityCostSumTest(CostModelSumTestCase): ROBOT_MODEL = example_robot_data.load('icub_reduced').model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
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 swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # 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.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, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.WrenchCone(np.identity(3), self.mu, np.array([0.1, 0.05])) wrenchCone = crocoddyl.CostModelContactWrenchCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameWrenchCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 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.id].name + "_footTrack", footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.id, pinocchio.Motion.Zero()) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, footVel, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.id].name + "_impulseVel", impulseFootVelCost, 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(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
class FrameVelocityCostSumTest(CostModelSumTestCase): ROBOT_MODEL = pinocchio.buildSampleModelHumanoidRandom() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('rleg5_joint'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref)
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
class FrameVelocityCostTest(CostModelAbstractTestCase): ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) vref = crocoddyl.FrameMotion(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.Motion.Random()) COST = crocoddyl.CostModelFrameVelocity(ROBOT_STATE, vref) COST_DER = FrameVelocityCostModelDerived(ROBOT_STATE, vref=vref)
def createFootSwitchModel(self, supportFootIds, swingFootTask): """ Action model for a foot switch phase. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return action model for a foot switch 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., 0.]).T) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) 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("footTrack_" + str(i), footTrack, 1e7) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.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-3) for i in swingFootTask: vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6) # 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, 0.) return model
# ]) # target = np.array([np.sin(angle), 0, np.cos(angle)])) runningCostModel = crocoddyl.CostModelSum(state, actuation.nu) terminalCostModel = crocoddyl.CostModelSum(state, actuation.nu) target = np.array(conf.target) footName = 'foot' footFrameID = robot_model.getFrameId(footName) assert (robot_model.existFrame(footName)) Pref = crocoddyl.FrameTranslation(footFrameID, target) # If also the orientation is useful for the task use # Mref = crocoddyl.FramePlacement(footFrameID, # pinocchio.SE3(R, conf.n_links * np.matrix([[np.sin(angle)], [0], [np.cos(angle)]]))) footTrackingCost = crocoddyl.CostModelFrameTranslation(state, Pref, actuation.nu) Vref = crocoddyl.FrameMotion(footFrameID, pinocchio.Motion(np.zeros(6))) footFinalVelocity = crocoddyl.CostModelFrameVelocity(state, Vref, actuation.nu) # simulating the cost on the power with a cost on the control power_act = crocoddyl.ActivationModelQuad(conf.n_links) u2 = crocoddyl.CostModelControl( state, power_act, actuation.nu) # joule dissipation cost without friction, for benchmarking stateAct = crocoddyl.ActivationModelWeightedQuad( np.concatenate([np.zeros(state.nq + 1), np.ones(state.nv - 1)])) v2 = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx), actuation.nu) joint_friction = CostModelJointFrictionSmooth(state, power_act, actuation.nu) joule_dissipation = CostModelJouleDissipation(state, power_act, actuation.nu) # PENALIZATIONS bounds = crocoddyl.ActivationBounds(