cmower / optas

OpTaS: An optimization-based task specification library for trajectory optimization and model predictive control.
https://cmower.github.io/optas/
Other
101 stars 14 forks source link

Potential issue with Quaternion class #83

Closed cmower closed 1 year ago

cmower commented 1 year ago

While I've been implementing unit tests, I realized that the way we implement Quaternion multiplication is "backwards". See the test below.

https://github.com/cmower/optas/blob/ef1dbde50153c2fc38033878eabbc9bfa5a96b85/tests/test_spatialmath.py#L416-L426

I generate two random quaternions, then multiply them using optas.spatialmath methods, and then I perform the same calculation but with Scipy - and compare the output to check optas is getting the correct answer (obviously this is assuming Scipy always has the correct answer).

The issue is that Scipy has the opposite convention to optas. My thought is, should optas adopt the same convention as Scipy? Or should we stick with our own convention?

@joaomoura24, do you have any thoughts on this?

In terms of required effort to make the change, if we want to make the switch we need to modify the following methods.

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/spatialmath.py#L496-L506

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/models.py#L794-L828

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/models.py#L834-L840

One reason perhaps to stick with our current convention, however, is that if you look at how we compute general homogenous transforms (below) for robot links you can see it is the same way round as our current convention for quaternion multiplication.

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/models.py#L736-L742

joaomoura24 commented 1 year ago

The convention is that a transformation is always a post multiplication. Meaning if you have a transformation q0 followed by q1, then the combined transformation will be q1*q0. It seems to me that that's what scipy is doing, right?

I don't even understand how it would work correctly if it was otherwise, actually, but i seem to remember that at some point i changed the quaternion multiplication to post multiplication in one of the functions because it was giving the wrong result

joaomoura24 commented 1 year ago

Here I changed this line a while back to a pre multiplication. That's what it should be in my perspective:

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/models.py#L812

Indeed, i notice that for the homogeneous transformations you are using a post multiplication like so:

https://github.com/cmower/optas/blob/3eeeaf3309f75d6ffbc3b158055f651005b0e1d6/optas/models.py#L713

I don't know who that would work, but i also haven't tested it.

cmower commented 1 year ago

I guess what ever convention I started with a got working is what I stuck with - I am happy to stick with what we have for now if it's not causing an issue. Happy to close this issue? Can re-open if this becomes a problem.

joaomoura24 commented 1 year ago

Yeah, my feeling is that we will eventually have to look into it. But we can reopen it once we ran into problems.

cmower commented 1 year ago

Sounds good, closing for now.