SIGABRT in Custom DifferentialActionModel

[psclklnk]

Hello everyone and thanks for this wonderful library!

I am currently trying to implement a gravity-compensated differential action model by starting from the example given in the iPython notebook and trying to reverse engineer the C++ code.

My current code looks like this:

import crocoddyl
import numpy as np
import pinocchio as pin
from typing import Optional


class DifferentialActionModelGravityCompensatedFwdDynamics(crocoddyl.DifferentialActionModelAbstract):

    def __init__(self, state, actuation, costModel, armature: Optional[np.ndarray] = None,
                 damping: Optional[np.ndarray] = None):
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)
        self.costs = costModel
        self.actuation = actuation

        self.armature = np.zeros(self.state.nv)
        if armature is not None:
            self.armature[:] = armature

        self.damping = np.zeros(self.state.nv)
        if damping is not None:
            self.damping[:] = damping

    def calc(self, data, x, u=None):
        if u is None:
            u = self.unone

        self.actuation.calc(data.multibody.actuation, x, u)
        q, v = x[: self.state.nq], x[-self.state.nv:]

        pin.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
        data.M = data.pinocchio.M
        data.M[range(self.state.nv), range(self.state.nv)] += self.armature
        data.Minv = np.linalg.inv(data.M)
        data.xout = np.einsum("ij,j->i", data.Minv, u - data.pinocchio.nle + data.pinocchio.g - self.damping * v)

        pin.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)
        pin.updateFramePlacements(self.state.pinocchio, data.pinocchio)
        self.costs.calc(data.costs, x, u)
        data.cost = data.costs.cost

    def calcDiff(self, data, x, u=None):
        q, v = x[: self.state.nq], x[-self.state.nv:]
        if u is None:
            u = self.unone

        self.actuation.calc(data.multibody.actuation, x, u)

        # Computing the dynamics derivatives
        grav_comp_derivatives = pin.computeGeneralizedGravityDerivatives(self.state.pinocchio, data.pinocchio, q)
        damping_derivatives = -np.diag(self.damping)
        pin.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout)
        fx = -np.hstack([data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv])
        fx[:, :self.state.nq] = grav_comp_derivatives
        fx[:, -self.state.nv:] = damping_derivatives
        data.Fx = fx
        data.Fu = data.Minv

        # Computing the cost derivatives
        self.costs.calcDiff(data.costs, x, u)

    def createData(self):
        data = crocoddyl.DifferentialActionModelAbstract.createData(self)
        data.pinocchio = pin.Data(self.state.pinocchio)
        data.multibody = crocoddyl.DataCollectorActMultibody(data.pinocchio, self.actuation.createData())
        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

My main problem with the code is that the call to

self.actuation.calc(data.multibody.actuation, x, u)

leads to the python code terminating with a SIGABRT signal. Most likely, this is caused by an incorrect setup of the data but I was unfortunately not able to find any details on this part on the documentation.

It would be awesome if a more-experienced library user could point me to the issue here.

Best
Pascal

[cmastalli]

Hi @psclklnk!

You haven't created the actuation data. You need to do as follows:

    def createData(self):
        data = DifferentialActionDataGravityCompensatedFwdDynamics(self)
        return data

class DifferentialActionDataGravityCompensatedFwdDynamics(crocoddyl.DifferentialActionDataAbstract):
    def __init__(self, model):
        crocoddyl.DifferentialActionDataAbstract.__init__(self, model)
        self.pinocchio = pinocchio.Model.createData(model.state.pinocchio)
        self.multibody = crocoddyl.DataCollectorMultibody(self.pinocchio)
        self.actuation = model.actuation.createData()
        self.costs = model.costs.createData(self.multibody)
        self.costs.shareMemory(self)

Btw, you should call self.actuation.calcDiff within calcDiff function.

[psclklnk]

Hi @cmastalli!

Thanks for the quick help! I updated my code as follows:

import crocoddyl
import numpy as np
import pinocchio as pin
from typing import Optional


class DifferentialActionModelGravityCompensatedFwdDynamics(crocoddyl.DifferentialActionModelAbstract):

    def __init__(self, state, actuation, costModel, armature: Optional[np.ndarray] = None,
                 damping: Optional[np.ndarray] = None):
        crocoddyl.DifferentialActionModelAbstract.__init__(self, state, state.nv, costModel.nr)
        self.costs = costModel
        self.actuation = actuation

        self.armature = np.zeros(self.state.nv)
        if armature is not None:
            self.armature[:] = armature

        self.damping = np.zeros(self.state.nv)
        if damping is not None:
            self.damping[:] = damping

    def calc(self, data, x, u=None):
        if u is None:
            u = self.unone

        self.actuation.calc(data.actuation, x, u)
        q, v = x[: self.state.nq], x[-self.state.nv:]

        pin.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)
        data.M = data.pinocchio.M
        data.M[range(self.state.nv), range(self.state.nv)] += self.armature
        data.Minv = np.linalg.inv(data.M)
        data.xout = np.einsum("ij,j->i", data.Minv, u - data.pinocchio.nle + data.pinocchio.g - self.damping * v)

        pin.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)
        pin.updateFramePlacements(self.state.pinocchio, data.pinocchio)
        self.costs.calc(data.costs, x, u)
        data.cost = data.costs.cost

    def calcDiff(self, data, x, u=None):
        q, v = x[: self.state.nq], x[-self.state.nv:]
        if u is None:
            u = self.unone

        self.actuation.calcDiff(data.actuation, x, u)

        # Computing the dynamics derivatives
        grav_comp_derivatives = pin.computeGeneralizedGravityDerivatives(self.state.pinocchio, data.pinocchio, q)
        damping_derivatives = -np.diag(self.damping)
        pin.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout)
        fx = -np.hstack([data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv])
        fx[:, :self.state.nq] = grav_comp_derivatives
        fx[:, -self.state.nv:] = damping_derivatives
        data.Fx = fx
        data.Fu = data.Minv

        # Computing the cost derivatives
        self.costs.calcDiff(data.costs, x, u)

    def createData(self):
        return DifferentialActionDataGravityCompensatedFwdDynamics(self)


class DifferentialActionDataGravityCompensatedFwdDynamics(crocoddyl.DifferentialActionDataAbstract):
    def __init__(self, model):
        crocoddyl.DifferentialActionDataAbstract.__init__(self, model)
        self.pinocchio = pin.Model.createData(model.state.pinocchio)
        self.multibody = crocoddyl.DataCollectorMultibody(self.pinocchio)
        self.actuation = model.actuation.createData()
        self.costs = model.costs.createData(self.multibody)
        self.costs.shareMemory(self)

but unfortunately I still receive a SIGABRT when calling

self.actuation.calc(data.actuation, x, u).

I also tried the following alternative data class

class DifferentialActionDataGravityCompensatedFwdDynamics(crocoddyl.DifferentialActionDataAbstract):
    def __init__(self, model):
        crocoddyl.DifferentialActionDataAbstract.__init__(self, model)
        self.pinocchio = pin.Model.createData(model.state.pinocchio)
        self.multibody = crocoddyl.DataCollectorActMultibody(self.pinocchio, self.actuation.createData())
        self.costs = model.costs.createData(self.multibody)
        self.costs.shareMemory(self)

where I store the actuation data inside the multibody data. Nonetheless, calling

self.actuation.calc(data.multibody.actuation, x, u)

still led to the same outcome.

Do you have an idea what else could be the problem?

Best
Pascal