Switch Forward and Inverse Kinematics to leverage Eigen::Vector

[Levi-Armstrong]

@simonschmeisser and @gavanderhoorn Let use know if you see an issue with the planned changes.

We are planning to switch OPW Kinematics to leverage Eigen::VectorXd instead of a raw pointer for both forward and inverse kinematics.

• The forward kinematics would take a Eigen::VectorXd.
• The inverse kinematics would return a std::vector<Eigen::VectorXd> which would be of size 8 and any solution that had nans would include a empty Eigen::VectorXd.

[Levi-Armstrong]

@marip8

[marip8]

Including @JeroenDM in this discussion as well.

To clarify the previous comment, this is what we're envisioning

+ template<typename T>
+ using Solution = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template <typename T>
- void inverse(const Parameters<T>& params, const Transform<T>& pose, T* out) noexcept;
+ std::vector<Solution> inverse(const Parameters<T>& params, const Transform<T>& pose) noexcept;

template <typename T>
- Transform<T> forward(const Parameters<T>& p, const T* qs) noexcept;
+ Transform<T> forward(const Parameters<T>& p, const Solution& qs) noexcept;

Some questions to consider:

• Should the vector of solutions always be length 8 even if some solutions from OPW contain nan values?
  • If yes:
    • Should Solution be a dynamic vector, capable of being length zero for invalid solutions, or should it be a fixed length of size 6 with invalid solutions as nan values?
    • Are there benefits of using std::array instead of std::vector as the solution container?

@Levi-Armstrong and my current thoughts are that the vector of solutions should always be length 8, but invalid solutions should be represented by an Eigen dynamic vector of length 0

[gavanderhoorn]

@marip8 wrote:

* Are there benefits of using `std::array` instead of `std::vector` as the solution container?

the former avoids dynamic memory allocation, as it's a compile-time sized type. As for a 6dof there is an a-priori known max, using a statically allocated type offers a nice, if minimal, optimisation opportunity (ie: it's free, doesn't limit and does not have any corner cases).

As to whether it's a sufficient advantage over a dynamically sized container is hard to say. It probably comes down to preference a bit.

Personally I like that:

• the intent is really clear: the solver is going to return 8 solutions
• no memory will be allocated by the solver (or at least not for the solutions)
• an entry containing NaN really means there was no solution "in that quadrant" / for that configuration: with a dynamically sized list you could wonder whether the solver actually considered all configurations or just returned "something" (ie: an arbitrary number < 8)
• std::array is completely identical to a regular double[..], making interfacing with C and other languages trivial (although std::vector is also guaranteed to be contiguously allocated, not all language wrappers/FFI can easily deal with vectors)

@Levi-Armstrong wrote:

We are planning to switch OPW Kinematics to leverage Eigen::VectorXd instead of a raw pointer for both forward and inverse kinematics.

Again, personally, I don't really like seeing plain Eigen::VectorXd types being used 'everywhere'. Reason being that it doesn't convey any semantics. It's just a vector of doubles, and those doubles could mean anything.

Perhaps a typedeffed (or using) version would already be better.

(you could of course say that a std::array<double, ..> also doesn't really have any semantics, and you would be right)

The nice thing about std::array though (besides the points I already mentioned) is that it's just a simple, contiguous array in memory. This makes it easy to use with other systems, while Eigen should still be able to Map it.

I don't recall whether it's a lot of effort to get at the raw data in a Eigen::Matrix.

---

Overall I'm not necessarily against this change, but I would at the very least like it if we could avoid using plain Eigen types.

I also don't believe there is really a satisfying argument either against or for this change. Especially not because we're talking about a maximum of 8 elements.

Perhaps it could help the discussion if you could provide your rationale for why you'd like to make this change. Right now it seems like a change just because you want to change it.

[Levi-Armstrong]

Again, personally, I don't really like seeing plain Eigen::VectorXd types being used 'everywhere'. Reason being that it doesn't convey any semantics. It's just a vector of doubles, and those doubles could mean anything.

I want to make sure this is what you are recommending is create a typedef as below then use the IKSolution throughout?

using IKSolution = Eigen::VectorXd;

Perhaps it could help the discussion if you could provide your rationale for why you'd like to make this change. Right now it seems like a change just because you want to change it.

The motivation is to have a consistent format throughout our motion planning libraries. Currently we are looking at updating descartes_light and tesseract format and would like them to be the same to avoid unnecessary data translations.

[marip8]

Perhaps it could help the discussion if you could provide your rationale for why you'd like to make this change. Right now it seems like a change just because you want to change it.

For me the reasons for this change are to:

• Eliminate ambiguity with the float pointer representation of joint solutions.
  • Currently there is an implicit assumption that the pointer points to 48 doubles, and that you should access them six at a time to get the unique solutions. Using a vector of vector of doubles is much more intuitive and less prone to user error.
• Return objects rather than modify output parameters

[JeroenDM]

Return objects rather than modify output parameters

I'm a big fan of this! It would be interesting to know what the performance implication of this when the calling code re-uses the output vector for different calls. But maybe that rarely happens in practice. Or maybe the compiler optimizes return arguments sufficiently.

As for the Eigen / std choice, I don't really have an opinion. I use them myself interchangeably and I'd be happy to update the corresponding MoveIt plugin when necessary.

[gavanderhoorn]

@JeroenDM wrote:

Or maybe the compiler optimizes return arguments sufficiently.

modern compilers (with RVO) will most likely generate equivalent code.

@marip8 wrote:

Return objects rather than modify output parameters

yes, this is indeed nicer.

In #50, std::array is used for both individual Solutions and the aggregate set of Solutions.

If we're going to move away from out-parameters, there is no way for someone not reading the code (and us promising it in documentation) to verify whether memory is allocated or not.

In that case, using std::vector would probably be OK as well -- but you might lose the ability to use Eigen::Map, I haven't checked.

[gavanderhoorn]

Final comment: @Jmeyer1292 might've implemented opw_kinematics the way he did to avoid depending on any 3rd party libraries. opw_kinematics was trivially buildable on C++11 supporting platforms.

The introduction of Eigen has made this no longer true.

[marip8]

opw_kinematics has had a dependency on Eigen since the original commit, so this change doesn't introduce any new dependencies

[gavanderhoorn]

Ok, then that's not a concern indeed.

[Levi-Armstrong]

In that case, using std::vector would probably be OK as well -- but you might lose the ability to use Eigen::Map, I haven't checked.

In the process I also tested the std::vector and it also works with Eigen::Map because it stores the items continuous in memory. This is nice and I was not aware you could do this with Eigen::Map.