DualQuaternion incorrectly initialising xyzw

[Nickick-ICRS]

Hi,

In general, the DualQuaternion class is quite awkward to use (e.g. it would be clean to have a from_SE3() etc. constructor), however when working with casadi arrays, the identity and transform_point functions appear not to work?

Example:

import numpy as ntp
import casadi as cs
from liecasadi import SE3, SO3, DualQuaternion

th_0 = cs.SX.sym('th_0', (1, 1))
th_1 = cs.SX.sym('th_1', (1, 1))
cs_pt = cs.SX.sym('pt', (3, 1))
cs_np_pt = cs_np.array([1, 0, 0])
ntp_pt = ntp.array([1, 0, 0])

R0 = SO3(cs.np.array([0, cs.sin(th_0/2), 0, cs.cos(th_0/2)]))
R1 = SO3(cs.np.array([0, 0, cs.sin(th_1/2), cs.cos(th_1/2)]))

T = DualQuaternion.from_matrix(SE3(
    pos=cs.np.zeros(3), xyzw=(R1 * R0).as_quat().coeffs()).as_matrix())

# Works as expected for numpy array
print(T.transform_point(ntp_pt))
# Works as expected for casadi.np array
print(T.transform_point(cs_np_pt))
# Crash with below error for casadi array
print(T.transform_point(cs_pt))

The error I get is:

NotImplementedError: Wrong number or type of arguments for overloaded function 'dot'. Possible prototypes are: dot(DM,DM) dot(SX,SX) dot(MX,MX) You have: '(SX,[SX])'

I've identified the problem is at line 200 in DualQuaternion.py (by extension also in line 188): p.Qd.xyzw = [xyz[0], xyz[1], xyz[2], 0]

Changing this to p.Qd.xyzw = cs.np.array([xyz[0], xyz[1], xyz[2], 0]) appears to solve the problem. I expect there are several other locations which may contain the same problem?

If you can confirm that I'm using the library as expected then this is a bug, with a pretty trivial solution, no? Then I don't mind opening a PR with a bug fix.

[traversaro]

fyi @Giulero

[Giulero]

Hi @Nickick-ICRS! Thanks for opening the issue :)

Yeah you're right. The problem is the wrong initialization of the quaternion in the transform_point method: The quaternion is initialized with a list. This is clearly wrong:

p.Qd = Quaternion([xyz[0], xyz[1], xyz[2], 0])

I will open a PR to fix this changing it to:

xyzw = cs.np.array([xyz[0], xyz[1], xyz[2], 0])
# xyzw = cs.vertcat(xyz[0], xyz[1], xyz[2], 0) 
p.Qd = Quaternion(xyzw)

Thanks again for opening the issue!

[Giulero]

Open PR #9.

[Giulero]

PR merged. @Nickick-ICRS could you please test if the fix does the job? Thanks!

[Nickick-ICRS]

Works for me!