search

cmm-21 / a2

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

analytical jacobian, derivate of leaf joint incorrect #41

Open lthiet opened 3 years ago

lthiet commented 3 years ago

Hello,

I'm stuck on a problem for the analytical jacobian and it would be nice to get a few pointers if that's not too late.

I did not manage to implement the analytical jacobian like described in the IK_notes, because I was not sure what to do when I needed to bring it back to world coordinates using FK.

Instead I derived the rotateVec function with respect to the angle, and computed it for each joint in the coordinate frame of the parent rigid body. The analytical jacobian matches the estimated jacobian but not for the last 4 degrees of freedom, unless I hard-code a specific value for the angle. That is, what I get for the last 4 degrees of freedom have the correct magnitude, but they are all offset by the same rotation.

Did anyone follow a similar implementation and managed to see what was wrong?

Also, the robot movement looks similar to the estimated Jacobian in the video, but because of the problem as described above I would not get full point right (hardcoding an angle into the derivative)? Would it be possible to know how many points I lose provided I do not manage to solve the issue as described above?

Thank you.

eastskykang commented 3 years ago

Could you print out estimated Jacobian and analytic Jacobian values and post here?

lthiet commented 3 years ago

In the end I managed to implement the analytical jacobian more like described in the IK notes. But the problem still persists.

On the left are values for the estimated jacobian and on the right is computed analytical jacobian, for 1 leg. All the derivation for each joint is correct except for the leaf one, where the magnitude is correct but rotation is incorrect.

c 0, r 0
est     1       comp    1       error   -1.10134e-13
c 0, r 1
est     0       comp    0       error   0
c 0, r 2
est     0       comp    0       error   0
c 1, r 0
est     0       comp    0       error   0
c 1, r 1
est     1       comp    1       error   -1.10134e-13
c 1, r 2
est     0       comp    0       error   0
c 2, r 0
est     0       comp    0       error   0
c 2, r 1
est     0       comp    0       error   0
c 2, r 2
est     1       comp    1       error   -1.10134e-13
c 3, r 0
est     0.2625  comp    0.2625  error   -4.37374e-10
c 3, r 1
est     0       comp    0       error   0
c 3, r 2
est     -0.135  comp    -0.135  error   2.24824e-10
c 4, r 0
est     0       comp    0       error   0
c 4, r 1
est     -0.2625 comp    -0.2625 error   4.37374e-10
c 4, r 2
est     -0.491446       comp    -0.491446       error   8.19044e-10
c 5, r 0
est     0.491446        comp    0.491446        error   -8.19044e-10
c 5, r 1
est     0.135   comp    0.135   error   -2.24824e-10
c 5, r 2
est     0       comp    0       error   0
c 6, r 0
est     0.491446        comp    0.491446        error   -8.19044e-10
c 6, r 1
est     0.075   comp    0.075   error   -1.24964e-10
c 6, r 2
est     0       comp    0       error   0
c 7, r 0
est     0       comp    0       error   0
c 7, r 1
est     0       comp    0       error   0
c 7, r 2
est     0       comp    0       error   0
c 8, r 0
est     0       comp    0       error   0
c 8, r 1
est     0       comp    0       error   0
c 8, r 2
est     0       comp    0       error   0
c 9, r 0
est     0       comp    0       error   0
c 9, r 1
est     0       comp    0       error   0
c 9, r 2
est     0       comp    0       error   0
c 10, r 0
est     0       comp    0       error   0
c 10, r 1
est     0       comp    0       error   0
c 10, r 2
est     0       comp    0       error   0
c 11, r 0
est     0       comp    0       error   0
c 11, r 1
est     0       comp    8.32667e-17     error   -8.32667e-17
c 11, r 2
est     0.491446        comp    0.491446        error   -8.18766e-10
c 12, r 0
est     0       comp    0       error   0
c 12, r 1
est     0       comp    0       error   0
c 12, r 2
est     0       comp    0       error   0
c 13, r 0
est     0       comp    0       error   0
c 13, r 1
est     0       comp    0       error   0
c 13, r 2
est     0       comp    0       error   0
c 14, r 0
est     0       comp    0       error   0
c 14, r 1
est     0       comp    0       error   0
c 14, r 2
est     0       comp    0       error   0
c 15, r 0
est     0       comp    0       error   0
c 15, r 1
est     0       comp    0       error   0
c 15, r 2
est     0       comp    0       error   0
c 16, r 0
est     0       comp    0       error   0
c 16, r 1
est     0.134239        comp    0.235612        error   -0.101373
c 16, r 2
est     0.245723        comp    0.151285        error   0.0944385
c 17, r 0
est     0       comp    0       error   0
c 17, r 1
est     0       comp    0       error   0
c 17, r 2
est     0       comp    0       error   0
c 18, r 0
est     0       comp    0       error   0
c 18, r 1
est     0       comp    0       error   0
c 18, r 2
est     0       comp    0       error   0
c 19, r 0
est     0       comp    0       error   0
c 19, r 1
est     0       comp    0       error   0
c 19, r 2
est     0       comp    0       error   0
nwicki commented 3 years ago

