Creating a core class (or varios) that formulates contact robot problems

[wxmerkt]

In GitLab by @cmastalli on Mar 2, 2019, 09:42

Along crocoddyl, we use these formulation for integrative unit-testing, examples and apps. We have duplicated plenty of times lines of code like this:

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():
        costModel.addCost("track%d"%fid, weight=100.,
                          cost = CostModelFramePlacement(rmodel,fid,ref,actModel.nu))

    if com is not None:
        costModel.addCost("com", weight=10000.,
                          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

which curiously we use the same gains for biped and quadruped examples.

I detected this problem quite time ago and I decide to create a temporal class for it, i.e. SimpleQuadrupedalWalkingProblem.

In this issue, I propose to pursue this idea by improving this class and moving to crocoddyl. Later we should replace progressively every piece that formulate robot contact problems.

[wxmerkt]

In GitLab by @cmastalli on Mar 2, 2019, 09:43

@nmansard and @proyan please take time to think about this point.

[wxmerkt]

In GitLab by @cmastalli on Mar 2, 2019, 09:44

mentioned in merge request !120

[wxmerkt]

In GitLab by @nmansard on Mar 7, 2019, 12:09

This is in connection with the locomotion module written by @proyan . We already have a class for describing locomotion problem, in the multicontactapi https://gepgitlab.laas.fr/loco-3d/multicontact-api/ . We need to use it to create the DDP problem automatically. A first example already exists in @proyan locomotion module. The work is to make it stronger. The first part of this work is to clean and re-structure the MCAPI, which we are discussing with the HPP group. I keep you in touch of that.

For Crocoddyl, the first step in that direction is to move the locomotion module back in the lib. Related to #115

[wxmerkt]

In GitLab by @nmansard on Mar 7, 2019, 12:09

closed