Creating a core class (or varios) that formulates contact robot problems
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.