(Core dumped) when using Franka Panda

[spykspeigel]

I tried using the Panda arm in the arm manipulation example (inside crocoddyl/examples folder).

import os
import sys

import crocoddyl
import pinocchio
import numpy as np
import example_robot_data

WITHDISPLAY = 'display' in sys.argv or 'CROCODDYL_DISPLAY' in os.environ
WITHPLOT = 'plot' in sys.argv or 'CROCODDYL_PLOT' in os.environ

panda_arm = example_robot_data.load('panda')
robot_model = panda_arm.model

state = crocoddyl.StateMultibody(robot_model)
runningCostModel = crocoddyl.CostModelSum(state)
terminalCostModel = crocoddyl.CostModelSum(state)

framePlacementResidual = crocoddyl.ResidualModelFramePlacement(state, robot_model.getFrameId("panda_finger_joint2"),
                                                               pinocchio.SE3(np.eye(3), np.array([.0, .1, .1])))
uResidual = crocoddyl.ResidualModelControl(state)
xResidual = crocoddyl.ResidualModelControl(state)
goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
xRegCost = crocoddyl.CostModelResidual(state, xResidual)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)

runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
runningCostModel.addCost("xReg", xRegCost, 1e-4)
runningCostModel.addCost("uReg", uRegCost, 1e-4)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)

actuationModel = crocoddyl.ActuationModelFull(state)
dt = 1e-3
runningModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt)
# runningModel.differential.armature = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.])
terminalModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.)
# terminalModel.differential.armature = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.])

T = 250
q0 = panda_arm.q0
x0 = np.concatenate([q0, pinocchio.utils.zero(robot_model.nv)])
problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)

# Creating the DDP solver for this OC problem, defining a logger
solver = crocoddyl.SolverDDP(problem)

# Solving it with the DDP algorithm
solver.solve()

I get the following error:

free(): invalid pointer
Aborted (core dumped)

I am not sure about the origin of the error.

[cmastalli]

The error comes from an problem setup mistake in your code. Your asking to track a reference system (in the framePlacementResidual) that does not exist in the panda robot.

[wxmerkt]

Your asking to track a reference system (in the framePlacementResidual) that does not exist in the panda robot.

That's correct - should we check if a joint exists and throw an exception if it doesnt during the constructor of the residual?

[spykspeigel]

@cmastalli @wxmerkt I have updated the code above using the correct joint name. Somehow the solver returns False without solving the problem.

[cmastalli]

That's correct - should we check if a joint exists and throw an exception if it doesnt during the constructor of the residual?

The problem is in Pinocchio as getFrameId doesn't trigger an issue. I could implement a safe policy if we know what happens when getFrameId is used with an undefined frame.

@jcarpent what is the Pinocchio policy with respect to this matter? For instance, I see that it returns an index that it is model.nframes; however, I am not sure if I could assume this is part of the Pinocchio API.

[jcarpent]

Yes, this is the currrent policy. We return the number of elements in the vector