Update MatrixLog6 and MatrixLog3

[omartin2010]

somehow, in certain cases, I was unable to generate a trajectory without getting an error that the operator / can't work between list (omgmat) and float... so changing that fixed it for me.

[HuanWeng]

Hi Olivier,

Thank you for your work. It is a nice catch! It seems to me that the issue comes from a non-float output from MatrixLog3. Maybe we need to fix that function?

Do you have examples getting errors?

Many thanks, Huan Weng

[omartin2010]

I don't recall the exact scenario but I'm working on a project where I do the last project I had to do on the Coursera class, and am trying to do it on a real physical robot so while converting it to a velocity inverse kinematics approach (and also not a youbot, but rather a diffdrive lego robot), I was initially doing tests just to get it moving in the X direction, so set everything else to 0. If I recall correctly, this is when I've had this operation fail too. Also, I had a division by zero at some point that I've had changed through calculating the limit of the expression so that we get a proper output. I's in the MatrixLog function. Let me add this to the PR. Let me know your thoughts.

[HuanWeng]

Hi Olivier,

The issues you reported are quite interesting and I also talked with Prof. Lynch, one of the authors of the textbook. Basically we may need to suspend your pull request until some relevant examples for those errors are provided. If some similar errors with examples reported, which haven't happened for the past two years, I will definitely accept your fix and even rewrite the function somehow.

For your error about division by 0, I checked the algorithm and code for MatrixLog3. The code strictly follows the algorithm mentioned in the textbook, discussing 3 different conditions and checking one be one: (a) If R == IdentityMatrix (b) Else if tr(R) = -1 (c) Else Conceptually whenever theta = 0, the R matrix will be an identity matrix and it will go into condition (a) instead of (c). So it is not necessary to add any "if" statement to check theta any more. However, there will be some computational errors from computers such as '1.00000 != 1' and we tried to avoid these errors by using function 'NearZero' in condition (a) and redefining cos(theta) into the range [-1, 1] in condition (c). If we assume a correct R is provided as the input of this function, then in order to recur the error you met, this R should be special enough that it is pretty close to the identity matrix but doesn't satisfy the judgement NearZero(np.linalg.norm(R - np.eye(3))) in the code of condition (a). I think that case is super rare and maybe impossible. I haven't proved the possibility yet but if that is possible, I will merge your fix and rewrite the function to calculate theta = arccos((tr(R) - 1) / 2.0) first and then discuss the conditions based on the value of theta I get, instead of following the steps in the textbook, hoping that can be more efficient and avoid any computational error.

For your error about non-float output from MatrixLog3, I think it is pretty weird because the return values for different conditions should be all floats. It will help us a lot if we have some examples.

Anyway, thank you for your long-term concern about our code. Hope my explanation makes sense to you. Let me know your thoughts.

[omartin2010]

I get what you're saying and I thank you for taking the time to go through the explanation. Somehow I was indeed having the problem, and now I can't repro for now. So I've undid my changes, kept them ready to reapply should the problem resurface, and I'll reopen that issue/pr with specific inputs that break it if it does happen. thanks!

[omartin2010]

Got a repro :

import modern_robotics as mr import numpy as np T = np.array([[0,0,-0.9998082, -0.28284], [0,-1,0,0], [-0.99998082,0,0,-0.28284], [0,0,0,1]]) mr.MatrixLog6(T) Traceback (most recent call last): File "", line 1, in File "/src/repos/RoboTeam/odometry/modern_robotics.py", line 370, in MatrixLog6

