Closed JadTawil-theonly closed 3 years ago
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.
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]])
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:
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:
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:
After building with the following commands:
and running the tests,
I get the following terminal output:
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?