Log-derivative with a single non-singular task
Suppose that the pose task has a non-singular Jacobian, and we have a single task in our inverse kinematics problem. Surprisingly, in that case a simplification occurs and the log-derivative has no effect on the resulting velocity:
J(q) non-singular ⇒[J(q)q˙=−αe(q) ⇔ bJ0b(q)q˙=−αe(q)]Note the square brackets in this formula: the two Jacobian matrices are not equal, which would have been ∀v,J(q)v=bJ0b(q)v; rather, they define the same solution q˙ to the task.
Here is one way to prove this formula:
J(q)q˙q˙q˙q˙=−αe(q)=−αJ(q)−1e(q)=−αbJ0b(q)−1Jlog6(Ttb)−1e(q)=−αbJ0b(q)−1e(q)where the last step derives from the general identity Jlog6(T−1)−1log6(T)=log6(T). We can check this identity analytically with the simpler formula for Jlog3 on SO(3) (MLT: equations (143) and (144)). Denoting by θ=log3(R), we have:
Jlog3(R−1)−1log3(R)=[I−θ21−cosθ[−θ]×+θ3θ−sinθ[−θ]×2]θ=θ−(…)[θ]×θ+(…)[θ]×2θ=θwhere we used the fact that v×v=0 for any vector v. The proof on SE(3) using the more complicated formula of Jlog6, which is listed for instance in the Jlog6 documentation, is left to the reader 😉 We can also check it numerically:
>>> import pinocchio as pin
>>> import numpy as np
>>> T = pin.SE3.Random()
>>> pin.log6(T)
v = -0.0952361 1.04225 -0.718962
w = -0.85911 -2.44664 0.51629
>>> pin.Motion(np.linalg.inv(pin.Jlog6(T.inverse())) @ pin.log6(T))
v = -0.0952361 1.04225 -0.718962
w = -0.85911 -2.44664 0.51629
where we called np.linalg.inv for illustrative purposes. (In practice, it is better to np.linalg.solve(A, b) rather than np.linalg.inv(A) @ b. Don't invert that matrix.)
Log-derivative with multiple weighted tasks
A key question in inverse kinematics arises when we have multiple tasks that cannot be all fulfilled at the same time. As of writing this note, the two main answers to this question are weighted and lexicographic optimization. In weighted optimization, we associate a weight w>0 to each task, and penalize task residuals more for tasks that have higher weights. This is the approach followed by Pink, an inverse kinematics library in Python based on Pinocchio.
Before v0.8.0, the pose task in Pink implemented the frame Jacobian bJ0b(q) as task Jacobian. This had worked in practical cases from the library's users, until we tried to apply it in conjunction with a damping task to an omnidirectional wheeled robot. Here is the resulting behavior in Pink v0.7.0:
Upgrading to Pink v0.8.0, which added the missing log-derivative to the Jacobian formula, running the same code results in:
The corresponding code is available in the omnidirectional_wheeled_robot.py example from the project repository. The numerical instability before v0.8.0 is related to the fact that this example weighs position and orientation coordinates differently, and that the task is in competition with a damping task. If we suppose no constraint, the quadratic program solved by the IK can be written as:
v∗=argvmin21vTPv+qTv ⇔ Pv∗=−qWith our pose task and a damping task, the matrix P and vector q can be expressed as:
JlJbPq:=−Jlog6(Ttb):=bJ0b(q)=JbTJlTWJlJb+λI=−JbTJlTWewhere W is a diagonal weight matrix applied to the position and orientation coordinates of our pose task (docs). If there were no damping task (λ=0), we could simplify both sides of the equation Pv∗=−q by the inverse (WJlJb)−T, resulting in the simplification:
Pv∗JlJbv∗Jbv∗v∗=−q=−e=−Jl−1e=−e=−Jb−1ewhere we used again the identity Jl−1e=e. Again, in the absence of another task, we see how using the proper J=JlJb or the frame Jacobian Jb results in the same behavior. However, as soon as we add the damping task (λ>0) we can unroll the same derivation and the resulting solution becomes:
v∗=−[Jb+λJl−1W−1Jl−TJb−T]−1eBecause of the damping task, the log-derivative Jl is not simplified away any more and affects the resulting velocity. If we had used the frame Jacobian Jb as task Jacobian, we would have obtained a different velocity:
vwrong=−[Jb+λW−1Jb−T]−1eThe latter is the velocity that was computed by Pink v0.7.0, the former is the velocity computed by Pink v0.8.0.