Initial and final velocity of a trajectory

[g-lisi]

Hi, Thank you for sharing this great tool with the community!

I need to generate a trajectory with time-varying velocity limits. For this, I had to split the trajectory into segments, each with its own velocity limits. I also had to add "transition" segment to allow for a transition from one segment to the other at non-zero velocity. This works well except for the boundary conditions of each segment.

I would like the velocity at the boundary to be non-zero (also negative) with a "seamless" transition. For example, if I want to go from vlim=-4 to vlim=-6, currently the only way would be to do -4 -> 0 -> -6, but I want it to be -4-> -6. I realized that the input to compute_trajectory(start_v, end_v) has to be always positive. So I thought of fixing the boundary conditions by bc_type of the CubicSpline.

However, it seems like TOPPRA ignores bc_type, and in transition segments, I always end up having an S-shaped segment, when I would like to be more U shaped.

Is there any way to force initial and final velocity (also negative)?

Thank you in advance Best regards

[jmirabel]

Python or C++ ?

[g-lisi]

Python

[jmirabel]

I won't be of much help then. Did you try https://github.com/hungpham2511/toppra/blob/33a871dbc6a597a9ce9fe1a44f67e15dc845fc9f/toppra/constraint/linear_joint_velocity.py#L56 ?

[g-lisi]

If you have an example in c++, I can switch easily. I am at an initial exploratory stage with my project so I can use c++.

But in the meantime, I will take a look at JointVelocityConstraintVarying. Thank you for the quick replies!

[jmirabel]

In C++, check the doc of https://github.com/hungpham2511/toppra/blob/develop/cpp/src/toppra/constraint/linear_joint_velocity.hpp

[g-lisi]

Thank you for the help, I was able to achieve exactly what I wanted in Python. For anyone who is interested I used these as reference (specifically JointVelocityConstraintVarying):

https://hungpham2511.github.io/toppra/_modules/toppra/constraint/linear_joint_velocity.html

https://github.com/hungpham2511/toppra/blob/77a647c14f2ece702ea9e7a8ea0f58e343aabe60/tests/tests/constraint/test_joint_velocity.py

All I had to do was to add something like this to my already working script:

ss_wpts = np.r_[0, 1, 2]
vlim_wpts = [[[-1, 2], [-1, 2]],
             [[-2, 3], [-2, 3]],
             [[-1, 0], [-1, 0]]]
vlim_spl = CubicSpline(ss_wpts, vlim_wpts)
pc_vel = ta.constraint.JointVelocityConstraintVarying(vlim_spl)
instance = algo.TOPPRA([pc_vel, pc_acc], path)

Note that vlim_spl does not have to be a Spline, but it can be any function:

vlim_func: (float) -> np.ndarray
        A function that receives a scalar (float) and produce an array
        with shape (dof, 2). The lower and upper velocity bounds of
        the j-th joint are given by out[j, 0] and out[j, 1]
        respectively.

as explained in the first link above.