CLF: line 225, AssertionError in ControlLaw assert result.is_success()

[dvogureckiy99]

I have created fork of this repository for my Solo12 robot, but I have got error when I try run one of the control methods.

Traceback (most recent call last):
  File "./simulate.py", line 190, in <module>
    simulator.AdvanceTo(sim_time)
  File "/home/dvogureckiy99/git/quadruped_drake/controllers/basic_controller.py", line 305, in DoSetControlTorques
    u = self.ControlLaw(context, q, v)
  File "/home/dvogureckiy99/git/quadruped_drake/controllers/clf_controller.py", line 225, in ControlLaw
    assert result.is_success()
AssertionError

[vincekurtz]

Hi Dmitriy,

Unfortunately I don't currently have the bandwidth to help debug your Solo12 implementation in detail, but here's what I know.

That error message indicates that the CLF-QP is infeasible for some reason. You can read more about the CLF control method here. It's likely that something about the different robot model is leading to infeasibility. You will need to change the high-level planner to be consistent with the new robot, for example. Once you have a sense of what the CLF-QP is trying to accomplish, you might check out the Drake tutorial on debugging mathematical programs to find which constraints are causing the infeasibility.

[dvogureckiy99]

Thank you very much, Vince Kurtz :)

Sorry for next spamming, but I will write all this for the case when you have got enough bandwidth for reviewing it.

But this error appears also when I setup ID,PC,CLF algorithms.

  File "/home/dvogureckiy99/git/quadruped_drake/controllers/inverse_dynamics_controller.py", line 224, in ControlLaw
    assert result.is_success()
AssertionError

And I forgot to mention that all this methods return: EXIT: Optimal Solution Found. so that optimization goes well.

With B,MPTC methods there is another errors: Failure at multibody/plant/tamsi_solver.cc:218 in SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0' failed.

[dvogureckiy99]

I found out what is causing the error: as soon as I reduce the body mass to at least 2 or change center of mass coordinates, or change some of joint coordinates, this error appears. I can assume that for some reason the presented algorithm stops working as soon as we change the parameters of the robot at least a little.

If I instead of assert result.is_success() write assert result.get_solver_details() I am getting error:

RuntimeError: Actuation input port for model instance quad contains NaN.

Maybe there is a problem with OsqpSolver and if I would use GurobiSolver() the problem will disappear ?

[dvogureckiy99]

I try increase masses from zero values of foot links to enable relevent computations, including better collision detection. With B method I get new error:

RuntimeError: MultibodyPlant's discrete update solver failed to converge at simulation time = 0.495 with discrete update period = 0.005. This usually means that the plant's discrete update period is too large to resolve the system's dynamics for the given simulation conditions. This is often the case during abrupt collisions or during complex and fast changing contact configurations. Another common cause is the use of high gains in the simulation of closed loop systems. These might cause numerical instabilities given our discrete solver uses an explicit treatment of actuation inputs. Possible solutions include:
  1. reduce the discrete update period set at construction,
  2. decrease the high gains in your controller whenever possible,
  3. switch to a continuous model (discrete update period is zero),      though this might affect the simulation run time.

I reduced dt to 1e-4 and simulation goes well, but with this method robot stay at one place and simulating doesn't run.

With method MPTC,PC,CLF and dt to 1e-4 simulating doesn't run and stopped there File "/home/dvogureckiy99/git/quadruped_drake/controllers/basic_controller.py", line 129, in Cv_fcn return self.plant_autodiff.CalcBiasTerm(self.context_autodiff)

With method ID and reduced dt to 1e-3 and Oh miracle the robot went.

[dvogureckiy99]

Hi Dmitriy,

Unfortunately I don't currently have the bandwidth to help debug your Solo12 implementation in detail, but here's what I know.

That error message indicates that the CLF-QP is infeasible for some reason. You can read more about the CLF control method here. It's likely that something about the different robot model is leading to infeasibility. You will need to change the high-level planner to be consistent with the new robot, for example. Once you have a sense of what the CLF-QP is trying to accomplish, you might check out the Drake tutorial on debugging mathematical programs to find which constraints are causing the infeasibility.

There is still a problem: when I setup initial z coordinate of body very much I have error:

RuntimeError: Actuation input port for model instance quad contains NaN.

Of course it can be solved by correct calculation of initial height given the values of the initial angles.

I tried this, but it doesn't help: a Nominal base position in basic_controller should be like in simulate.py.

[dvogureckiy99]

I solved the problem by adding 92 line of code p_body_nom[2] = 0.24094614699109293 to clf_controller.py. I think this way you can make any controller work.