search

hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
625 stars 170 forks source link

instance.compute_trajectory(0, 0) function takes very long #3

Closed JadBatmobile closed 6 years ago

JadBatmobile commented 6 years ago

Hello again,

I am running your kinematics_duration.py script, but unfortunately the jnt_traj, aux_traj = instance.compute_trajectory(0, 0) function is taking approximately 0.3 seconds to execute. This seems much longer than the original TOPP implementation, which clocked at around 0.08-0.1 seconds.

What do you think?

JadBatmobile commented 6 years ago

It is not specific to the constant time implementation btw, the same line in kinematics.py takes 0.28 seconds

hungpham2511 commented 6 years ago

In both examples the number of discretization point is 1001, which is really high. You should try 101 points instead as shown below:

    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 1, 101),
                           solver_wrapper='hotqpoases')

For an even faster speed you can try the newly implemented Seidel's solver as follow

    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 1, 101),
                           solver_wrapper='seidel')

It should be around 5-10 times faster now. But be aware this implementation is currently quite buggy.

In my PC, the timings are respectively 0.031274843216 secs (101 points/hotqpoases) and 0.0196042060852 secs (101 points/seidel) respectively.