Convergence problem when creating a differential action model

[Andrea8Testa]

Hello,

First of all, thank you for providing such a good optimal control library for locomotion. I am trying to implement a DifferentialActionModel accounting for contacts and actuation by following the example from introduction_to_crocoddyl.ipynb. My goal is to correctly run bipedal_walk.py by substituting the library model crocoddyl.DifferentialActionModelContactFwdDynamics with my personal DifferentialDynamics in biped.py.

class DifferentialDynamics(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self, state, actuation, contactModel, costModel):
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuation.nu, costModel.nr)
        self.contact = contactModel
        self.costs = costModel
        self.actuation = actuation
        self.enable_force = True
        self.armature = np.zeros(0)

    def calc(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
        pinocchio.computeCentroidalMomentum(self.state.pinocchio, data.pinocchio)
        
        self.actuation.calc(data.multibody.actuation, x, u)
        self.contact.calc(data.multibody.contacts, x)
       
        pinocchio.forwardDynamics(self.state.pinocchio, data.pinocchio, data.multibody.actuation.tau,
                             data.multibody.contacts.Jc[:self.contact.nc, :], data.multibody.contacts.a0[:self.contact.nc])
        data.xout = data.pinocchio.ddq
        self.contact.updateAcceleration(data.multibody.contacts, data.pinocchio.ddq)
        self.contact.updateForce(data.multibody.contacts, data.pinocchio.lambda_c)
        
        # Computing the cost value and residuals
        self.costs.calc(data.costs, x, u)
        data.cost = data.costs.cost

    def calcDiff(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        data.Kinv = np.zeros([self.state.nv + self.contact.nc, self.state.nv + self.contact.nc])
        pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout, data.multibody.contacts.fext)
        pinocchio.getKKTContactDynamicMatrixInverse(self.state.pinocchio, data.pinocchio, data.multibody.contacts.Jc[:self.contact.nc, :])

        self.actuation.calcDiff(data.multibody.actuation, x, u)
        self.contact.calcDiff(data.multibody.contacts, x)
        a_partial_dtau = data.Kinv[:self.state.nv,:self.state.nv]
        a_partial_da = data.Kinv[:self.state.nv,-self.contact.nc:]
        f_partial_dtau = data.Kinv[-self.contact.nc:,:self.state.nv]
        f_partial_da = data.Kinv[-self.contact.nc:,-self.contact.nc:]
    
        data.Fx[:,:self.state.nv] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dq)
        data.Fx[:,-self.state.nv:] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dv)
        data.Fx -= np.matmul(a_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
        data.Fx += np.matmul(a_partial_dtau, data.multibody.actuation.dtau_dx)
        data.Fu = np.matmul(a_partial_dtau, data.multibody.actuation.dtau_du)
        
        data.df_dx = np.zeros([self.contact.nc,self.state.nv*2])
        data.df_du = np.zeros([self.contact.nc,self.actuation.nu])
        # Computing the cost derivatives
        if self.enable_force:
          data.df_dx[:self.contact.nc,:self.state.nv] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dq)
          data.df_dx[:self.contact.nc,-self.state.nv:] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dv)
          data.df_dx[:self.contact.nc,:] += np.matmul(f_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
          data.df_dx[:self.contact.nc,:] -= np.matmul(f_partial_dtau, data.multibody.actuation.dtau_dx)
          data.df_du[:self.contact.nc,:] = np.matmul(-f_partial_dtau, data.multibody.actuation.dtau_du)
          
          self.contact.updateAccelerationDiff(data.multibody.contacts, data.Fx[-self.state.nv:,:])
          self.contact.updateForceDiff(data.multibody.contacts, data.df_dx[:self.contact.nc,:], data.df_du[:self.contact.nc,:])

        # Computing the cost derivatives
        self.costs.calcDiff(data.costs, x, u)
        
    def set_armature(self, armature):
        if armature.size is not self.state.nv:
            print('The armature dimension is wrong, we cannot set it.')
        else:
            self.enable_force = False
            self.armature = armature.T

    def createData(self):
        data = crocoddyl.DifferentialActionModelAbstract.createData(self)
        data.pinocchio = pinocchio.Data(self.state.pinocchio)
        data.multibody = crocoddyl.DataCollectorActMultibodyInContact(data.pinocchio, self.actuation.createData(), self.contact.createData(data.pinocchio))
        data.costs = self.costs.createData(data.multibody)
        data.costs.shareMemory(data) # this allows us to share the memory of cost-terms of action model
        return data

Running the code, I do not get any error, but the solver cannot converge. In the output trajectories, the robot seems to fall beyond the support platform, so there should be something wrong with the contact handling.

> python3 bipedal_walk.py "display"
*** SOLVE walking ***
iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
   0  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-08  1.00000e-08   0.0020     0
   1  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-07  1.00000e-07   0.0020     0
   2  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-06  1.00000e-06   0.0020     0
   3  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-05  1.00000e-05   0.0020     0
   4  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-04  1.00000e-04   0.0020     0
   5  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-03  1.00000e-03   0.0020     0
   6  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-02  1.00000e-02   0.0020     0
   7  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e-01  1.00000e-01   0.0020     0
   8  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+00  1.00000e+00   0.0020     0
   9  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+01  1.00000e+01   0.0020     0
iter 	 cost 	      stop 	    grad 	  xreg 	      ureg 	 step 	 feas
  10  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+02  1.00000e+02   0.0020     0
  11  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+03  1.00000e+03   0.0020     0
  12  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+04  1.00000e+04   0.0020     0
  13  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+05  1.00000e+05   0.0020     0
  14  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+06  1.00000e+06   0.0020     0
  15  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+07  1.00000e+07   0.0020     0
  16  4.06430e+07  0.00000e+00  -0.00000e+00  1.00000e+08  1.00000e+08   0.0020     0

Do I forget to implement a part of the Action Model? Is something in my class misdefined?

[cmastalli]

Hi @Andrea8Testa! You should write

data.xout[:] = data.pinocchio.ddq

as we need to write the data internally, rather than modify the object.

Let me know if this fixes your issue.

[Andrea8Testa]

Hi @cmastalli, thanks for your suggestion.

Unfortunately, I cannot see any difference in the output.

[cmastalli]

You also need to write the internal data in these lines:

        data.Fx -= np.matmul(a_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
        data.Fx += np.matmul(a_partial_dtau, data.multibody.actuation.dtau_dx)
        data.Fu = np.matmul(a_partial_dtau, data.multibody.actuation.dtau_du)

Additionally, you are allocating data online. data.Kinv, data.df_dx and data.df_du should be created in createData.

[Andrea8Testa]

I modified the code to address the suggestions, but the result still does not change.

class DifferentialSEADynamics(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self, state, actuation, contactModel, costModel):
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuation.nu, costModel.nr)
        self.contact = contactModel
        self.costs = costModel
        self.actuation = actuation
        self.enable_force = True
        self.armature = np.zeros(0)

    def calc(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
        pinocchio.computeCentroidalMomentum(self.state.pinocchio, data.pinocchio)
        
        self.actuation.calc(data.multibody.actuation, x, u)
        self.contact.calc(data.multibody.contacts, x)
        
        pinocchio.forwardDynamics(self.state.pinocchio, data.pinocchio, data.multibody.actuation.tau,
                             data.multibody.contacts.Jc[:self.contact.nc, :], data.multibody.contacts.a0[:self.contact.nc])
        data.xout[:] = data.pinocchio.ddq
        self.contact.updateAcceleration(data.multibody.contacts, data.pinocchio.ddq)
        self.contact.updateForce(data.multibody.contacts, data.pinocchio.lambda_c)
        
        # Computing the cost value and residuals
        self.costs.calc(data.costs, x, u)
        data.cost = data.costs.cost

    def calcDiff(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout, data.multibody.contacts.fext)
        pinocchio.getKKTContactDynamicMatrixInverse(self.state.pinocchio, data.pinocchio, data.multibody.contacts.Jc[:self.contact.nc, :])

        self.actuation.calcDiff(data.multibody.actuation, x, u)
        self.contact.calcDiff(data.multibody.contacts, x)
        a_partial_dtau = data.Kinv[:self.state.nv,:self.state.nv]
        a_partial_da = data.Kinv[:self.state.nv,-self.contact.nc:]
        f_partial_dtau = data.Kinv[-self.contact.nc:,:self.state.nv]
        f_partial_da = data.Kinv[-self.contact.nc:,-self.contact.nc:]
        
        data.Fx[:,:self.state.nv] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dq)
        data.Fx[:,-self.state.nv:] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dv)
        data.Fx[:,:] -= np.matmul(a_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
        data.Fx[:,:] += np.matmul(a_partial_dtau, data.multibody.actuation.dtau_dx)
        data.Fu[:,:] = np.matmul(a_partial_dtau, data.multibody.actuation.dtau_du)

        # Computing the cost derivatives
        if self.enable_force:
          data.df_dx[:self.contact.nc,:self.state.nv] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dq)
          data.df_dx[:self.contact.nc,-self.state.nv:] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dv)
          data.df_dx[:self.contact.nc,:] += np.matmul(f_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
          data.df_dx[:self.contact.nc,:] -= np.matmul(f_partial_dtau, data.multibody.actuation.dtau_dx)
          data.df_du[:self.contact.nc,:] = np.matmul(-f_partial_dtau, data.multibody.actuation.dtau_du)
          
          self.contact.updateAccelerationDiff(data.multibody.contacts, data.Fx[-self.state.nv:,:])
          self.contact.updateForceDiff(data.multibody.contacts, data.df_dx[:self.contact.nc,:], data.df_du[:self.contact.nc,:])

        # Computing the cost derivatives
        self.costs.calcDiff(data.costs, x, u)
        
    def set_armature(self, armature):
        if armature.size is not self.state.nv:
            print('The armature dimension is wrong, we cannot set it.')
        else:
            self.enable_force = False
            self.armature = armature.T

    def createData(self):
        data = crocoddyl.DifferentialActionModelAbstract.createData(self)
        data.pinocchio = pinocchio.Data(self.state.pinocchio)
        data.multibody = crocoddyl.DataCollectorActMultibodyInContact(data.pinocchio, self.actuation.createData(), self.contact.createData(data.pinocchio))
        data.costs = self.costs.createData(data.multibody)
        data.costs.shareMemory(data) # this allows us to share the memory of cost-terms of action model
        data.Kinv = np.zeros([self.state.nv + self.contact.nc, self.state.nv + self.contact.nc])
        data.df_dx = np.zeros([self.contact.nc,self.state.nv*2])
        data.df_du = np.zeros([self.contact.nc,self.actuation.nu])
        return data

May a quasiStatic function help the solver? In crocoddyl.DifferentialActionModelContactFwdDynamics, it is implemented directly in the class definition.

[cmastalli]

I modified the code to address the suggestions, but the result still does not change.

class DifferentialSEADynamics(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self, state, actuation, contactModel, costModel):
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuation.nu, costModel.nr)
        self.contact = contactModel
        self.costs = costModel
        self.actuation = actuation
        self.enable_force = True
        self.armature = np.zeros(0)

    def calc(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
        pinocchio.computeCentroidalMomentum(self.state.pinocchio, data.pinocchio)
        
        self.actuation.calc(data.multibody.actuation, x, u)
        self.contact.calc(data.multibody.contacts, x)
        
        pinocchio.forwardDynamics(self.state.pinocchio, data.pinocchio, data.multibody.actuation.tau,
                             data.multibody.contacts.Jc[:self.contact.nc, :], data.multibody.contacts.a0[:self.contact.nc])
        data.xout[:] = data.pinocchio.ddq
        self.contact.updateAcceleration(data.multibody.contacts, data.pinocchio.ddq)
        self.contact.updateForce(data.multibody.contacts, data.pinocchio.lambda_c)
        
        # Computing the cost value and residuals
        self.costs.calc(data.costs, x, u)
        data.cost = data.costs.cost

    def calcDiff(self, data, x, u):
        q, v = x[:self.state.nq], x[-self.state.nv:]
        
        pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout, data.multibody.contacts.fext)
        pinocchio.getKKTContactDynamicMatrixInverse(self.state.pinocchio, data.pinocchio, data.multibody.contacts.Jc[:self.contact.nc, :])

        self.actuation.calcDiff(data.multibody.actuation, x, u)
        self.contact.calcDiff(data.multibody.contacts, x)
        a_partial_dtau = data.Kinv[:self.state.nv,:self.state.nv]
        a_partial_da = data.Kinv[:self.state.nv,-self.contact.nc:]
        f_partial_dtau = data.Kinv[-self.contact.nc:,:self.state.nv]
        f_partial_da = data.Kinv[-self.contact.nc:,-self.contact.nc:]
        
        data.Fx[:,:self.state.nv] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dq)
        data.Fx[:,-self.state.nv:] = np.matmul(-a_partial_dtau, data.pinocchio.dtau_dv)
        data.Fx[:,:] -= np.matmul(a_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
        data.Fx[:,:] += np.matmul(a_partial_dtau, data.multibody.actuation.dtau_dx)
        data.Fu[:,:] = np.matmul(a_partial_dtau, data.multibody.actuation.dtau_du)

        # Computing the cost derivatives
        if self.enable_force:
          data.df_dx[:self.contact.nc,:self.state.nv] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dq)
          data.df_dx[:self.contact.nc,-self.state.nv:] = np.matmul(f_partial_dtau, data.pinocchio.dtau_dv)
          data.df_dx[:self.contact.nc,:] += np.matmul(f_partial_da, data.multibody.contacts.da0_dx[:self.contact.nc,:])
          data.df_dx[:self.contact.nc,:] -= np.matmul(f_partial_dtau, data.multibody.actuation.dtau_dx)
          data.df_du[:self.contact.nc,:] = np.matmul(-f_partial_dtau, data.multibody.actuation.dtau_du)
          
          self.contact.updateAccelerationDiff(data.multibody.contacts, data.Fx[-self.state.nv:,:])
          self.contact.updateForceDiff(data.multibody.contacts, data.df_dx[:self.contact.nc,:], data.df_du[:self.contact.nc,:])

        # Computing the cost derivatives
        self.costs.calcDiff(data.costs, x, u)
        
    def set_armature(self, armature):
        if armature.size is not self.state.nv:
            print('The armature dimension is wrong, we cannot set it.')
        else:
            self.enable_force = False
            self.armature = armature.T

    def createData(self):
        data = crocoddyl.DifferentialActionModelAbstract.createData(self)
        data.pinocchio = pinocchio.Data(self.state.pinocchio)
        data.multibody = crocoddyl.DataCollectorActMultibodyInContact(data.pinocchio, self.actuation.createData(), self.contact.createData(data.pinocchio))
        data.costs = self.costs.createData(data.multibody)
        data.costs.shareMemory(data) # this allows us to share the memory of cost-terms of action model
        data.Kinv = np.zeros([self.state.nv + self.contact.nc, self.state.nv + self.contact.nc])
        data.df_dx = np.zeros([self.contact.nc,self.state.nv*2])
        data.df_du = np.zeros([self.contact.nc,self.actuation.nu])
        return data

