Attempting to obtain same results with cpp as in python

[JadTawil-theonly]

Hello,

I am attempting to further verify that the cpp interface gives the same (or at least similar) results to the python interface.

I have added a few things to test_algorithm.cpp in order to figure this out.

First of all, at the top of test_algorithm.cpp, you have the following python code commented out:

import toppra as ta
import numpy as np

path = ta.SplineInterpolator([0, 1, 2, 3], [[0, 0], [1, 3], [2, 4], [0, 0]])

def print_cpp_code(p):
    out = ""
    for seg_idx in range(p.cspl.c.shape[1]):
        out += "coeff{:d} << ".format(seg_idx)
        for i, t in enumerate(p.cspl.c[:, seg_idx, :].flatten().tolist()):
            if i == len(p.cspl.c[:, seg_idx, :].flatten().tolist()) - 1:
                out += "{:f};\n".format(t)
            else:
                out += "{:f}, ".format(t)
    return out

print(print_cpp_code(path))
print("breakpoints: {}".format([0, 1, 2, 3]))
x_eval = [0, 0.5, 1., 1.1, 2.5]
print("Eval for x_eval = {:}\npath(x_eval)=\n{}\npath(x_eval, 1)=\n{}\npath(x_eval, 2)=\n{}".format(
    x_eval, path(x_eval), path(x_eval, 1), path(x_eval, 2)))

pc_vel = ta.constraint.JointVelocityConstraint([1.0, 1.0])
pc_acc = ta.constraint.JointAccelerationConstraint([0.2, 0.2], discretization_scheme=0)

instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 3, 51), solver_wrapper='qpoases')
sdds, sds, _, K = instance.compute_parameterization(0, 0, return_data=True)
feasible_sets = instance.compute_feasible_sets().

I run this code, except i do not use the solver_wrapper qpoases, since i did not have that installed. I simply use the default solver (seidel).

So the only differencce is that the line

instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 3, 51), solver_wrapper='qpoases')

is

instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 3, 51))

After running this, the terminal output is as such:

coeff0 << -0.500000, -0.500000, 1.500000, 0.500000, 0.000000, 3.000000, 0.000000, 0.000000;
coeff1 << -0.500000, -0.500000, 0.000000, -1.000000, 1.500000, 2.500000, 1.000000, 3.000000;
coeff2 << -0.500000, -0.500000, -1.500000, -2.500000, 0.000000, -1.000000, 2.000000, 4.000000;

breakpoints: [0, 1, 2, 3]
Eval for x_eval = [0, 0.5, 1.0, 1.1, 2.5]
path(x_eval)=
[[0.     0.    ]
 [0.3125 1.5625]
 [1.     3.    ]
 [1.1495 3.2395]
 [1.5625 2.8125]]
path(x_eval, 1)=
[[ 0.     3.   ]
 [ 1.125  3.125]
 [ 1.5    2.5  ]
 [ 1.485  2.285]
 [-1.875 -3.875]]
path(x_eval, 2)=
[[ 3.   1. ]
 [ 1.5 -0.5]
 [ 0.  -2. ]
 [-0.3 -2.3]
 [-4.5 -6.5]]

Now, I was hoping to see the same values for positions, velocities, and accelerations on the cpp side.

My attempt involved me adding a few lines to the 'ControllableSets' test in test_algorithm.cpp:

