Vibration occurs when self collision constraint enabled

[yanghanjiang]

Environment:

• placo version: 0.6.2
• placo installation type: installed from source
• python version: 3.8.10
• eigen version: 3.4.0
• OS version: ubuntu 20.04

Issue Details:
I'm trying to control a humanoid's upper body motion using PlaCo. To avoid self-collisions between the arms, I enabled the self-collision constraint. However, vibrations occur when the arms get close enough to trigger the self-collision constraint. When this happens, the joint accelerations show significant fluctuations, as depicted below:

In order to suppress these vibrations, I implemented acceleration limits based on velocity limits which already available in KinematicsSolver. Below are the code details:

  if (acceleration_limits)
  {
    problem.add_constraint(qd->expr(6) <= (dt * Eigen::VectorXd::Ones(N - 6) * 100.0 + robot.state.qd.bottomRows(N - 6)) * dt);
    problem.add_constraint((-dt * Eigen::VectorXd::Ones(N - 6) * 100.0 + robot.state.qd.bottomRows(N - 6)) * dt <= qd->expr(6));
  }

Math principle:

$$\frac{\left(\frac{\delta q}{dt}-v_{last}\right)}{dt} < a_u$$

$$\frac{\left(\frac{\delta q}{dt}-v_{last}\right)}{dt} > -a_u$$

It seems acceleration limits make effective, as the accelerations are clamped to 100.0.

However, an error occurs when I attempt to reduce the acceleration threshold from 100.0 to 10.0. The error log is as follows:

You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
Viewer URL: http://127.0.0.1:7000/static/
Traceback (most recent call last):
  File "tora_one_cartesian_regularization_collision.py", line 136, in <module>
    run_loop()
  File "/usr/local/lib/python3.8/dist-packages/ischedule/ischedule.py", line 107, in run_loop
    run_pending()
  File "/usr/local/lib/python3.8/dist-packages/ischedule/ischedule.py", line 80, in run_pending
    task.func()
  File "tora_one_cartesian_regularization_collision.py", line 112, in loop
    solver.solve(True)
RuntimeError: QPError: Problem: Infeasible QP (check your hard inequality constraints)

How to explain this error from a math perspective? The issue persists even after I changed the priorities of all tasks, such as the frame task and self-collision task, from hard to soft.
Is there a better solution to suppress these vibration? Should I consider adding a joint acceleration task, similar to the existing joint task? Any hints or suggestions would be greatly appreciated.

[Gregwar]

Hello, there are several points here so I address separately for clarity

1. Self collisions constraint

First, as a disclaimer, PlaCo self-collision system is quite experimental, it was not extensively tested.
About using self-collisions, some questions:

• Do you use velocity limits ?
• Do you have a L2 regularization task ?
• What collision model do you have (is it pure shape approximation, meshes...) ?
• The example documentation doesn't appear to have vibrations (see video], do you have a guess on what is different in your setup ?

2. About limiting accelerations

I understand what you suggest and I already considered this feature. However, I believe limiting the acceleration is not appropriate in a first-order solver.

Suppose you want an effector to reach a target with zero velocity. The effector will accelerate towards its target position to reduce its error, and the velocity will ramp according to your constraint. However, when reaching the target, the velocity will not be zero and because of the acceleration limit, it will not be possible to stop instantly. This will result in an overshoot, and the same problem is likely to happen again and again creating an oscillation.

Limiting acceleration requires either control of the velocity (like the damping "KD" term of a PID controller), or some proper trajectory planning. The QP IK solver is totally near-sighted and not able to do that. For those reasons, I currently don't believe it is a good idea.

3. Understanding QP Failure

QP failures happen when the constraints are not compatible. For example, it is possible that your acceleration limit makes it impossible to avoid hitting a joint limit.

Velocity and joint limits are currently always hard (I could consider adding an option to make them soft), you might want to try disabling them to understand if they are the source of conflict.

For debug purpose, you can consider:

try:
        solver.solve(True)
    except:
        solver.problem.dump_status()
        raise

This will dump the number of equalities and inequalities still remaining in the underlying problem that was formulated

I hope this helps!

[yanghanjiang]

Thanks for your comment! @Gregwar

1. Self Collision Constraint

• Do you use velocity limits ? Yes.
• Do you have a L2 regularization task ? Yes. It seems that I have a default L2 regularization task because I did not use solver.add_regularization_task to specify a custom one. I also added a posture task to try to keep the torso of the wheeled humanoid upright.
• What collision model do you have (is it pure shape approximation, meshes...) ? Meshes.
• The example documentation doesn't appear to have vibrations (see video), do you have a guess on what is different in your setup ? It's true that the video shows no vibrations occurring. As mentioned in Error occurs when running example humanoid_collisions.py from the repository placo-examples #6 I changed the self collision task priority from hard to soft with a weight 10.0, and it works well, no vibrations occured. I found that the example use pure shapes in collision constraint, which is different from my implemention, where I use meshes in collision task.
  So I tried using meshes in collision task in example, and then vibrations occured.
  
  Below are some source files that could reproduce the result:
  python script
  collisions config
  urdf file

3. Understanding QP Failure

I tried disabling joint limits and velocity limits, and no QP failures occurred, though robot configuration looked strange. This seems to confirm your suspicion.