Solver returns False with Franka Panda arm manipulation task

[spykspeigel]

I still face the issue that the arm_manipulation example with the Franka arm does not work.
The following is the code snippet used. Even without a frame placement cost, the solver returns False.
Any help is highly appreciated.

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_joint7"),
                                                               pinocchio.SE3(np.eye(3), np.array([.0, .01, .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-2
runningModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt)
terminalModel = crocoddyl.IntegratedActionModelEuler(
    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.)

T = 150
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()

[wxmerkt]

Hi @spykspeigel,
I've identified the root cause of your problem to be with the URDF of the Panda that you are using, cf. Gepetto/example-robot-data#122

However, below I will provide some insights into the debugging that uncovered this - and pointers to what to check with similar failures in the future.

1. Using the verbose callback: I've noticed that your problem does not have a verbose callback, which in most cases helps to narrow down the source of the problem (solver.setCallbacks([crocoddyl.CallbackVerbose()]). However, even after adding those, your problem does not converge.
2. The early return with False without an output points to the x-regularisation exceeding the maximal value during the 0th iteration: These are the two corner cases which would not trigger the callbacks but exit with False immediately:

• https://github.com/loco-3d/crocoddyl/blob/devel/src/core/solvers/ddp.cpp#L69
• https://github.com/loco-3d/crocoddyl/blob/devel/src/core/solvers/ddp.cpp#L108

3. Increasing regularisation is linked to failure to apply Cholesky decomposition to the Quu matrix: So, what can lead to the computeDirection failing, leading it to increase regularisation? Primarily, if the Cholesky decomposition of the Quu matrix fails. Checking solver.Quu, we can see all zeros or nans - that triggered the failed Cholesky decomposition (not positive definite), but not the root cause of the issue.
4. Applying quasiStatic to get a suitable initial guess for us returns all zeros: This can have two root causes: Either we are in a singular configuration or something with the dynamic properties is wrong.
5. Changing q0 to a random configuration does not return valid quasi-static torques: Curiously, changing q0 = pinocchio.randomConfiguration(robot_model) does not fix the issue with zero quasi-static torques. This points to something with the dynamic properties of the model being wrong.
6. Checking the URDF shows 0 inertials and masses:
   

I agree that it might be challenging at first to identify this root cause without warnings - the failure case in your example isn't due to the problem formulation or Crocoddyl, but a wrong URDF in example-robot-data.

Hope this helps,
Wolfgang

[spykspeigel]

Thanks a lot, Wolfgang. It definitely helps as I can use the correct URDF now. Your answer also provides me insights on debugging which could be helpful in the future as well.

[cmastalli]

Thanks a lot, Wolfgang. It definitely helps as I can use the correct URDF now. Your answer also provides me insights on debugging which could be helpful in the future as well.

Nice! could you create a PR in the example-robot-data so others do not struggle again with this issue?

Thanks @wxmerkt! Indeed, your comments are very useful for anyone.