C++ Implementation

[hungpham2511]

We will use this issue to flesh out some key designs of the C++ interface.

There are a few core components that are required.

• [x] Trajectory representation;
• [x] Constraint representation;
• [x] LP solver wrapper;
• [x] Core TOPPRA pipeline;
• [x] Matrix manipulation libraries in C++?

I am most inexperienced with trajectory representation in C++. Use OpenRAVE before with its representation, but bringing it over is a huge dependency that I am not willing to make. In the Python implementation I am basically using scipy.interpolate extensively.

[jmirabel]

Here is what I am most familiar with:

• matrix representation: Eigen
• trajectory representation: there are two different things to represent: the path (i.e. a non time-parameterized curve in the configuration space) and the time parameterisation. I don't have strong opinion on this but I think it should be independent of OpenRave.
• constraint representation: I guess here, we need a dynamic library. I am very familiar with Pinocchio and I think it is a good candidate for this task.

That being said, I think the core TOPPRA pipeline is agnostic of the trajectory and constraint representation, as long as we define an abstract base class. I think it would be great to be able to use the C++ implementation with any dynamic library and with any trajectory representation.

We could then add a second library in the repo that contains the implementation of the constraints using the dynamic library of our choice and some trajectory representation.

We should also discuss what license the code will have. Do you have any constraints on this ?

[jmirabel]

One extra mention: Pinocchio could also be used to interpolate the trajectories. It does the interpolation in Lie groups.

[hungpham2511]

Totally agree on the abstract base class. I propose we make use of the current Python interfaces. Have a look:

• constraints: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/constraint/linear_constraint.py#L7
• trajectory/geometric path: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/interpolator.py#L84
• LP/QP solver wrapper interface https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/solverwrapper/solverwrapper.py#L42

The main toppra algorithm is relatively straightforward once we have all the pieces working.

Regarding license, I propose to use the MIT license, the same as the Python codebase. What do you think?

Another point is testing, I would like the code to have good unit test coverage. Shall we use google-test?

[jmirabel]

• constraints: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/constraint/linear_constraint.py#L7

Any reason not to use this one: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/constraint/constraint.py#L30

Regarding license, I propose to use the MIT license, the same as the Python codebase. What do you think?

MIT is fine for me.

Another point is testing, I would like the code to have good unit test coverage. Shall we use google-test?

I almost never used google-test but I guess it isn't hard to use.

[hungpham2511]

• constraints: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/constraint/linear_constraint.py#L7

Any reason not to use this one: https://github.com/hungpham2511/toppra/blob/c013302cf78fe369c61a293789ee32cac0f4f759/toppra/constraint/constraint.py#L30

Regarding license, I propose to use the MIT license, the same as the Python codebase. What do you think?

MIT is fine for me.

Another point is testing, I would like the code to have good unit test coverage. Shall we use google-test?

I almost never used google-test but I guess it isn't hard to use.

LinearConstraint has a more concrete signature, and covers a good range of constraints. The more abstract Constraint is to provide support for other forms of constraints (such as conic, etc). These constraints can't be handled by qpOASES anyway.

Feel free to open PRs to develop branch directly whenever you have something. By today I will set up a skeleton for the cpp code. Please let me know what you will be working on in this thread so that we don't work on the same thing.

Edit: I see that https://github.com/stack-of-tasks/pinocchio is header-only. Would you recommend that approach? Not entirely sure of the advantages/disadvantages myself.

[jmirabel]

Feel free to open PRs to develop branch directly whenever you have something. By today I will set up a skeleton for the cpp code. Please let me know what you will be working on in this thread so that we don't work on the same thing.

Ok.

Edit: I see that https://github.com/stack-of-tasks/pinocchio is header-only. Would you recommend that approach? Not entirely sure of the advantages/disadvantages myself.

Pinocchio is a lighter dependency than OpenRAVE. It is close to be minimalist for the need of TOPP-RA. The torque limit constraint can be easily implemented using RNEA. A good option for this library regarding torque limits is:

• provide an abstract class with a pure virtual method computeTorque(q, v, a) and
• provide an implementation of the torque limits using Pinocchio that can be enabled / disabled with a CMake flag.

[hungpham2511]

I have the boilerplate setup. A tentative implementation plan to get the library to a decent state could consist of:

• [x] Abstract path interface;
• [x] A simple polynomial path class (for testing);
• [ ] More complex path implementation (probably not this time);
• [x] Abstract constraint interface;
• [x] Simple acceleration/velocity constraints;
• [x] Torque constraints (pinocchio, openrave);
• [ ] Cartesian accel/velocity constraints (probably not this time);
• [ ] Linear frictional constraints (probably not this time);
• [x] Wrapper for qpOASES;
• [x] TOPPRA algorithm;

I will be working on the first two this week.

[jmirabel]

Thank you. I will start working on the constraints by tomorrow. not this time mean left for future developments ?

[quangounet]

@jmirabel For TOPP in Lie groups we have a paper (https://www.ntu.edu.sg/home/cuong/docs/TOPPSO3SE3.pdf) and an implementation (https://github.com/dinhhuy2109/TOPP-SO3). You could perhaps re-use some derivations.

[jmirabel]

@quangounet Thanks for the pointer. I think I read this article when I implemented splines in Lie groups in HPP.

[jmirabel]

By the way, @quangounet, when I implemented the splines, I first looked for how to compute weighted centroids in SO(3) because that would be a more appropriate way to have splines in SO(3) (rather than in so(3), then integrated). I couldn't find anything (without optimization). Would you have any pointer on this ?

[jmirabel]

How do we organize ourselves for implementation of TOPPRA algorithm ?

[hungpham2511]

The TOPPRA algorithm is quite straightforward to implement given the available solver wrappers. I can implement something quite quickly. I would probably use the same interface as the Python implementation, what do you think?

https://github.com/hungpham2511/toppra/blob/24ab163c1999e04c02075919145351ae140f370e/toppra/algorithm/algorithm.py#L14

The Python impl has a provisional double inheritance for non-TOPPRA algorithm. Sadly it is never used. For this C++ impl, I think we only need a single abstract interface and two derived classes: TOPPRA and TOPPRAsd (specified duration). Other variants (smoothing, tracking etc) can then be added in by subclassing.

Another thing we need to do is recovery of the final trajectory once a parametrization is available, as in here.

https://github.com/hungpham2511/toppra/blob/24ab163c1999e04c02075919145351ae140f370e/toppra/algorithm/algorithm.py#L89

Also, more tests for the solver wrappers/constraints are always good.

[Linjackffy]

@jmirabel manif maybe a good choice for implementing splines in Lie groups. It provides many useful curves like bspline and bezier.

[jmirabel]

@Linjackffy What is manif ?

[jmirabel]

About the architecture of the TOPPRA algorithm, yes we can keep the current structure. I would only put the mechanism to find one available solver either outside the class or in a static function but not in the constructor, and let the constructor simply take a (pointer to a ?) solver.

[Linjackffy]

@jmirabel You could look at this webpage: manif

[jmirabel]

Thanks. I know personally the authors of this work. It is an interesting work. I think this package was created after I implemented splines in Lie groups.

[jmirabel]

The main algorithm has been implemented so I think this issue can be closed. Bug reports or feature requests can go into their own specific issue. @hungpham2511 if you agree, I let you close.