Question to add multiple point of contacts on each foot of bipedal robot

[abhirajsingh101]

I'm trying to add multiple points of contact for my bipedal robot in its each foot. For each foot I need 5 contacts (one in center and four others at its four corners). Although I was using center points on each foot earlier, i.e., a single point of contact for each foot, it was working fine. But if I add more than one point to each foot, it is not able to solve the problem.

     # add point of contact at center for support foot
    contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
    for i in supportFootIds:
        supportContactModel = crocoddyl.ContactModel6D(
            self.state,
            i,
            pin.SE3.Identity(),
            pin.LOCAL_WORLD_ALIGNED,
            nu,
            np.array([0.0, 50.0]),
        )
        contactModel.addContact(
            self.rmodel.frames[i].name + "_contact", supportContactModel
        )
    
   # add additional points of contact in right foot if it is in contact with ground
    if self.rfId in supportFootIds:
        additional_contacts = [self.rfootFR_id, self.rfootFL_id, self.rfootHR_id, self.rfootHL_id]  
        for i in additional_contacts:
            additionalContactModel = crocoddyl.ContactModel3D(
                self.state,
                i,
                np.array([0.0, 0.0, 0.0]),
                pin.LOCAL_WORLD_ALIGNED,
                nu,
                np.array([0.0, 50.0]),
            )
            contactModel.addContact(
                self.rmodel.frames[i].name + "_additional_contact", additionalContactModel
            )

as soon as I add these additional contacts, it can't solve the problem.
Are there more things that I need to take care of while adding more contact points? Let me know if I should provide any other details about my code for better understanding.
Are there any limitations in the current library for adding number of points of contacts? I also tried adding costs for these additional contact points, but didn't work.
Thanks in advance for your help!!

[cmastalli]

For understanding technical details please read our Crocoddyl paper. However, the short answer is "you need to use a 6D contact model and wrench cone".

[abhirajsingh101]