TYPED_TEST(ProblemInstance, ControllableSets) {
  toppra::algorithm::TOPPRA problem{this->v, this->path};
  problem.setN(50);
  problem.solver(std::make_shared<TypeParam>());
  auto ret_code = problem.computePathParametrization();
  const auto& data = problem.getParameterizationData();

  ASSERT_EQ(data.gridpoints.size(), 51);
  ASSERT_TRUE(data.gridpoints.isApprox(toppra::Vector::LinSpaced(51, 0, 3)));

  toppra::Vector e_K_max(51);
  e_K_max << 0.06666667, 0.07624309, 0.08631706, 0.09690258, 0.1005511, 0.09982804,
      0.09979021, 0.1004364, 0.10178673, 0.10184412, 0.09655088, 0.09173679, 0.08734254,
      0.08331796, 0.07962037, 0.07621325, 0.07306521, 0.07014913, 0.0674415, 0.06492188,
      0.06257244, 0.06037764, 0.05832397, 0.05639984, 0.05459563, 0.05290407,
      0.05132158, 0.04985238, 0.04852317, 0.04745694, 0.04761905, 0.05457026,
      0.06044905, 0.06527948, 0.08479263, 0.10990991, 0.13252362, 0.15269631,
      0.15777077, 0.12111776, 0.09525987, 0.07641998, 0.06232537, 0.05154506,
      0.04314353, 0.03257513, 0.02268898, 0.01495548, 0.0088349, 0.00394283, 0.;

  ASSERT_EQ(ret_code, toppra::ReturnCode::OK)
      << "actual return code: " << (int)ret_code;
  for (int i = 0; i < 51; i++)
    EXPECT_NEAR(data.controllable_sets(i, 1), e_K_max(i), TEST_PRECISION)
        << "idx: " << i;

  toppra::ParametrizationData pd = problem.getParameterizationData();

    toppra::Vector gridpoints =
      pd.gridpoints;  // Grid-points used for solving the discretized problem.
    toppra::Vector vsquared =
        pd.parametrization;  // Output parametrization (squared path velocity)
    std::shared_ptr<toppra::parametrizer::ConstAccel> ca =
        std::make_shared<toppra::parametrizer::ConstAccel>(this->path, gridpoints, vsquared);
    std::cout << "ca->validate() = " << ca->validate() << std::endl;
    ASSERT_TRUE(ca->validate());

    toppra::Vector times2(5);
    times2 << 0, 0.5, 1., 1.1, 2.5;
    toppra::Vectors path_pos2;
    path_pos2 = ca->eval(times2, 0);  // TODO this function call fails
    toppra::Vectors path_vel2;
    path_vel2 = ca->eval(times2, 1);  // TODO this function call fails
    toppra::Vectors path_acc2;
    path_acc2 = ca->eval(times2, 2);  // TODO this function call fails
    ASSERT_EQ(path_pos2.size(), 5);
    ASSERT_EQ(path_pos2.at(0).rows(), 2);
    ASSERT_EQ(path_vel2.size(), 5);
    ASSERT_EQ(path_vel2.at(0).rows(), 2);
    ASSERT_EQ(path_acc2.size(), 5);
    ASSERT_EQ(path_acc2.at(0).rows(), 2);
    Eigen::MatrixXd path_pos2_ = Eigen::MatrixXd::Zero(2, 5);
    Eigen::MatrixXd path_vel2_ = Eigen::MatrixXd::Zero(2, 5);
    Eigen::MatrixXd path_acc2_ = Eigen::MatrixXd::Zero(2, 5);
    formatVecToMat(path_pos2, path_pos2_);
    formatVecToMat(path_vel2, path_vel2_);
    formatVecToMat(path_acc2, path_acc2_);
    std::cout << "path_pos2_\n " << path_pos2_ << std::endl;
    std::cout << "path_vel2_\n " << path_vel2_ << std::endl;
    std::cout << "path_acc2_\n " << path_acc2_ << std::endl;
    std::cout << "times2 \n " << times2.transpose() << std::endl;
}

After building with the following commands:

cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -DBUILD_WITH_PINOCCHIO=OFF -DBUILD_WITH_qpOASES=OFF -DBUILD_WITH_GLPK=OFF -DPYTHON_BINDINGS=OFF ..
make -j5

and running the tests,

I get the following terminal output:

path_pos2_
           0 0.000103877  0.00164815  0.00240736     0.05913
          0   0.0250344    0.100537    0.121781    0.633979
path_vel2_
           0 0.000829861  0.00655556  0.00869439   0.0894012
          0    0.100274    0.202111    0.222779    0.507362
path_acc2_
          0 0.00496528  0.0194444  0.0233866  0.0956188
       0.2   0.201632   0.206111   0.207253   0.200076
times2 
   0 0.5   1 1.1 2.5

This is not the expected.

There is no way the cpp interface is deeply flawed, so certainly in the way i am using it is the problem?

[hungpham2511]

Interesting. Actually, we use that commented Python code to check if it is consistent with the C++ API and yes it did. Unless there were some regressions.

breakpoints: [0, 1, 2, 3]
Eval for x_eval = [0, 0.5, 1.0, 1.1, 2.5]
path(x_eval)=
[[0.     0.    ]
 [0.3125 1.5625]
 [1.     3.    ]
 [1.1495 3.2395]
 [1.5625 2.8125]]
path(x_eval, 1)=
[[ 0.     3.   ]
 [ 1.125  3.125]
 [ 1.5    2.5  ]
 [ 1.485  2.285]
 [-1.875 -3.875]]
path(x_eval, 2)=
[[ 3.   1. ]
 [ 1.5 -0.5]
 [ 0.  -2. ]
 [-0.3 -2.3]
 [-4.5 -6.5]]

These are evaluations of the un-retimed path. If you want to see the retimed path, you should do this instead

instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 3, 51), solver_wrapper='seidel', parametrizer="ParametrizeConstAccel")
retime_traj = instance.compute_trajectory()

See this example of the Python API.

[hungpham2511]

The output matches exactly up to numerical errors from my experiment.

In [13]: traj.eval([0, 0.5, 1.0, 1.1, 2.5])
Out[13]:
array([[0.00000000e+00, 0.00000000e+00],
       [1.03877055e-04, 2.50344015e-02],
       [1.64814405e-03, 1.00536911e-01],
       [2.40735400e-03, 1.21780429e-01],
       [5.91298212e-02, 6.33977764e-01]])

In [14]: traj.evald([0, 0.5, 1.0, 1.1, 2.5])
Out[14]:
array([[0.        , 0.        ],
       [0.00082986, 0.10027418],
       [0.00655554, 0.20211086],
       [0.00869437, 0.22277855],
       [0.08940095, 0.5073612 ]])

In [15]: traj.evaldd([0, 0.5, 1.0, 1.1, 2.5])
Out[15]:
array([[0.        , 0.19999975],
       [0.00496527, 0.20163169],
       [0.0194444 , 0.20611085],
       [0.02338655, 0.20725301],
       [0.09561861, 0.20007532]])