search

cmm-21 / a2

Assignment 2 - Kinematic walking controller
5 stars 0 forks source link

Play unstable #10

Closed Glanfaloth closed 3 years ago

Glanfaloth commented 3 years ago

For ex.2, the trotting sometimes works fine. But the app window sometimes closes after a few seconds, sometimes closes immediately after clicking play button. Any idea why? Thank you! (I am using Gauss Newton)

Glanfaloth commented 3 years ago

The jacobian matrix turns nan nan nan... at some point, what could be the reason

eastskykang commented 3 years ago

Could be two reasons.

  1. wrong Jacobian matrix.
  2. numerical instability.

I will leave a comment regarding the numerical instability.

Gauss-Newton update includes (JTJ)-1JT term. Here, computing inverse matrix (JTJ)-1 by .inverse() is never a good idea. (what would happen if (JTJ) is singular and not invertible?)

Rather use linear solver as I suggested in the hint:

// - when you compute inverse of the matrix, use ldlt().solve() instead of inverse() function. this is numerically more stable.
//   see https://eigen.tuxfamily.org/dox-devel/group__LeastSquares.html
Glanfaloth commented 3 years ago

Thank you Dongho, I did use ldlt().solve() for inversion. Could the Jacobian matrix be wrong even when the doggy trotts perfectly before the app crashes?

eastskykang commented 3 years ago

@Glanfaloth I think I need more information. Could we discuss about this in Thursday's tutorial session? Meanwhile, you may want to try this in debugging mode to identify what causes the problem. In debug mode, eigen library will give you more rich information that help you to find the problem. (if the app crashes, debugger will stop at the exact point causing the problem)

Glanfaloth commented 3 years ago

Debugging tells me "Assertion failed: (IS_ZERO( (getRotationQuaternion(gamma, c) getRotationQuaternion(beta, b) getRotationQuaternion(alpha, a) * q.inverse()) .vec() .norm() / 10e5)), function computeEulerAnglesFromQuaternion, file ./src/libs/utils/include/utils/mathUtils.h, line 308." I don't understand...

eastskykang commented 3 years ago

This indicates the robot's state value were diverged.

I think we should identify when the Jacobian exactly turns to nan. I'd like to suggest you to simulate the IK controller with the button here. It advances just one step of simulation. Print out Jacobian matrix, and see how the value changes as you do the simulation.

Screenshot 2021-03-25 at 23 00 29

Also please record your simulation and upload your video here (you can use YouTube or any cloud that allow you to share your video file). That would help to discuss.

Glanfaloth commented 3 years ago

I already tried printing out Jacobian matrix. The results differ from time to time. Sometimes it turns nan immediately and sometimes the it lasts for one minute before crashing. Averagely things go wrong after around 10 sec. Here is a screenshot of the moment when the Jacobian changes to nan: image

eastskykang commented 3 years ago

@Glanfaloth Please try not overwriting Jacobian matrix but rather declare another matrix variable. Let me know if it works.

i.e. not this 

J = J.block(0,6,3,q.size() - 6);

but try this.

Matrix J2 = J.block(0,6,3,q.size() - 6);
// and use J2 for inverse kinematics

I also experienced a similar problem with clang compiler once.

Glanfaloth commented 3 years ago

Wow it works now!!! Thank you so much!!!!