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 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
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 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) 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) frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(self.state, i[0], pinocchio.Motion.Zero(), pinocchio.LOCAL, nu) footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual) impulseFootVelCost = crocoddyl.CostModelResidual(self.state, frameVelocityResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e7) costModel.addCost(self.rmodel.frames[i[0]].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.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-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) if self.integrator == 'euler': model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) elif self.integrator == 'rk4': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.four, 0.) elif self.integrator == 'rk3': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.three, 0.) elif self.integrator == 'rk2': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.) else: model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
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 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) 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.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, 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-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
import crocoddyl import pinocchio import example_robot_data import numpy as np from test_utils import NUMDIFF_MODIFIER, assertNumDiff # Create robot model, data, state and actuation ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) # Create friction cone and its activation activation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(np.array([0] * 4), np.array([np.inf] * 4))) # Contact CoP position cost unittest CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu) CONTACT_6D_RF = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_6D_LF = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D_RF) CONTACTS.addContact("l_sole_contact", CONTACT_6D_LF)
contactModel.addContact(rightFoot + "_contact", supportContactModelRight) contactData = contactModel.createData(rdata) # Cost for self-collision maxfloat = sys.float_info.max xlb = np.concatenate([ -maxfloat * np.ones(6), # dimension of the SE(3) manifold rmodel.lowerPositionLimit[7:], -maxfloat * np.ones(state.nv) ]) xub = np.concatenate([ maxfloat * np.ones(6), # dimension of the SE(3) manifold rmodel.upperPositionLimit[7:], maxfloat * np.ones(state.nv) ]) bounds = crocoddyl.ActivationBounds(xlb, xub, 1.) xLimitResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu) xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds) limitCost = crocoddyl.CostModelResidual(state, xLimitActivation, xLimitResidual) # Cost for state and control xResidual = crocoddyl.ResidualModelState(state, x0, actuation.nu) xActivation = crocoddyl.ActivationModelWeightedQuad( np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [10] * state.nv)**2) uResidual = crocoddyl.ResidualModelControl(state, actuation.nu) xTActivation = crocoddyl.ActivationModelWeightedQuad( np.array([0] * 3 + [10.] * 3 + [0.01] * (state.nv - 6) + [100] * state.nv)**2) xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_3D = crocoddyl.ContactModel3D( 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, True) 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, frictionCone, ROBOT_MODEL.getFrameId('r_sole'), ACTUATION.nu), 1.) COSTS.addCost( "l_sole_friction_cone", crocoddyl.CostModelContactFrictionCone(ROBOT_STATE, activation, frictionCone, ROBOT_MODEL.getFrameId('l_sole'), ACTUATION.nu), 1.) MODEL = crocoddyl.DifferentialActionModelContactFwdDynamics( ROBOT_STATE, ACTUATION, CONTACTS, COSTS, 0., True) DATA = MODEL.createData()
def __init__(self, nx=12, nu=12, mu=0.8, log=True): crocoddyl.ActionModelAbstract.__init__(self, crocoddyl.StateVector(nx), nu, 1) #:param state: state description, #:param nu: dimension of control vector, #:param nr: dimension of the cost-residual vector (default 1) # Time step of the solver self.dt = 0.02 # Mass of the robot self.mass = 2.97784899 # Inertia matrix of the robot in body frame (found in urdf) self.gI = np.diag([0.00578574, 0.01938108, 0.02476124]) # Friction coefficient self.mu = mu # Matrix A -- X_k+1 = AX + BU -- self.A = np.zeros((12, 12)) # Matrix B self.B = np.zeros((12, 12)) # Vector xref self.xref = np.zeros(12) # Vector g self.g = np.zeros((12, )) # Weight vector for the state self.stateWeight = np.full(12, 2) # Weight on the friction cost self.frictionWeight = 0.1 # Weight on the command forces self.weightForces = np.full(12, 0.1) # Levers Arms used in B self.lever_arms = np.zeros((3, 4)) # Initial position of footholds in the "straight standing" default configuration # footholds = [[ px_foot1 , px_foot2 ... ] , # [ py_foot1 , py_foot2 ... ] , # [ pz_foot1 , pz_foot2 ... ] ] 2D -- pz = 0 self.footholds = np.array([[0.19, 0.19, -0.19, -0.19], [0.15005, -0.15005, 0.15005, -0.15005], [0.0, 0.0, 0.0, 0.0]]) # S matrix # Represents the feet that are in contact with the ground such as : # S = [1 0 0 1] --> FL(Front Left) in contact, FR not , HL not , HR in contact self.S = np.ones(4) # List of the feet in contact with the ground used to compute B # if S = [1 0 0 1] ; L_feet = [1 1 1 0 0 0 0 0 0 1 1 1] self.L_feet = np.zeros(12) # Cone crocoddyl class R = np.identity(3) # Rotation matrix wrt inertial frame number_facets = 4 # Number of facets for cone approximation self.cone = crocoddyl.FrictionCone(R, self.mu, number_facets, False) self.lb = np.tile(self.cone.lb, 4) self.ub = np.tile(self.cone.ub, 4) # Matrice Fa of the cone class, repeated 4 times (4 feet) self.Fa = np.zeros((20, 12)) # Inequality activation model. # The activation is zero when r is between the lower (lb) and upper (ub) bounds, beta # determines how much of the total range is not activated. This is the activation # equations: # a(r) = 0.5 * ||r||^2 for lb < r < ub # a(r) = 0. for lb >= r >= ub. self.activation = crocoddyl.ActivationModelQuadraticBarrier( (crocoddyl.ActivationBounds(self.lb, self.ub))) self.dataCost = self.activation.createData() self.Lu_f = np.zeros(12) self.Luu_f = np.zeros((12, 12)) # Log parameters self.log = log self.log_cost_friction = [] self.log_cost_state = [] self.log_state = [] self.log_u = [] # Other dynamic models self.I_inv = np.zeros((3, 3)) self.derivative_B = np.zeros( (12, 12)) # Here lever arm is constant, not used
import pinocchio import example_robot_data import numpy as np from test_utils import NUMDIFF_MODIFIER, assertNumDiff # Create robot model, data, state and actuation ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) # Create wrench cone and its activation wrenchCone = crocoddyl.WrenchCone(np.identity(3), 0.7, np.array([0.1, 0.05])) activation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(wrenchCone.lb, wrenchCone.ub)) # Contact wrench-cone cost unittest CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu) CONTACT_6D_r = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_6D_l = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D_r)
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) if self.integrator == 'euler': model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) elif self.integrator == 'rk4': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.four, timeStep) elif self.integrator == 'rk3': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.three, timeStep) elif self.integrator == 'rk2': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.two, timeStep) else: model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) return model
# 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( np.concatenate([np.zeros(1), -1e3 * np.ones(state.nx - 1)]), 1e3 * np.ones(state.nx)) stateAct = crocoddyl.ActivationModelWeightedQuadraticBarrier( bounds, np.concatenate([np.ones(1), np.zeros(state.nx - 1)])) nonPenetration = crocoddyl.CostModelState(state, stateAct, np.zeros(state.nx), actuation.nu) # MAXIMIZATION jumpBounds = crocoddyl.ActivationBounds( -1e3 * np.ones(state.nx), np.concatenate([np.zeros(1), +1e3 * np.ones(state.nx - 1)])) jumpAct = crocoddyl.ActivationModelWeightedQuadraticBarrier( bounds, np.concatenate([-np.ones(1), np.zeros(state.nx - 1)])) maximizeJump = crocoddyl.CostModelState(state, jumpAct, np.ones(state.nx), actuation.nu)