Using constraints in custom action models (Python)

[ajordana]

I would like to create a LQR model with linear constraints using the constraint management API of Crocoddyl v2, and it seems that I cannot create arbitrary constraints with my custom action model: indeed, my custom action model inherits from the abstract class DifferentialActionModelAbstract which does not accept any form of constraint model attribute.

I also tried to specify the constraint bounds manually in my custom model, but found that setters were not binded in python.

Here is a piece of code illustrating what I am trying to do:

import numpy as np
import crocoddyl

class DifferentialActionModelLQR(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self):
        self.nq = 2
        self.nv = 2
        self.ndx = 2
        self.nx = self.nq + self.nv
        nu = 2
        nr = 1
        ng = 4
        nh = 0

        state = crocoddyl.StateVector(self.nx)

        ## I would like to specify the following type of contraint in my custom model

        # lower_bound = np.array([-np.inf] * ng)
        # upper_bound = np.array([10.0] * ng)
        # constraints = crocoddyl.ConstraintModelManager(state, nu)
        # state_res = crocoddyl.ResidualModelState(state, np.zeros(self.nx), nu)
        # state_ctr = crocoddyl.ConstraintModelResidual(
        #     state, state_res, lower_bound, upper_bound
        # )
        # constraints.addConstraint("state_constraint", state_ctr)

        ## Alternatively, I would like to specify the upper bound manually and implement g, Gx, and Gu in the calc and calc diff

        # self.g_lb = lower_bound
        # self.g_ub = upper_bound

        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, nu, nr, ng, nh)

    def calc(self, data, x, u=None):
        data.cost = 0
        data.xout = x
        data.g = x

    def calcDiff(self, data, x, u=None):
        data.Fx = np.zeros([self.ndx, self.nx])
        data.Fu = np.zeros([self.ndx, self.nu])

        data.Lx = np.zeros([self.nx])
        data.Lu = np.zeros([self.nu])
        data.Lxx = np.zeros([self.nx, self.nx])
        data.Luu = np.zeros([self.nu, self.nu])
        data.Lxu = np.zeros([self.nx, self.nu])

        data.Gx = np.eye(self.nx)
        data.Gu = np.eye(self.nu)

lqr_diff_running = DifferentialActionModelLQR()

Hence it appears that in the current API, constraints can only be added as constraint models in the derived classes (e.g. DifferentialActionModelFreeFwdDynamics) which is limiting as it means that the user must define a StateMultibody, which in turn implies to use a Pinocchio model.

One potential way to address this would be to allow the constructor of DifferentialActionModelAbstract to take directly an abstract constraint model. Alternatively, there could be additional bindings in order to specify the lower and upper bound of the constraints manually in a custom python action models inheriting from DifferentialActionModelAbstract.

Is my understanding correct, and if so, is it possible to enhance the API accordingly ?

[cmastalli]

Hi @ajordana,

Thanks for telling us about this limitation in our Python bindings.

You can change the lower/upper bounds on inequality constraints using this https://github.com/loco-3d/crocoddyl/pull/1171.

I'll close this issue, but feel free to ask any questions.

[ajordana]

Thank you very much @cmastalli! I was able to create a constrained LQR by inheriting from ActionModelAbstract.

However, I noticed the following: when I create a constrained LQR by inheriting from DifferentialActionModelAbstract, I am able to set g_ub and g_lb but it seems that the Euler integration resets them to their default value.

Here is an example:

import numpy as np
import crocoddyl

class DifferentialActionModelLQR(crocoddyl.DifferentialActionModelAbstract):
    def __init__(self):
        nx = 4
        nu = 2
        nr = 1
        ng = 4
        nh = 0

        state = crocoddyl.StateVector(nx)
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, nu, nr, ng, nh)

        self.g_lb = np.array([-np.inf] * ng)
        self.g_ub = np.array([10.0] * ng)

    def calc(self, data, x, u=None):
        pass

    def calcDiff(self, data, x, u=None):
        pass

lqr_diff = DifferentialActionModelLQR()
print("Differential model g_ub = ", lqr_diff.g_ub)
dt = 0.01
lqr = crocoddyl.IntegratedActionModelEuler(lqr_diff, dt)
print("Integrated model g_ub = ", lqr.g_ub)

Is there something wrong in what I am doing?

[cmastalli]

@ajordana -- please see #1180. It should fix your issue.

[ajordana]

Yes, this fixes the issue. Thank you!