• (1.0 / theta - 1.0 / tan(theta / 2.0) / 2) \ TypeError: unsupported operand type(s) for /: 'list' and 'float'`

Also, another repro is this :

import modern_robotics as mr import numpy as np R = np.array([[0.99999537671646099, -3.0053589173723027e-06, 1.459270762282916e-05], \ ... [2.9147968441247535e-05, 1.000028863655217, 2.9044591635465217e-05], \ ... [1.4593265857620885e-05, -2.9017323016039543e-06, 0.9999953761582262]]) mr.MatrixLog3(R) Traceback (most recent call last): File "", line 1, in File "/src/repos/RoboTeam/odometry/modern_robotics.py", line 169, in MatrixLog3 return theta / 2.0 / sin(theta) * (R - np.array(R).T) #EDIT OLIVIER ZeroDivisionError: float division by zero

Problem is that acos(1) = 0 and the return divides by sin(0) which is 0... Am I missing something? I can modify the trigger for NearZero to be smaller but I had already made it to 1e-3 instead of 1e-6. I guess the point is that eventually if you get a value > 1 for acosinput, it will be set to 1 and the same will happen.

In this case, the MatrixLog6 function, I've also edited to get a value close to 1 but not quite one on acos input

if acosinput > 1: # Changed from > to >= ### EDIT TO REPRO acosinput = 0.999999 # 0.999999 # Changed from 1 to 0.999999 elif acosinput < -1: acosinput = -1
theta = acos(acosinput)

So that theta isn't quite 0 even if very close.

Thoughts?

[HuanWeng]

Hi Olivier,

I tested your code, after uninstalling the modern_robotics (MR) package I had before, directly zip downloading the latest MR package from here and cp'ing into my workspace. Here is the code in my script.py:

import modern_robotics as mr
import numpy as np

T = np.array([[0,0,-0.9998082, -0.28284], [0,-1,0,0], [-0.99998082,0,0,-0.28284], [0,0,0,1]])
print(mr.MatrixLog6(T))

R = np.array([[0.99999537671646099, -3.0053589173723027e-06, 1.459270762282916e-05],[2.9147968441247535e-05, 1.000028863655217, 2.9044591635465217e-05],[1.4593265857620885e-05, -2.9017323016039543e-06, 0.9999953761582262]])
print(mr.MatrixLog3(R))

And here is the result:

So MatrixLog6 works well in my laptop but MatrixLog3 doesn't in this case. The result print(mr.MatrixLog3(R)) should be a zero matrix. The reason of the bug is that the matrix jumped into condition (c) in that function and the return value was divided by 0, as you met. Then I modified the function MatrixLog3, just printing out np.linalg.norm(R - np.eye(3)) at beginning to see if R was close to identity matrix. The result is 5.4885520678691454e-05, larger than the threshold in the function NearZero. So I changed in NearZero from 1e-6 to 1e-4 and then the problem fixed.

I ran this test in both python2.7 and 3.6. I don't know if you changed the source code somewhere or by mistake, but I guess you can try reinstalling/using the latest version of our package. Any thoughts?

[omartin2010]

What about the case where, in the case where acosinput > 1, we set it to 1 (in MatrixLog6), and then theta = acos(1) = 0... and inevitably if theta = 0... the return statement fail because there's 1/0 in there.

So unless I set the acosinput = something very close to 1 but not quite there (0.999999), it will work. And if we put the NearZero boundary to be close to zero, aren't there cases where it yield 1e-3 and if the threshold is set to 1e-4, it yield False to the NearZero call, and with a quick analysis : if np.trace(R) = 3.1 for example, acosinput = (3.1 - 1) /2 which is > 1, theta becomes 0 and we have the problem again.

This is without looking at the other changes I had suggested but this is an edge case that does happen perhaps closer to a singularity situation (I'm driving a diffdrive robot and this function gets called in the feedbackcontrol fonction (I've done Prof Lynch's online course :) - but don't have enough experience in the topic to yet find other ways), I conclude that the limit is perhaps better suiting this function and the NearZero might not even be needed if you'd simply define the function as :

def MatrixLog6(T):
  R, p = TransToRp(T)
  acos = (np,trace(R) - 1) / 2.0
  if acosinput > 1:
    acosinput = 0.999999
  elif acosinput < -1:
    acosinput = -1
  theta = acos(acosinput)
...

[HuanWeng]

I think redefining acosinput may be not reasonable or useful because any case which needs to be redefined should have jumped into condition (a) and (b) unless computational errors still exist. And that cannot prevent the error of division by 0.

I agree with your new fix of this function (probably MatrixLog3 as well). That is actually what I suggested to Prof. Lynch when we talked. Basically it is like calculating theta first and discussing the conditions based on the value of theta. I think that may be more efficient and robust. I didn't want to change it because it doesn't follow the steps mentioned in the textbook (it is not a big deal though) and no one reported any bug. Since we now get this bug I am definitely happy to merge your current changes. Let me know if you can make that changes or I will update these soon, as well as those in MATLAB and Mathematica versions.

Thank you again for improving our package!

[omartin2010]

I've edited the file in the PR, It should be ok now and it does incorporate the change from > 1 to >=1 and maxing acosinput to 0.99999. Let me know if that change looks sound to you.

I forgot to change the output of the MatrixLog3 function so that the proper type is returned and we don’t need to do a type cast from within MatrixLog6. Do you want to do it?

cheers

[HuanWeng]

Great! I will modify them somehow and let you check if you don't mind.

[HuanWeng]

I modified these 2 functions. Please check: 6de81fb
I tested the original examples and your examples. They work. Any thoughts? Again, I still don't think MatrixLog3 will give non-float numbers since it always outputs np.array.

Merry Christmas!