May a quasiStatic function help the solver? In crocoddyl.DifferentialActionModelContactFwdDynamics, it is implemented directly in the class definition.

Most of the time, FDDP handles problems without warmstart well. You issue is likely to be in the differential action. You must first check that the derivatives are correct. You could use DifferentialActionModelNumDiff class.

[Andrea8Testa]

Hi @cmastalli,

In biped.py, I used the class DifferentialActionModelNumDiff to inspect the computed dmodel as

dmodel = DifferentialSEADynamics(self.state, self.actuation, contactModel, costModel)
derivative = crocoddyl.DifferentialActionModelNumDiff(dmodel)

I printed the derivatives using the command derivative.model.createData(). They result to be always null.

Also in the variable problem sent to the solver the derivatives appear to be null.

problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
print((problem.terminalData.Fx))
...

Do you have any clue about the origin of this problem?

[cmastalli]

Hi @cmastalli,

In biped.py, I used the class DifferentialActionModelNumDiff to inspect the computed dmodel as

dmodel = DifferentialSEADynamics(self.state, self.actuation, contactModel, costModel)
derivative = crocoddyl.DifferentialActionModelNumDiff(dmodel)

I printed the derivatives using the command derivative.model.createData(). They result to be always null.

Also in the variable problem sent to the solver the derivatives appear to be null.

problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
print((problem.terminalData.Fx))
...

Do you have any clue about the origin of this problem?

First of all, the DifferentialActionModelNumDiff class does not fully support the computation of all the derivatives; it doesn't compute the Hessians. We are interested to make it fully compatible but we haven't had time to finish this feature. Therefore, it cannot be used with ShootingProblem and any of our solvers.

Second, you are using the class wrongly. You need to call calc and then calcDiff to retrieve the derivatives. This class is intended to be used as any action model.

Third, we have been working on the same model (SEA actuators with contact dynamics). And what I can tell you is that the contact class is not compatible (in its current form) with this type of problem. We have a working code but still not a super clean API.

I will close this issue, as the kind of support that you need has to come from your supervisor.