extending the c++ interface for plotting the parametrization

[ndehio]

The new c++ method plot_parametrization(const int n_sample) reimplements the python-function plot_parametrization() from the file toppra/parametrizer.py.

The function is tested in the test test_toppra_approach.cpp. Note, however, that the output is similar but not identical to the python example. Your feedback is welcome.

[ndehio]

For the future: how can I apply your clang format?

[hungpham2511]

Hi @ndehio, thanks for the contribution. However, I do not suggest this approach for "debugging" because it can be quite limited. For instance, the script that you printed can only used to debug and nothing else. I think it is better to serialize only the object internal data into a file stream and save to disk, then write a single python script to visualize it. This serialized data stream can then be used for debugging, logging, etc..

Currently, this is implemented for the geometric path via the serialize/deserialize methods here

toppra/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp

Line 116 in 214942d

void PiecewisePolyPath::serialize(std::ostream &O) const {

For this feature, I propose to implement serialize/deserialize methods for ConstAccel instead. We can store the result of the most recent evaluation and serialize those altogether.

[ndehio]

I see your point, it makes sense to stay with the serialization approach.
However, did you have a look at the generated plot? I think there is still a problem with the c++ implementation of the parametrization.

[hungpham2511]

Suggested change

	Vector _velocities = this->m_vsquared;
	Vector _velocities = this->m_vsquared.cwiseSqrt();

[hungpham2511]

We have test cases that verify the consistency of the output parametrization between the python and C++ implementation, so that shouldn't be the cause.

There is 1 bug in your code, I have commented on it.

One reason why the results are different is that the input path is not the same as in the python example. You can try the following Python code instead

import toppra
import numpy as np

s_array = np.array([0, 1, 2])
wp_array = np.array([(0, 0), (1, 2), (2, 0)])
wpp_array = np.array([(0, 0), (1, 1), (0, 0)])
# path = toppra.SplineInterpolator(s_array, wp_array)

path = toppra.SimplePath(s_array, wp_array, wpp_array)

pc_vel = toppra.constraint.JointVelocityConstraint([[-1, 1], [-0.5, 0.5]])
pc_acc = toppra.constraint.JointAccelerationConstraint([[-0.05, 0.2], [-0.1, 0.3]])

instance = toppra.algorithm.TOPPRA([pc_vel, pc_acc], path)
instance.compute_parameterization(0, 0)

instance.problem_data.return_code
instance.problem_data.gridpoints  #doctest: +ELLIPSIS
instance.problem_data.sd_vec  #doctest: +ELLIPSIS

path_new = toppra.ParametrizeConstAccel(path, instance.problem_data.gridpoints, instance.problem_data.sd_vec)
path_new.plot_parametrization(show=True)

[hungpham2511]

Both python and cpp code should produce a figure similar to the one above.

[github-actions]

Stale pull request message