unittest/python/test_armature.py
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2):
    '''
    Creating the action model for floating-base systems. A walker system
    is by default a floating-base system.
    contactIds is a list of frame Ids of points that should be in contact.
    effectors is a dict of key frame ids and SE3 values of effector references.
    '''
    actModel = ActuationModelFreeFloating(rmodel)
    State = StatePinocchio(rmodel)

    # Creating a 6D multi-contact model, and then including the supporting foot
    contactModel = ContactModelMultiple(rmodel)
    for cid in contactIds:
        contactModel.addContact('contact%d' % cid, ContactModel6D(rmodel, cid, ref=None))

    # Creating the cost model for a contact phase
    costModel = CostModelSum(rmodel, actModel.nu)
    wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
    costModel.addCost('xreg',
                      weight=1e-1,
                      cost=CostModelState(rmodel,
                                          State,
                                          ref=rmodel.defaultState,
                                          nu=actModel.nu,
                                          activation=ActivationModelWeightedQuad(wx)))
    costModel.addCost('ureg', weight=1e-4, cost=CostModelControl(rmodel, nu=actModel.nu))
    for fid, ref in effectors.items():
        if not isinstance(ref, SE3):
            ref = SE3(eye(3), a2m(ref))
        costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, actModel.nu))

    if com is not None:
        costModel.addCost("com", weight=100., cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu))

    # Creating the action model for the KKT dynamics with simpletic Euler
    # integration scheme
    dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, costModel)
    model = IntegratedActionModelEuler(dmodel)
    model.timeStep = integrationStep
    return model

    Creating the action model for floating-base systems. A walker system
    is by default a floating-base system.
    contactIds is a list of frame Ids of points that should be in contact.
    effectors is a dict of key frame ids and SE3 values of effector references.
    
Creating the action model for floating-base systems. A walker system
is by default a floating-base system.
contactIds is a list of frame Ids of points that should be in contact.
effectors is a dict of key frame ids and SE3 values of effector references.
Creating the action model for floating - base systems . A walker system is by default a floating - base system . contactIds is a list of frame Ids of points that should be in contact . effectors is a dict of key frame ids and SE3 values of effector references .
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2):
    actModel = ActuationModelFreeFloating(rmodel)
    State = StatePinocchio(rmodel)
    contactModel = ContactModelMultiple(rmodel)
    for cid in contactIds:
        contactModel.addContact('contact%d' % cid, ContactModel6D(rmodel, cid, ref=None))
    costModel = CostModelSum(rmodel, actModel.nu)
    wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv)
    costModel.addCost('xreg',
                      weight=1e-1,
                      cost=CostModelState(rmodel,
                                          State,
                                          ref=rmodel.defaultState,
                                          nu=actModel.nu,
                                          activation=ActivationModelWeightedQuad(wx)))
    costModel.addCost('ureg', weight=1e-4, cost=CostModelControl(rmodel, nu=actModel.nu))
    for fid, ref in effectors.items():
        if not isinstance(ref, SE3):
            ref = SE3(eye(3), a2m(ref))
        costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, actModel.nu))
    if com is not None:
        costModel.addCost("com", weight=100., cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu))
    dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, costModel)
    model = IntegratedActionModelEuler(dmodel)
    model.timeStep = integrationStep
    return model
def runningModel ( contactIds , effectors , com = None , integrationStep = 1e-2 ) : actModel = ActuationModelFreeFloating ( rmodel ) State = StatePinocchio ( rmodel ) contactModel = ContactModelMultiple ( rmodel ) for cid in contactIds : contactModel . addContact ( 'contact%d' % cid , ContactModel6D ( rmodel , cid , ref = None ) ) costModel = CostModelSum ( rmodel , actModel . nu ) wx = np . array ( [ 0 ] * 6 + [ .1 ] * ( rmodel . nv - 6 ) + [ 10 ] * rmodel . nv ) costModel . addCost ( 'xreg' , weight = 1e-1 , cost = CostModelState ( rmodel , State , ref = rmodel . defaultState , nu = actModel . nu , activation = ActivationModelWeightedQuad ( wx ) ) ) costModel . addCost ( 'ureg' , weight = 1e-4 , cost = CostModelControl ( rmodel , nu = actModel . nu ) ) for fid , ref in effectors . items ( ) : if not isinstance ( ref , SE3 ) : ref = SE3 ( eye ( 3 ) , a2m ( ref ) ) costModel . addCost ( "track%d" % fid , weight = 100. , cost = CostModelFramePlacement ( rmodel , fid , ref , actModel . nu ) ) if com is not None : costModel . addCost ( "com" , weight = 100. , cost = CostModelCoM ( rmodel , ref = com , nu = actModel . nu ) ) dmodel = DifferentialActionModelFloatingInContact ( rmodel , actModel , contactModel , costModel ) model = IntegratedActionModelEuler ( dmodel ) model . timeStep = integrationStep return model
Creating the action model for floating-base systems.
Creating the action model for floating - base systems .
'''
    Creating the action model for floating-base systems. A walker system
    is by default a floating-base system.
    contactIds is a list of frame Ids of points that should be in contact.
    effectors is a dict of key frame ids and SE3 values of effector references.
    ''' # Creating a 6D multi-contact model, and then including the supporting foot # Creating the cost model for a contact phase # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme
contactIds
effectors
com
integrationStep