Skip to content

Navigation Menu

Sign in
Sign up

SIGABRT in Custom DifferentialActionModel #1136

Discussion options

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
You must be logged in to vote

Replies: 2 comments

Comment options

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.
You must be logged in to vote
0 replies
Comment options

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
You must be logged in to vote
0 replies
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
question Further information is requested
Converted from issue

This discussion was converted from issue #1135 on May 22, 2023 18:01.

AltStyle によって変換されたページ (->オリジナル) /