@cmastalli , Thank you for your prompt reply!
I went through the paper and tried using 6D contact with wrench cone. But still doesn't seem to work.
It works well and converges in about 100 iterations if I use single 6D contact point with wrench cone in each foot. But as soon as I add one more point on same foot, it doesn't converge.
For now, I'm just trying to add one extra point of contact on the right foot, but the problem isn't converging even after trying to tune different parameters. Can there be any issue with weights assigned to each cost?
I'll provide more details about the code (it is similar to the bipedal walking code present in the Crocoddyl repository):

    def createSwingFootModel(
    self, x0, timeStep, supportFootIds, comTask=None, swingFootTask=None, initialSwingKnot=False, RL_StairN=None, zeroVelScaler=None, additionalJoint=False
):
    """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
    if RL_StairN is None:
        RL_StairN = self.RL_StairN
    if self._fwddyn:
        nu = self.actuation.nu
    else:
        if self.rfId in supportFootIds:
            nu = self.state.nv + 6 * (len(supportFootIds) + 1)
        else:
            nu = self.state.nv + 6 * len(supportFootIds)
    contactModel = crocoddyl.ContactModelMultiple(self.state, nu)
    for i in supportFootIds:
        supportContactModel = crocoddyl.ContactModel6D(
            self.state,
            i,
            pin.SE3.Identity(),
            pin.LOCAL_WORLD_ALIGNED,
            nu,
            np.array([0.0, 50.0]),
        )
        contactModel.addContact(
            self.rmodel.frames[i].name + "_contact", supportContactModel
        )
    costModel = crocoddyl.CostModelSum(self.state, nu)
    if self.rfId in supportFootIds:
        additional_contacts = [self.rfootFR_id]  
        for i in additional_contacts:
            additionalContactModel = crocoddyl.ContactModel6D(
                self.state,
                i,
                pin.SE3.Identity(),
                pin.LOCAL_WORLD_ALIGNED,
                nu,
                np.array([0.0, 50.0]),
            )
            contactModel.addContact(
                self.rmodel.frames[i].name + "_additional_contact", additionalContactModel
            )

            cone = crocoddyl.WrenchCone(self.Rsurf, self.mu, self.footBox)
            wrenchResidual = crocoddyl.ResidualModelContactWrenchCone(
                self.state, i, cone, nu, self._fwddyn
            )
            wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier(
                crocoddyl.ActivationBounds(cone.lb, cone.ub)
            )
            wrenchCone = crocoddyl.CostModelResidual(
                self.state, wrenchActivation, wrenchResidual
            )
            costModel.addCost(
                self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1
            )

            # stancePlacementWeight = np.array( # [x, y, z, roll, pitch, yaw] weights for stance stability
            #     [50.0] + [50.0] + [100] * 4
            # )
            # stairSE3 = pin.SE3.Identity()
            # # stairSE3.translation[2] = self.dictStairN[i] * self.stairHeight
            # stairSE3.translation = self.dictFootTrans[i].translation
            # stairSE3.rotation = self.dictFootTrans[i].rotation
            # stanceRotationResidual = crocoddyl.ResidualModelFramePlacement(
            #     self.state, i, stairSE3, nu
            # )
            # stateActivation = crocoddyl.ActivationModelWeightedQuad(stancePlacementWeight**2)
            # stanceStable = crocoddyl.CostModelResidual(
            #     self.state, stateActivation, stanceRotationResidual,
            # )
            # costModel.addCost( # Stance stability cost
            #     self.rmodel.frames[i].name + "_stanceStable", stanceStable, 1e4
            # )

    # Creating the cost model for a contact phase
    
    if isinstance(comTask, np.ndarray):
        comResidual = crocoddyl.ResidualModelCoMPosition(self.state, comTask, nu)
        comweights = np.array([1.0, 1.0, 1.0])
        activationModel = crocoddyl.ActivationModelWeightedQuad(comweights**2)
        comTrack = crocoddyl.CostModelResidual(self.state, activationModel, comResidual)
        costModel.addCost("comTrack", comTrack, 1e5)
        
    for i in supportFootIds:
        cone = crocoddyl.WrenchCone(self.Rsurf, self.mu, self.footBox)
        wrenchResidual = crocoddyl.ResidualModelContactWrenchCone(
            self.state, i, cone, nu, self._fwddyn
        )
        wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier(
            crocoddyl.ActivationBounds(cone.lb, cone.ub)
        )
        wrenchCone = crocoddyl.CostModelResidual(
            self.state, wrenchActivation, wrenchResidual
        )
        costModel.addCost(
            self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e4
        )

        # stancePlacementWeight = np.array( # [x, y, z, roll, pitch, yaw] weights for stance stability
        #     [50.0] + [50.0] + [100] * 4
        # )
        # stairSE3 = pin.SE3.Identity()
        # # stairSE3.translation[2] = self.dictStairN[i] * self.stairHeight
        # stairSE3.translation = self.dictFootTrans[i].translation
        # stairSE3.rotation = self.dictFootTrans[i].rotation
        # stanceRotationResidual = crocoddyl.ResidualModelFramePlacement(
        #     self.state, i, stairSE3, nu
        # )
        # stateActivation = crocoddyl.ActivationModelWeightedQuad(stancePlacementWeight**2)
        # stanceStable = crocoddyl.CostModelResidual(
        #     self.state, stateActivation, stanceRotationResidual,
        # )
        # costModel.addCost( # Stance stability cost
        #     self.rmodel.frames[i].name + "_stanceStable", stanceStable, 1e7
        # )

    if swingFootTask is not None:
        for i in swingFootTask:
            stateWeights = self.swingStateWeights
            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
                self.state, i[0], i[1], nu
            )
            stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)
            footTrack = crocoddyl.CostModelResidual(
                self.state, stateActivation, framePlacementResidual,
            )
            costModel.addCost( # target foot position cost
                self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e6
            )
            maxfloat = sys.float_info.max
            boundInfSE3 = np.array([maxfloat]*6)
            boundFloorSE3 = -boundInfSE3
            

    stateWeights = self.allStateWeights_rot*1.
    if zeroVelScaler is not None:
        stateWeights[-12:] *= zeroVelScaler
    stateResidual = crocoddyl.ResidualModelState(
        self.state, self.rmodel.defaultState, nu
    )
    stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2)
    stateReg = crocoddyl.CostModelResidual(
        self.state, stateActivation, stateResidual
    )
    if self._fwddyn:
        ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu)
    else:
        ctrlResidual = crocoddyl.ResidualModelJointEffort(
            self.state, self.actuation, nu
        )
    ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual)
    # costModel.addCost("stateReg", stateReg, 1e1) # quadratic cost to deviations in state
    # costModel.addCost("ctrlReg", ctrlReg, 1e2) # penalises large control inputs

    ##########################################
    # Creating the hard limit bounds for joint velocities
    velocity_limits = {
        'JOINT_L_HIP_ABD_ENC': (-2.3038, 2.3038),
        'JOINT_L_HIP_INT': (-2.0944, 2.0944),
        'JOINT_L_HIP_FLEX': (-7.3304, 7.3304),
        'JOINT_L_KNEE': (-7.3304, 7.3304),
        'JOINT_L_INV': (-1.99, 1.99),
        'JOINT_L_PLA': (-1.885, 1.885),
        'JOINT_R_HIP_ABD_ENC': (-2.3038, 2.3038),
        'JOINT_R_HIP_INT': (-2.0944, 2.0944),
        'JOINT_R_HIP_FLEX': (-7.3304, 7.3304),
        'JOINT_R_KNEE': (-7.3304, 7.3304),
        'JOINT_R_INV': (-1.99, 1.99),
        'JOINT_R_PLA': (-1.885, 1.885),
    }

    ##########################################
    # Creating the hard limit bounds
    maxfloat = sys.float_info.max
    xlb = np.concatenate(
        [
            -maxfloat * np.ones(6),  # dimension of the SE(3) manifold (base translation and rotation unbounded)
            self.rmodel.lowerPositionLimit[7:], # joint positions lower limits
            -maxfloat * np.ones(self.state.nv), # joint velocities lower limits
        ]
    )
    xub = np.concatenate(
        [
            maxfloat * np.ones(6),  # dimension of the SE(3) manifold (base translation and rotation unbounded)
            self.rmodel.upperPositionLimit[7:], # joint positions upper limits
            maxfloat * np.ones(self.state.nv), # joint velocities upper limits
        ]
    )
    for i, (lower_limit, upper_limit) in enumerate(velocity_limits.values()):
        xlb[24+i] = lower_limit
        xub[24+i] = upper_limit
    bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
    xLimitResidual = crocoddyl.ResidualModelState(self.state, np.zeros_like(x0), nu)
    xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
    limitCost = crocoddyl.CostModelResidual(self.state, xLimitActivation, xLimitResidual)
    costModel.addCost("xLimit", limitCost, 1e10) # quadratic cost to deviations in position and velocity
    
    # Creating the action model for the KKT dynamics with simpletic Euler
    # integration scheme
    if self._fwddyn:
        dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(
            self.state, self.actuation, contactModel, costModel, 0.0, True
        )
    else:
        dmodel = crocoddyl.DifferentialActionModelContactInvDynamics(
            self.state, self.actuation, contactModel, costModel
        )
    if self._control == "one":
        control = crocoddyl.ControlParametrizationModelPolyOne(nu)
    elif self._control == "rk4":
        control = crocoddyl.ControlParametrizationModelPolyTwoRK(
            nu, crocoddyl.RKType.four
        )
    elif self._control == "rk3":
        control = crocoddyl.ControlParametrizationModelPolyTwoRK(
            nu, crocoddyl.RKType.three
        )
    else:
        control = crocoddyl.ControlParametrizationModelPolyZero(nu)
    if self._integrator == "euler":
        model = crocoddyl.IntegratedActionModelEuler(dmodel, control, timeStep)
    elif self._integrator == "rk4":
        model = crocoddyl.IntegratedActionModelRK4(dmodel, control, timeStep)
    return model

Below is the terrainWalkWPG code:

   def terrainWalkWPG(
                self, swingFoot, strideLength, distBetweenLegs,
                stepHeight=0.1, timeStep=0.025, stepKnots=30, supportKnots=30,
                terrainNormal=np.array([0.0, 0.0, 1.0]), terrainMidZ=0.0,
                ):
    self.beforeProb()
    
    timeStep = 0.01
    stepKnots = int(200*self.knotScaler)
    supportKnots = int(200*self.knotScaler)

    swingfoot_now = swingFoot
    if strideLength > 0.4 or distBetweenLegs > 0.4:
        stepKnots = int(200*self.knotScaler)
        supportKnots = int(200*self.knotScaler)
        timeStep = 0.01

    if swingfoot_now == "right":
        stepY = -distBetweenLegs
    else:
        stepY = distBetweenLegs

    problem = self.gait.createTerrainWalkProblem(
        self.x0, step_x=strideLength, step_y=stepY,
        stepHeight=stepHeight, timeStep=timeStep,
        stepKnots=stepKnots, supportKnots=supportKnots,
        taskSwingFoot=swingfoot_now, doubleGen=False, firstStep=False,
        terrainNormal=terrainNormal, terrainMidZ=terrainMidZ,
    )

    ddp = crocoddyl.SolverBoxFDDP(problem)
    ddp.th_stop = 1e-9
    init_xs = [self.x_last] * (problem.T + 1)
    init_us = []
    maxiter = 10000
    regInit = 0.1

    # Add the verbose callback
    verbose_cb = CallbackVerbose()
    ddp.setCallbacks([verbose_cb])

    feasibility = ddp.solve(init_xs, init_us, maxiter, False, regInit)
    if not feasibility:
        raise RuntimeError("The optimal control problem is not feasible")

    self.afterProb(ddp, stepKnots, supportKnots)
    # ddp.xs = np.zeros_like(ddp.xs)
    self.display.rate = -1
    self.display.freq = 1
    self.display.displayFromSolver(ddp)

    
    return ddp

  class CallbackVerbose(crocoddyl.CallbackAbstract):
      def __init__(self):
          crocoddyl.CallbackAbstract.__init__(self)
  
      def __call__(self, solver):
          print(f"Iteration {solver.iter}: Cost {solver.cost}, Step Length {solver.stepLength}")
          if hasattr(solver, 'stop'):
              print(f" Stop Criteria: {solver.stop}")
          if hasattr(solver, 'd'):
              print(f" Gradient Norm: {solver.d}")

Below is the screenshot of the result, it doesnt' converge and stops after few iterations.

[cmastalli]

You are over-constraining the problem by adding two or more 6D contacts per leg. You must read carefully our paper for a better understanding.