cone friction constraint for six-legged robot

[Wjzsearcher]

Hi! Thanks for this wonderful library.

• I have a question here:

I want to use the crocoddyl to control a six-legged robot (per leg with three joints). I modified the trotting demo for quadruped by only adding extra supporting legs. But the results seemed very bad (the displaying motion was terrible) with higher cost value and longer iterative time (almost 100 iter). The cost weight, code structure and others are the same as the trotting demo, except supporting legs number. The urdf of my robot is correct at all.

• Things got interesting when I modified the cone friction constraint in the code.

If I reduced the number of cone fricition constraint for the cost (e.g., reduce 6 cone friction constraint to 5 cone friction constraint in doubleSupport phase), the results was compute quickly as the demo (10-14 iter) and the displaying motion seemed reasonable.

Although this way is absolutely wrong for physical realism, I wonder why adding just two extra legs compared to a quadruped would result in such a different result, even 5 cone friction constraints can get feasible result but 6 cone friction constraints cannot.

Is there any way to solve this problem?

I really appreciate the great work, thanks

[cmastalli]

Hi @Wjzsearcher,

It is not obvious for us to understand the issues that you are encountered. From a theoretical point of view, you shouldn't have a deprived convergence rate for robots with 6 legs with 6 friction cones.

I suspect you might have wrongly setup your problem. For instance, you could have an over-constrained OC problem.

It could be useful if you provide a simple script to reproduce.

[Wjzsearcher]

Hi, thanks for your reply. I went back and looked at the code. I just modified the __init__ function and createTrottingProblem function in class SimpleQuadrupedalGaitProblem.

In the __init__ function, I just added two extra legs to initialize foot id. And the createTrottingProblem function was modifed as follow:

def createTrottingProblem(self, x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots):
        q0 = x0[:self.state.nq]
        pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
        pinocchio.updateFramePlacements(self.rmodel, self.rdata)
        rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
        rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
        rmFootPos0 = self.rdata.oMf[self.rmFootId].translation
        lmFootPos0 = self.rdata.oMf[self.lmFootId].translation  # add two extra legs
        lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
        lhFootPos0 = self.rdata.oMf[self.lhFootId].translation

        comRef = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
        # Defining the action models along the time instances
        loco3dModel = []

        doubleSupport = [
            self.createSwingFootModel(
                timeStep, [self.lfFootId, self.rfFootId,
                           self.lhFootId, self.rhFootId,
                           self.lmFootId, self.rmFootId]) # add two extra legs
            for k in range(supportKnots)
        ]

        if self.firstStep is True:
            rf_lm_rhStep = self.createFootstepModels(comRef, [rfFootPos0, lmFootPos0, rhFootPos0],
                                                     0.5 * stepLength, stepHeight, timeStep, stepKnots,
                                                     [self.lfFootId, self.rmFootId,
                                                         self.lhFootId],
                                                     [self.rfFootId, self.lmFootId, self.rhFootId]) # add two extra legs
            self.firstStep = False
        else:
            rf_lm_rhStep = self.createFootstepModels(comRef, [rfFootPos0, lmFootPos0, rhFootPos0],
                                                     stepLength, stepHeight, timeStep, stepKnots,
                                                     [self.lfFootId, self.rmFootId,
                                                         self.lhFootId],
                                                     [self.rfFootId, self.lmFootId, self.rhFootId]) # add two extra legs

        lf_rm_lhStep = self.createFootstepModels(comRef, [lfFootPos0, rmFootPos0, lhFootPos0],
                                                 stepLength, stepHeight, timeStep, stepKnots,
                                                 [self.rfFootId, self.lmFootId,
                                                     self.rhFootId],
                                                 [self.lfFootId, self.rmFootId, self.lhFootId]) # add two extra legs

        loco3dModel += doubleSupport + rf_lm_rhStep
        loco3dModel += doubleSupport + lf_rm_lhStep

        problem = crocoddyl.ShootingProblem(
            x0, loco3dModel[:-1], loco3dModel[-1])
        return problem

As you can see, I just added two extra legs in supportFootIds. For the swingFootTask, the swing motion displayed correct, so I thought the urdf was no problem. But I had a deprived convergence rate if there were 6 friction cone constraints. If the friction cone constraints lower than 6 (e.g.,5 or 4), it could converge.

So is there anything wrong with the problem setup?