I just had the same problem. Instead of using the vector from the local joint to the point, I just used the point itself for every joint not dependent on q_i. I guess this is because it is a subtraction and gets cancelled through partial derivation.

Edit: Also, I hope we get a derivation outline in the solution because I would like to know if my assumptions are also correct in theory :)

lthiet commented 3 years ago

Mh, describing the point from the rigid body makes it that only the first joint (qIndex = 6) is correct now :(

However, using either the vector from RB to point or joint to point, both somehow converges and the visualization still look alright in both cases..

c 0, r 0
est     1       comp    1       error   -1.10134e-13
c 0, r 1
est     0       comp    0       error   0
c 0, r 2
est     0       comp    0       error   0
c 1, r 0
est     0       comp    0       error   0
c 1, r 1
est     1       comp    1       error   -1.10134e-13
c 1, r 2
est     0       comp    0       error   0
c 2, r 0
est     0       comp    0       error   0
c 2, r 1
est     0       comp    0       error   0
c 2, r 2
est     1       comp    1       error   -1.10134e-13
c 3, r 0
est     0.2625  comp    0.2625  error   -4.37374e-10
c 3, r 1
est     0       comp    0       error   0
c 3, r 2
est     -0.135  comp    -0.135  error   2.24824e-10
c 4, r 0
est     0       comp    0       error   0
c 4, r 1
est     -0.2625 comp    -0.2625 error   4.37374e-10
c 4, r 2
est     -0.491446       comp    -0.491446       error   8.19044e-10
c 5, r 0
est     0.491446        comp    0.491446        error   -8.19044e-10
c 5, r 1
est     0.135   comp    0.135   error   -2.24824e-10
c 5, r 2
est     0       comp    0       error   0
c 6, r 0
est     0.491446        comp    0.491446        error   -8.19044e-10
c 6, r 1
est     0.075   comp    0.075   error   -1.24964e-10
c 6, r 2
est     0       comp    0       error   0
c 7, r 0
est     0       comp    0       error   0
c 7, r 1
est     0       comp    0       error   0
c 7, r 2
est     0       comp    0       error   0
c 8, r 0
est     0       comp    0       error   0
c 8, r 1
est     0       comp    0       error   0
c 8, r 2
est     0       comp    0       error   0
c 9, r 0
est     0       comp    0       error   0
c 9, r 1
est     0       comp    0       error   0
c 9, r 2
est     0       comp    0       error   0
c 10, r 0
est     0       comp    0       error   0
c 10, r 1
est     0       comp    0       error   0
c 10, r 2
est     0       comp    0       error   0
c 11, r 0
est     0       comp    0       error   0
c 11, r 1
est     0       comp    0.0671196       error   -0.0671196
c 11, r 2
est     0.491446        comp    0.368585        error   0.122862
c 12, r 0
est     0       comp    0       error   0
c 12, r 1
est     0       comp    0       error   0
c 12, r 2
est     0       comp    0       error   0
c 13, r 0
est     0       comp    0       error   0
c 13, r 1
est     0       comp    0       error   0
c 13, r 2
est     0       comp    0       error   0
c 14, r 0
est     0       comp    0       error   0
c 14, r 1
est     0       comp    0       error   0
c 14, r 2
est     0       comp    0       error   0
c 15, r 0
est     0       comp    0       error   0
c 15, r 1
est     0       comp    0       error   0
c 15, r 2
est     0       comp    0       error   0
c 16, r 0
est     0       comp    0       error   0
c 16, r 1
est     0.134239        comp    0.117806        error   0.0164332
c 16, r 2
est     0.245723        comp    0.0756423       error   0.170081
c 17, r 0
est     0       comp    0       error   0
c 17, r 1
est     0       comp    0       error   0
c 17, r 2
est     0       comp    0       error   0
c 18, r 0
est     0       comp    0       error   0
c 18, r 1
est     0       comp    0       error   0
c 18, r 2
est     0       comp    0       error   0
c 19, r 0
est     0       comp    0       error   0
c 19, r 1
est     0       comp    0       error   0
c 19, r 2
est     0       comp    0       error   0
eastskykang commented 3 years ago

@lthiet it's a bit suspicious that your IK still works with wrong Jacobian matrix... could be just a coincidence. (did you try to give commands to the robot?)

Can you elaborate more on your derivation on rotateVec?

eastskykang commented 3 years ago

Also can you print the value of Jacobian in different configuration? (e.g. after few steps of simulation)

lthiet commented 3 years ago

In the end I used how we compute dp/dtheta like suggested in IK, so I don't use my derivation on rotateVec anymore but both methods give exactly the same results. I simply differentiated rotateVec with respect to alpha.

For the IK, I tried all combinations, and IK is stable and give similar results to when I used estimated Jacobian. It only starts to get unstable when I push the speed too much but when I reset it back to something smaller it gets stable again. The only difference is when I push the forward speed very high, the legs animation is a bit different.

Here are two videos to compare : https://youtu.be/M6D2RvxiCyM https://youtu.be/ct-tkY7cBtA

eastskykang commented 3 years ago

@lthiet it's a bit hard to see what's happening but this stillshot indicates something is wrong.

image

If your IK solution is correct, (given high forward speed command) the robot should try to stretch its leg straight toward the target. but here, this is not the case. There's no reason for the robot to bend its knee at the very moment of this stillshot. (recall that what we are doing is minimizing position error)

You can see, in the demo with numerical Jacobian, the robot tries to stretch its legs straight.

lthiet commented 3 years ago

That's correct, I think my IK is correct since with num. jacobian it does streches its legs. Here's another video, with analytical jacobian, showing that IK is somewhat stable if the distance between each target points is not too great. So something is wrong the analytical jacobian.

https://youtu.be/PJvP8jdcY8Y

I'm printing the values of est. and comp. Jacobian right now

eastskykang commented 3 years ago

@lthiet what happens if you give turning speed? Does it work okayish?

lthiet commented 3 years ago

Here are the values of estimated and analytical jacobian, after 4 time steps, for 1 leg only looking at qIndexes of interest.

Just like the snapshot suggests, the first two joints are still correct, but the last one (the one that makes the legs bend when they should not) is not correct

c 6, r 0
est     0.491446        comp    0.491446        error   -8.19044e-10
c 6, r 1
est     0.075   comp    0.075   error   -1.24964e-10
c 6, r 2
est     0       comp    0       error   0
c 11, r 0
est     0       comp    0       error   0
c 11, r 1
est     0       comp    8.32667e-17     error   -8.32667e-17
c 11, r 2
est     0.491446        comp    0.491446        error   -8.18766e-10
c 16, r 0
est     0       comp    0       error   0
c 16, r 1
est     0.134239        comp    0.235612        error   -0.101373
c 16, r 2
est     0.245723        comp    0.151285        error   0.0944385
###################
c 6, r 0
est     0.487655        comp    0.487655        error   -8.12809e-10
c 6, r 1
est     0.0776137       comp    0.0776137       error   -1.29453e-10
c 6, r 2
est     0       comp    0       error   0
c 11, r 0
est     -3.22235e-05    comp    0       error   -3.22235e-05
c 11, r 1
est     0.00601467      comp    0.00601476      error   -8.63282e-08
c 11, r 2
est     0.488063        comp    0.488063        error   -8.13288e-10
c 16, r 0
est     -0.000751398    comp    0       error   -0.000751398
c 16, r 1
est     0.140252        comp    0.239269        error   -0.0990166
c 16, r 2
est     0.24234 comp    0.145432        error   0.096908
###################
c 6, r 0
est     0.484034        comp    0.484034        error   -8.06721e-10
c 6, r 1
est     0.0795721       comp    0.0795721       error   -1.32848e-10
c 6, r 2
est     0       comp    0       error   0
c 11, r 0
est     -9.89237e-05    comp    0       error   -9.89237e-05
c 11, r 1
est     0.0104804       comp    0.0104809       error   -4.66873e-07
c 11, r 2
est     0.484764        comp    0.484764        error   -8.08077e-10
c 16, r 0
est     -0.00137134     comp    0       error   -0.00137134
c 16, r 1
est     0.145286        comp    0.242584        error   -0.0972989
c 16, r 2
est     0.239354        comp    0.139831        error   0.0995224
###################
c 6, r 0
est     0.480577        comp    0.480577        error   -8.0091e-10
c 6, r 1
est     0.0810177       comp    0.0810177       error   -1.35054e-10
c 6, r 2
est     0       comp    0       error   0
c 11, r 0
est     -0.000172389    comp    0       error   -0.000172389
c 11, r 1
est     0.0137805       comp    0.0137815       error   -1.07824e-06
c 11, r 2
est     0.481553        comp    0.481553        error   -8.02633e-10
c 16, r 0
est     -0.00187115     comp    0       error   -0.00187115
c 16, r 1
est     0.149577        comp    0.245617        error   -0.0960401
c 16, r 2
est     0.236692        comp    0.134434        error   0.102259
lthiet commented 3 years ago

Yes, it works okay with turning speed : https://youtu.be/Q_eg9Fkwmmk

By the way, I'm also content knowing if I can get partial point given the current state?

eastskykang commented 3 years ago

Unfortunately, passing the test for Ex.4 is baseline (of course it should not be hard-coded for only passing the test). We cannot give partial points if it does not pass the test.

lthiet commented 3 years ago

I see, can I give more details about how I computed the analytical jacobian or would that give too much details and might spoil it for others?

eastskykang commented 3 years ago

The most tricky part is partial derivative dp/dqj. In fact, you can express this with very simple equation: () x () where x is cross product.

Please see the second tutorial slides and think a bit more about this.

lthiet commented 3 years ago

Yes that's what I am doing. Thanks for the help. I guess it would be nice to see the derivation details afterward to see where I got it wrong.