Jacobians calculation

[mikoff]

Hi,
let's say I want to obtain the derivatives of the residual w.r.t. the perturbation defined in the body frame.
The simplest example is the function f that maps the point from body to world frame. Perturbing the pose and taking the derivative is easy if I manually use first order approximation:

import sympy as sp
c, s, x, y = sp.symbols("c, s, x, y")
d, dx, dy = sp.symbols("d, dx, dy")
p_x, p_y = sp.symbols("p_x, p_y")

world_pose_body = sp.Matrix([[c, -s, x], [s, c, y], [0., 0., 1.]])
tau = sp.Matrix([[1., -d, dx], [d, 1., dy], [0., 0., 1.]])
p_b = sp.Matrix([[p_x], [p_y], [1.]])

f = world_pose_body * tau * p_b
jacobian = f.jacobian([d, dx, dy])

which produces the following result

print(jacobian)
>> Matrix([[-c*p_y - p_x*s, 1.0*c, -1.0*s], [c*p_x - p_y*s, 1.0*s, 1.0*c], [0, 0, 0]])

My question is how can I get this tangent-space jacobian using Symforce? I tried several things but didn't succeed:

import symforce.symbolic as sf

world_pose_body = sf.Pose2.symbolic("world_pose_body")
tau = sf.Pose2.symbolic("tau")
p_b = sf.V2.symbolic("p_b")

f = world_pose_body * tau * p_b

f.jacobian(tau.to_tangent())

>> [0, world_pose_body_0, -world_pose_body_1]
>> [0, world_pose_body_1, world_pose_body_0]

f.jacobian(tau.to_storage())
>> [p_b0*world_pose_body_0 - p_b1*world_pose_body_1, -p_b0*world_pose_body_1 - p_b1*world_pose_body_0, world_pose_body_0, -world_pose_body_1]
>> [p_b0*world_pose_body_1 + p_b1*world_pose_body_0, p_b0*world_pose_body_0 - p_b1*world_pose_body_1, world_pose_body_1, world_pose_body_0]

I certainly miss something, could you give me a hint on this?

[mikoff]

It was easy:

from symforce.ops import LieGroupOps
from symforce.ops import GroupOps

world_pose_body = sf.Pose2.symbolic("world_pose_body")
tau = sf.V3.symbolic("tau")

pose_perturbed = world_pose_body * sf.Pose2.from_tangent(tau)

f = pose_perturbed * p_b

f.jacobian(tau).subs(tau[0], 0.0)

However, according to the documentation the following expressions should be equal:

world_pose_body.compose(sf.Pose2.from_tangent(tau))
world_pose_body.retract(tau)

but they are not. Is it expected?

[aaron-skydio]

They should not be equal, we mention this in the docstring of Pose2 class (here). It's sort of a bug (or just weird behavior) in sphinx that it puts that docstring on that function in the docs - that docstring is from the base class LieGroup but does not apply to Pose2, since Pose2 overrides retract