hungpham2511 / toppra

robotic motion planning library

toppra: Time-Optimal Path Parameterization

• Overview
• Supports
• Citing toppra

Overview

toppra is a library for computing the time-optimal path parametrization for robots subject to kinematic and dynamic constraints. In general, given the inputs:

1. a geometric path p(s), s in [0, s_end];
2. a list of constraints on joint velocity, joint accelerations, tool Cartesian velocity, et cetera.

toppra returns the time-optimal path parameterization: s_dot (s), from which the fastest trajectory q(t) that satisfies the given constraints can be found.

Documentation and tutorials are available here.

You can install the package with pip:

pip install toppra

To install from source for development:

pip install -r requirement3.txt
pip install -e .

Support

Bug tracking

Please report any issues, questions or feature request via Github issues tracker.

Have a quick question? Try asking in our slack channel.

Contributions

Pull Requests are welcome! Create a Pull Request and we will review your proposal!

Credits

toppra was originally developed by Hung Pham (Eureka Robotics, former CRI Group) and Phạm Quang Cường (Eureka Robotics, CRI Group) with major contributions from talented contributors:

• Joseph Mirabel (C++ API)
• EdsterG (Python3 support).

If you have taken part in developing and supporting the library, feel free to add your name to the list.

The development is also generously supported by Eureka Robotics.

Citing toppra

If you use this library for your research, we encourage you to

1. reference the accompanying paper A new approach to Time-Optimal Path Parameterization based on Reachability Analysis, IEEE Transactions on Robotics, vol. 34(3), pp. 645-659, 2018.
2. put a star on this repository.

toppra's Issues

Users should be informed when there is failure

The parametrization algorithm might fail when:

1. There is actually no feasible solution;
2. The output trajectory does not satisfy input constraints.
3. ???

Users should be notified when any of these errors occur.

Get indexes or times of passed in joint values

I have a feature request. Is it possible in your implementation to be able to get out the times associated with the passed in points? Because right now, I get back a spline, which I can query for joint values at certain times, but short of creating a 1 KHz sampling for a trajectory, there's no way to really guarantee straight line motion. Additionally, many robot controllers are going to not work well with such a dense trajectory. I'd prefer to be able to just query what the times are for the passed joint values that were used to solve the problem. If these moved slightly no huge problem. I wrote some thing that goes through looking for the times where the joints occur but with complex trajectories the spline isn't exactly equal to the values passed in and you could end up finding something that matches best at the wrong place/time in the trajectory. Having something that keeps track of the indicies associated with the original knot points would be great.

gridpoints relevance in algorithm

Hello, I am getting very different trajectories depending on the grippoint paramaeter in algo.TOPPRA.

Here is the code:

`
def toppra_profiles(way_pts, am, vm):
scalar = 1
error_flag = True
while scalar < 10000000000:
way_pts2 = way_pts * scalar
max_vel2 = vm * scalar
max_acc2 = am * scalar

	path = ta.SplineInterpolator(np.linspace(0, 1, way_pts2.shape[0]), way_pts)
	vlim_ = np.ones(3) * max_vel2
	vlim = np.vstack((-vlim_, vlim_)).T
	# Create acceleration bounds, then acceleration constraint object
	alim_ = np.ones(3) * max_acc2
	alim = np.vstack((-alim_, alim_)).T
	pc_vel = constraint.JointVelocityConstraint(vlim)
	pc_acc = constraint.JointAccelerationConstraint(
		alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
	instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 1, 41),
						   solver_wrapper='hotqpoases')

	jnt_traj, aux_traj = instance.compute_trajectory(0, 0)

	if jnt_traj is not None:
		error_flag = False
		break
	else:
		print "trajectory is none profiles"

	scalar = scalar * 10

if not error_flag:
	duration = jnt_traj.get_duration()
	ts_sample = np.linspace(0, duration, 100)
	jt = jnt_traj.eval(ts_sample)
	qs_sample = jnt_traj.evaldd(ts_sample)
	vel_sample = jnt_traj.evald(ts_sample)

	return jt, vel_sample, qs_sample, ts_sample, duration
else:
	print "ERROR TRAJECTORY"
	return`

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

when i use np.linspace(0, 1, 21), it gives much different results than np.linspace(0, 1, 51) for example.

here is the output for 3 joints for 51:

here is the output for 3 joints for 21:

The differences are drastic.
I would like to discuss the relevance of the grippoint parameter. After reading the paper, i am aware it is the discretization of the optimization space. Do you expect it to produce such differences?

`pymanoid` is required when running `pytest -v`

When running

pytest -v

the file test_create_path_constraints.py also imports pymanoid.

Maybe you can note this earlier in the documentation or skip the test when pymanoid is not installed.

Additional information about the generated trajectory

Is there a way to determine velocity, acceleration and time information for a specific input waypoint?
For example, a evaluation function (similar to jnt_traj.eval()), that instead of taking as an input a time step, takes a waypoint ?

python requirements boken in latest updates

As of 5/17/2019, the top version of scipy in pip depends on python-3.5. It's unclear exactly what platform you are targeting as a default working platform, but if you are still shooting for Python 2.7 then you'll want scipy<1.3 in requirements.txt..

Can I pass in non-zero starting and goal velocity for each DOF?

I am wondering whether there is a way to specify the starting and goal positions and velocities for each DOF into functions such as compute_trajectory in scalar_example.py?

It seems that 0 is always passed in instance.compute_trajectory(0, 0). Is there a way to pass in a list of velocities of all the DOFs into compute_trajectory?

Another corner case

Hung,

I ran into a case today where The following two waypoints for a 6-DOF arm were sent to toppra, and it failed because the controllable sets saw a nan in K.

Reproduce by updating scalar_example.py:

    waypts = [[-9.089468271438139e-07, -0.46400441351211447, -0.5760014655483718, -3.9375206752326924e-07, -1.6999970211081608, 5.429519493702008e-06], [0.0, -0.464, -0.576, 0.0, -1.7, 0.0]]
    path = ta.SplineInterpolator(np.linspace(0, 1, len(waypts)), waypts)
    vlim = np.array([[-3, 3],[-3, 3],[-3, 3],[-3, 3],[-3, 3],[-3, 3]])
    alim = np.array([[-4, 4],[-4, 4],[-4, 4],[-4, 4],[-4, 4],[-4, 4] ])

However, I noticed that if I comment out the np.isnan(K[i]).any() if in both compute_controllable_sets() and compute_parameterization, that the rest of computer_paramterization() seems to work just fine and produce the correct answer.

So might you be able to either ignore nan for K or at least set nans in K to constants.MAXX?

Both of those "solutions" work for me in testing.

C++ Implementation

We will use this issue to flesh out some key designs of the C++ interface.

There are a few core components that are required.

• Trajectory representation;
• Constraint representation;
• LP solver wrapper;
• Core TOPPRA pipeline;
• Matrix manipulation libraries in C++?

I am most inexperienced with trajectory representation in C++. Use OpenRAVE before with its representation, but bringing it over is a huge dependency that I am not willing to make. In the Python implementation I am basically using scipy.interpolate extensively.

Durations under 0.2 seconds not possible?

I am using constant duration trajectory and desire trajectories with small durations,

I get this warning whenever i request a trajectory under 0.2 seconds

Desired duration 0.100000 seconds is not achievable. Returning the fastest parameterization with duration 0.200000 seconds

I am however providing acceleration and velocity limits that are extremely high, any ideas?

Constant Time Trajectories

Is it possible to plan trajectories with a specific duration?

Numerical Stability of Seidel Solver

I've posted the code to reproduce the bug here:
https://gist.github.com/EdsterG/6046289ed1f9ae0a738afd3c09eaa156

When running the given code snippet, I get the following error: The 0-th controllable set is empty. This path is not parametrizable.

The following changes fix the problem: changing the solver, removing the first way-point, removing the last way-point. This seems to be an issue with numerical stability in Seidel.

[Feature] Automatic selection of gridpoints

Feature added.
TODO: Add doc.

phase-plane plot

Is still possible using TOPP-RA to directly plot phase-plane? It was possible in TOPP but i cannot find the way to do this in TOPP-RA.
Thank you.

Need to Validate Gridpoints Internally

The following snippet of code crashes toppra, even though it shouldn't. I understand why it crashes, two of the grid points are extremely close, specifically gp[8] and gp[9]. However, the input is strictly increasing so it shouldn't crash.

import numpy as np

import toppra as tra


waypoints = np.array(
    [[0.016844487206594733, 0.4838585864979808, -0.02150790868407046, -1.7921138616816437, 0.013312268835191154, 0.8655538727538646, -0.010831701942519256],
     [0.12784104717718764, 0.4838631532810903, -0.01997162949235118, -1.7921109502680679, 0.012361392489820378, 0.8655569554312098, 0.10214172868158178],
     [0.23883760714778063, 0.48386772006419965, -0.018435350300631925, -1.7921080388544919, 0.0114105161444496, 0.8655600381085549, 0.2151151593056828],
     [0.3498341671183736, 0.48387228684730904, -0.016899071108912617, -1.792105127440916, 0.010459639799078796, 0.8655631207859, 0.32808858992978396],
     [0.4608307270889665, 0.48387685363041844, -0.015362791917193336, -1.7921022160273403, 0.009508763453708019, 0.8655662034632453, 0.4410620205538849],
     [0.5718272870595593, 0.4838814204135279, -0.013826512725474027, -1.7920993046137643, 0.008557887108337242, 0.8655692861405904, 0.5540354511779859],
     [0.6828238470301524, 0.4838859871966373, -0.012290233533754719, -1.7920963932001885, 0.007607010762966437, 0.8655723688179356, 0.6670088818020871],
     [0.7938204070007453, 0.48389055397974673, -0.010753954342035466, -1.7920934817866125, 0.006656134417595633, 0.8655754514952809, 0.7799823124261881],
     [0.9048169669713383, 0.4838951207628562, -0.009217675150316185, -1.7920905703730368, 0.005705258072224856, 0.8655785341726261, 0.8929557430502891],
     [1.0158135269419313, 0.4838996875459656, -0.007681395958596904, -1.792087658959461, 0.004754381726854079, 0.8655816168499713, 1.0059291736743903],
     [1.126810086912524, 0.483904254329075, -0.006145116766877623, -1.792084747545885, 0.003803505381483302, 0.8655846995273164, 1.1189026042984913],
     [1.2378066468831173, 0.4839088211121844, -0.004608837575158342, -1.7920818361323092, 0.0028526290361124973, 0.8655877822046616, 1.2318760349225926],
     [1.34880320685371, 0.48391338789529387, -0.0030725583834390335, -1.7920789247187334, 0.0019017526907417204, 0.8655908648820068, 1.3448494655466936],
     [1.459799766824303, 0.48391795467840326, -0.0015362791917197527, -1.7920760133051574, 0.0009508763453709435, 0.8655939475593519, 1.4578228961707946],
     [1.570796326794896, 0.4839225214615127, -4.440892098500626e-16, -1.7920731018915816, 1.3877787807814457e-16, 0.8655970302366971, 1.5707963267948957]]
)

path = tra.SplineInterpolator(np.linspace(0, 1, len(waypoints)), waypoints)

vlim_ = np.deg2rad([50, 50, 55, 40, 70, 75, 75])
alim_ = np.deg2rad([150, 150, 150, 150, 150, 150, 150])
pc_vel = tra.constraint.JointVelocityConstraint(np.vstack((-vlim_, vlim_)).T)
pc_acc = tra.constraint.JointAccelerationConstraint(
    np.vstack((-alim_, alim_)).T,
    discretization_scheme=tra.constraint.DiscretizationType.Interpolation
)

gp = np.array([0.0, 0.02040816326530612, 0.04081632653061224, 0.061224489795918366, 0.07142857142857141, 0.08163265306122448, 0.1020408163265306, 0.12244897959183673, 0.14285714285714282, 0.14285714285714285, 0.16326530612244897, 0.18367346938775508, 0.2040816326530612, 0.2142857142857143, 0.22448979591836732, 0.24489795918367346, 0.26530612244897955, 0.28571428571428564, 0.2857142857142857, 0.3061224489795918, 0.32653061224489793, 0.3469387755102041, 0.35714285714285704, 0.36734693877551017, 0.3877551020408163, 0.4081632653061224, 0.42857142857142855, 0.44897959183673464, 0.4693877551020408, 0.4897959183673469, 0.49999999999999994, 0.5102040816326531, 0.5306122448979591, 0.5510204081632653, 0.5714285714285713, 0.5714285714285714, 0.5918367346938775, 0.6122448979591836, 0.6326530612244897, 0.6428571428571428, 0.6530612244897959, 0.673469387755102, 0.6938775510204082, 0.7142857142857142, 0.7346938775510203, 0.7551020408163265, 0.7755102040816326, 0.7857142857142858, 0.7959183673469387, 0.8163265306122448, 0.836734693877551, 0.8571428571428571, 0.8571428571428572, 0.8775510204081632, 0.8979591836734693, 0.9183673469387754, 0.9285714285714286, 0.9387755102040816, 0.9591836734693877, 0.9795918367346939, 1.0])
assert np.all(np.diff(gp) > 0)

instance = tra.algorithm.TOPPRA(
    [pc_vel, pc_acc], path,
    gridpoints=gp,
    solver_wrapper="seidel"
)

jnt_traj, aux_traj = instance.compute_trajectory(0, 0)

assert jnt_traj is not None

Smoothing output joints profile

Actually topp-ra does not manage jerk constraints on joints or cartesian space. For industrial robots discontinuous joints acceleration leads to high vibrations because of elastic resonances.
On the other hand, filtering output joints profile deforms the nominal trajectory. The idea is to apply a FIR filter to the profile in s-s_dot space. In this way the nominal trajectory won't be deformed and the output joints profile becomes smoother (even if there's no control on the jerk value). I don't know if this solution garantees all set constraints in joints space (i.e. torques constraints).

about compute the reachable set

I tried the function compute_controllable_set(), it works well. When I tried to implement a similar but forward function compute_reachable_set(), it seems that something was wrong. I have forked your repo, and commit my modification, as you can see on forked_toppra

I really appreciate that if you can help me and point where was wrong when you are free. thank for your time

First Order Constraints

The paper mentions first order constraints, however I can't seem to find them in the current implementation. Can you point me in the right direction?

0.2 time for "small" motions

So occasionally, with let's say a gripper, I've noticed that in my system, toppra might get called with
[[-0.2,0.2],[-0.2-1e-6,0.2+1e-6],[-0.2,0.2]]. When this happens, no matter what the magnitude of the joint angles or the velocities or the accelerations, toppra always returns time = 0.2. This is true for any DOF and for any sized trajectory (though len(way_pts)==2 yields 5 (from algorithm.py), which I set to 1e-10, so my local version works for 2 waypoints).

Feature Request, per Way-Point Velocity Constraints

Support for per way-point velocity constraints.

Installation error

Importing toppra after running python setup.py install raises this error:

ModuleNotFoundError: No module named 'toppra._CythonUtils'

Can I use Cartesian Space path instead of Joint Space path as input for toppra？

I want to use toppra for my research. After I finish reading the documentation and tutorials, I found that the
paths are represented by Joint Space.Can I use Cartesian Space path as input for toppra?
Looking forward to your reply.

How TOPPRA calculate dynamics coefficients a(s), b(s), c(s) in joint_torque.py?

When I learn TOPPRA, I find that the dynamics coefficients a(s), b(s), c(s) is calculated in joint_torque.compute_constraint_params like below.

c = np.array( [self.inv_dyn(p_, v_zero, v_zero) for p_ in p] )
a = np.array( [self.inv_dyn(p_, v_zero, ps_) for p_, ps_ in zip(p, ps)] ) - c
b = np.array( [self.inv_dyn(p_, ps_, pss_) for p_, ps_, pss_ in zip(p, ps, pss)] ) - c

I don't know why. Actually, what is calculate in inv_dyn shoule be torque, not dynamics coefficient.
Why don't you do like this? [https://github.com/rdiankov/openrave/blob/7c5f5e27eec2b2ef10aa63fbc519a998c276f908/sandbox/mintime/MintimeProblemTorque.py]

Velocity Constraints Not Satisfied

When using varying joint velocity constraints, the final trajectory violates some of the constraints.

import numpy as np
import scipy
import toppra as ta


way_pts = [[1, -1, -1, 0, 0],
           [1, 1, -1, 0, 0],
           [-1, 1, -1, 0, 0],
           [-1, 1, 1, 0, 0]]
ws = np.linspace(0, 1, 4)
path = ta.SplineInterpolator(ws, way_pts)

# Create velocity bounds, then velocity constraint object
vlim_ = np.ones(5) * 10
vlim = np.vstack((-vlim_, vlim_)).T
# pc_vel = ta.constraint.JointVelocityConstraint(vlim)
vlim_func = scipy.interpolate.interp1d(ws, [vlim, vlim / 20, vlim, vlim], kind="zero", axis=0)
pc_vel = ta.constraint.JointVelocityConstraintVarying(vlim_func)
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(5) * 100
alim = np.vstack((-alim_, alim_)).T
pc_acc = ta.constraint.JointAccelerationConstraint(
    alim, discretization_scheme=ta.constraint.DiscretizationType.Interpolation)

# Setup a parametrization instance
instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')

# Retime the trajectory, only this step is necessary.
jnt_traj, _ = instance.compute_trajectory(0, 0)

ts_sample = jnt_traj.ss_waypoints
qs_sample = jnt_traj.eval(ts_sample)
qds_sample = jnt_traj.evald(ts_sample)

start = np.linalg.norm(qs_sample - way_pts[1], axis=1).argmin()
end = np.linalg.norm(qs_sample - way_pts[2], axis=1).argmin()
assert np.abs(qds_sample[start:end]) <= 0.5

Per Waypoint Time Constraint

Commit 74b99fa adds the ability to specify a total time for the whole trajectory. Is it possible to specify an exact time for multiple way-points and fail if the request isn't feasible?

Installation error (venv): no module named numpy

Not sure this is pebcak, so decided to report it.

Following the installation instructions here I ran into the following error while executing the command pip install -r requirements.txt in a virtualenv:

Collecting CVXcanon>=0.0.22 (from cvxpy<1.0.0,>=0.4.11->-r requirements.txt (line 6))
  Downloading https://files.pythonhosted.org/packages/9e/e6/63eb6e7dca5dcb723429e65b456d0e3638976e63f6696b7eb48fee3a491d/CVXcanon-0.1.1.tar.gz (694kB)
    100% |████████████████████████████████| 696kB 28.0MB/s 
    Complete output from command python setup.py egg_info:
    Traceback (most recent call last):
      File "<string>", line 1, in <module>
      File "/tmp/pip-install-xyDTn9/CVXcanon/setup.py", line 4, in <module>
        import numpy
    ImportError: No module named numpy

After manually installing numpy first with pip install numpy, pip was able to continue installing the other requirements.

Failures on dense trajectory that oddly succeed when made more dense

Hung,

First off, please don't take the repeated issues I'm filing in a negative way. I value this repository very much as I have found it very useful, I dumbly didn't realize that TOPP was an active research issue, and I was trying to solve myself for a week without progress until I stumbled upon Kunz and Stilman then TOPP-RA.

However, as I mentioned previously, your implementation depends heavily on getting that initial "Alignment" right when forming the cublic spline from the trajectory (Y) values to the 0..1 spaced (X) values. Otherwise the cubic spline will "oscillate" outside of the limits set by the knot points (which might be infeasible given joint limits or obstacles). For a single-DOF system, this is easy because you just figure out the percentage of overall motion and that lets you set the spline X values properly. For multi-DOF this isn't possible because joints move differently across the trajectory, especially for straight line motion trajectories.

So, in order to ensure that the spline doesn't move outside the limits set by the trajectory waypoints for each segment, I was densifying the trajectory before calculating the initial spline. For instance, if the max motion of any joint from waypoint A to B is 30 degrees, then I would add 29 waypoints between A and B (one every degree). This reduces the possibility of the spline moving out of the segment limits, by adding more knots.

I've found real world examples where running without densifying finds a solution, though again that the solution is outside the limits. Then running with densifying, causes toppra to fail to return an answer. But, I've found that densifying even more (e.g., 299 waypoints in the example above) can then cause toppra to find a solution.

I thought you'd be interested in this. I've found that by just increasing the densification factor on failures ( doing this a most N times so that it doesn't run forever), that I do eventually find a solution 100% of the time for a sense trajectory. The question is why splines created from dense, but not "really dense" trajectories fail.

The velocities start and end at non-zero values

Just run the basic example and find out the velocities are not zeros at beginning and ending. Wondering how come this happened since the initial and finals velocities should be zeros for robot controlling.

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

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?

Approximation of openrave trajectories

pypi release?

Have you considered releasing this module on pypi?

[Feature] Seidel solver scales LPs before solving

This should help improving toppra performance on problems with very short duration, such as the zero-motion trajectories reported by several users.

If there is anyone experienced with programming LP solvers with scaling would like to help or provide some references that would be awesome!

Spike in the Reachable set

I'm trying to use toppra to compute the optimal parametrization and joint torques for a cartesian elliptical task. I already have the joint trajectory computed inverting the kinematics and I'm feeding this to toppra, with joint velocity and joint torque bounds.

The path is given by 1000 equally spaced waypoints, with 5000 gridpoints (I've also tried with much more waypoints, less gridpoints and so on with no success)

While no error is shown, something seems to be wrong, in particular seeing the spike at the beginning of the feasible set. The joint torque and velocity are satisfying their limits, hinting that there should not be an error in the setting of the constraints.

I've tried both the qpoases and seidel solvers with no difference, as well as the scaling option.

Moreover, I'm using the same framework from a paper from my university which has solved the same problem in the past using TOPP, which managed to get a minimum time of 1 second, while I get 2 seconds.

Below is the same phase plane image from the article I'm following, that used TOPP

Do you have any suggestion on what could be causing this problem and how to proceed?

This is my code if it can help

path = ta.SplineInterpolator(time, joint_pos[:,:num_dof-1])

vel_lim = np.array([1.92, 1.92, 2.23, 2.23, 3.56, 3.21])
vel_lim_stack = np.vstack((-vel_lim, vel_lim)).T

pc_vel = constraint.JointVelocityConstraint(vel_lim_stack)


def inverse_dynamics(q, dq, ddq):
    q = np.append(q, 0)
    dq = np.append(dq, 0)
    ddq = np.append(ddq, 0)
    tau = inv_dyn(q, dq, ddq)

    return tau[:num_dof-1]

def F(q):
    return np.vstack((np.eye(num_dof-1), -np.eye(num_dof-1)))

def g(q):
    return np.append(torque_lim, torque_lim)

pc_torque = constraint.SecondOrderConstraint(inverse_dynamics, F, g, dof=num_dof-1)

gridpoints = np.linspace(0, path.duration, 5000)

instance = algo.TOPPRA([pc_vel, pc_torque], path, gridpoints=gridpoints, solver_wrapper='qpoases')
jnt_traj, aux_traj, int_data = instance.compute_trajectory(0, 0, return_data=True)

X = instance.compute_feasible_sets()

optimized trajectory cartesian velocity with the cartesian acceleration constraints

I implemented TOPP-RA on my robot. Can I know how can I check Cartesian velocity profile after constraining Cartesian accelerations ?

Configure CI to test against multiple python versions

TOPPRA for path planning

Im curious about a potential use case of toppra. Say i have a path, parameterized by x, y, and theta. So a vehicle or robot is travelling some arbitrary path in the world, with each point being defined by an x,y coordinate, and a heading of the vehicle theta.

Would toppra work for time parameterizing this path, given that the units of the x,y coordinates are meters, and theta is radians? do the units have to be the same?

thank you

Issues in handling Second-Order Constraint w/friction

The current implementation of SecondOrderConstraint is incapable of handling constraints with dimensions different from the robot's dof.

Possible solution: Perhaps a more general implementation of SecondOrderConstraint is needed. Joint-torque with friction can be cast as a special case of this general implementation.

Can you give me some advice about how to implement this library in a real industrial robot

Your library is very stable and efficient. So I want to apply this library to online trajectory planner in my robot lab. During the applying process, there is a problem that I don't know how to write the code about the real-time buffer. Computing the whole path is time-consuming.
So Could you give me some advice about how to write a real-time buffer when applying this library?

AttributeError: 'NoneType' object has no attribute 'get_duration'

Hello,

I posted about an issue eariler this week with regards to durations that were less than 0.2, the problem was a numerical conditioning one and it was solved by cloning in the newest repo.

However, another problem is occuring with the newest repo,

I am getting a AttributeError: 'NoneType' object has no attribute 'get_duration' error when the input waypoints are all very close to one another. The following function is what im using:

def toppra_constant_time(way_pts, am, vm, duration):

path = ta.SplineInterpolator(np.linspace(0, 1, way_pts.shape[0]), way_pts)
vlim_ = np.ones(3) * vm
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(3) * am
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
	alim, discretization_scheme=constraint.DiscretizationType.Interpolation)

# Setup a parametrization instance with hot-qpOASES
instance = algo.TOPPRAsd([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 1, 51),
						 solver_wrapper='hotqpoases')

instance.set_desired_duration(duration)

# Retime the trajectory, only this step is necessary.
jnt_traj, aux_traj = instance.compute_trajectory(0, 0)

ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
jt = jnt_traj.eval(ts_sample)

print jt
qs_sample = jnt_traj.evaldd(ts_sample)
vel_sample = jnt_traj.evald(ts_sample)

'''
a1 = [i[0] for i in jt]
a2 = [i[1] for i in jt]
a3 = [i[2] for i in jt]
plot_angles(a1, a2, a3, ts_sample)
'''

return duration, jt, vel_sample, qs_sample, ts_sample

The jnt_traj is being returned as None. The input is (for three joints):

`way_pts:
[[-0.00038593 -0.00038593 -0.00038593]
[ 0.00013093 0.00013093 0.00013093]
[ 0.00064752 0.00064752 0.00064752]
[ 0.00116383 0.00116383 0.00116383]
[ 0.00167987 0.00167987 0.00167987]
[ 0.00219563 0.00219563 0.00219563]
[ 0.00271112 0.00271112 0.00271112]
[ 0.00322633 0.00322633 0.00322633]
[ 0.00374127 0.00374127 0.00374127]
[ 0.00425594 0.00425594 0.00425594]]

am = 100
vm = 100
duration = 0.5`

the case that path-velocity enter zero

The figure below is the case that path-velocity enter zero. And no matter matter how large acceleration limits I give, path-velocity always enter zero in the same position.

The version of toppra I used is master branch. and code is below.

'''python

import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np
import matplotlib.pyplot as plt
import time

ta.setup_logging("INFO")

def main():
# Parameters
N_samples = 5
SEED = 9
dof = 6

# Random waypoints used to obtain a random geometric path. Here,
# we use spline interpolation.
# np.random.seed(SEED)
in_file = 'input_point.txt'
in_ = np.loadtxt(in_file)
way_pts = np.array(in_).T
path = ta.SplineInterpolator(np.linspace(0, 1, way_pts.shape[0]), way_pts, bc_type='not-a-knot')

# Create velocity bounds, then velocity constraint object
vlim_ = np.ones(dof) * np.pi * 4;
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(dof) * np.pi * 100;
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
    alim, discretization_scheme=constraint.DiscretizationType.Interpolation)

# Setup a parametrization instance. The keyword arguments are
# optional.
instance = algo.TOPPRA([pc_vel, pc_acc], path, np.linspace(0, 1, way_pts.shape[0]), solver_wrapper='seidel')

# Retime the trajectory, only this step is necessary.
t0 = time.time()
jnt_traj, aux_traj, data = instance.compute_trajectory(0, 0, return_data=True, bc_type='clamped')
# return_data flag outputs internal data obtained while computing
# the paramterization. This include the time stamps corresponding
# to the original waypoints. See below (line 53) to see how to
# extract the time stamps.
print("Parameterization time: {:} secs".format(time.time() - t0))

ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
qs_sample = jnt_traj.eval(ts_sample)  # sampled joint positions
qds_sample = jnt_traj.evald(ts_sample)  # sampled joint velocities
qdds_sample = jnt_traj.evaldd(ts_sample)  # sampled joint accelerations

for i in range(dof):
    # plot the i-th joint trajectory
    plt.plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
    # plot the i-th joint waypoints
    plt.plot(data['t_waypts'], way_pts[:, i], 'x', c="C{:d}".format(i))
plt.xlabel("Time (s)")
plt.ylabel("Joint position (rad/s^2)")
plt.show()

# Compute the feasible sets and the controllable sets for viewing.
# Note that these steps are not necessary.
_, sd_vec, _ = instance.compute_parameterization(0, 0)
X = instance.compute_feasible_sets()
K = instance.compute_controllable_sets(0, 0)

X = np.sqrt(X)
K = np.sqrt(K)

plt.plot(X[:, 0], c='green', label="Feasible sets")
plt.plot(X[:, 1], c='green')
plt.plot(K[:, 0], '--', c='red', label="Controllable sets")
plt.plot(K[:, 1], '--', c='red')
plt.plot(sd_vec, label="Velocity profile")
plt.title("Path-position path-velocity plot")
plt.xlabel("Path position")
plt.ylabel("Path velocity square")
plt.legend()
plt.tight_layout()
plt.show()

if __name__ == '__main__':
    main()

'''

way point is read form this file:
input_point.txt

The optimized path is not optimal

I optimized my path with the velocity and acceleration limits set as shown in the picture below. But I think the path is not optimal, since the max velocity and acceleration of optimized path are much lower than the limits.
So, is it possible to further optimize the path, so that the max velocity is nearly the limits and the path motion time is least?

Times found in optimization are not always reasonable

I've seen examples where toppra (both my "old" fork and the newest upstream) provide the same wrong answer in terms of time for the last waypoint in a trajectory. HEre is an example:
for 6 waypoints (6-DOF robot):

[[-0.030642413736830083, 0.9210788610725734, 1.1005370198753621, 0.0010817886435241263, 1.1229495518273334, 2.8986148215009915], [-0.030763529996659454, 0.9226167221835322, 1.098539142483565, 0.001086064487964933, 1.1234197081586665, 2.9100717970800467], [-0.03088464625648882, 0.9241545832944911, 1.0965412650917683, 0.00109034033240574, 1.12388986449, 2.921528772659102], [-0.03100576251631819, 0.9256924444054498, 1.0945433876999713, 0.0010946161768465468, 1.1243600208213334, 2.932985748238157], [-0.030993973983559547, 0.9343914313305473, 1.0990185656991058, 0.0010486544290009307, 1.1112167154104744, 2.9329954455064784], [-0.03098218545080091, 0.9430904182556445, 1.1034937436982406, 0.0010026926811553146, 1.0980734099996154, 2.9330051427747996]],
[0.785398, 0.6283190000000001, 0.8901180000000001, 0.942478, 0.942478, 1.50796],
[78.5398, 62.831900000000005, 89.0118, 94.2478, 94.2478, 150.796]

The last two lists above are the max velocities/accelerations.

If I run this through toppra, I get a final times of: [0.0, 0.018758242674088654, 0.027433411317876365, 0.037597795704716795, 0.06504036006938992, 53.71761885739489].

That last time is "crazy". If I simply do waypoints.append(waypoints[-1]) in order to make a 7 point trajectory where the last value is repeated, I get something more reasonable: [0.0, 0.018760785795569175, 0.027481922635328733, 0.037832897511885226, 0.06618539826249735, 0.09915751384422705, 0.13215386747421617]

E AssertionError: toppra is unable to find any installation of qpoases!

I install toppra in python2.7 environment and then install qpoases,qytest
but when I run qytest -s in tests folders
The erros occur:
solverwrapper/test_basic_can_linear.py:103:

---

self = <toppra.solverwrapper.qpoases_solverwrapper.qpOASESSolverWrapper object at 0x7fa5c9578990>
constraint_list = [JointVelocityConstraint(
Type: ConstraintType.CanonicalLinear
Discreti...]
J5: [-12.80443992 12.804439...e: ConstraintType.CanonicalLinear
...e: DiscretizationType.Collocation
Random Second-Order constraint (dof=6)
)]
path = <toppra.interpolator.SplineInterpolator object at 0x7fa5c94edad0>
path_discretization = array([0. , 0.005, 0.01 , 0.015, 0.02 , 0.025, 0.03 , 0.035, 0.04 ,
0..., 0.955, 0.96 , 0.965, 0.97 , 0.975, 0.98 , 0.985,
0.99 , 0.995, 1. ])

def __init__(self, constraint_list, path, path_discretization):

  assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!"

E AssertionError: toppra is unable to find any installation of qpoases!

../toppra/solverwrapper/qpoases_solverwrapper.py:29: AssertionError
______________________________________________________________________ test_basic_correctness[vel_accel-x_ineq2-g0-H1-30-hotqpOASES] _

Feature Request: Per Way-Point Acceleration Constraints

The ability to add time varying acceleration limits similar to the time varying velocity limit would be very useful

Implementation of the algorithm in C++?

Specifying derivatives when create new trajectory

The current trajectory classes do not allow the specification of trajectory.

Non-cubic results for non-cubic trajectories

So, I was looking to use your repo in our system, and in doing some testing, I noticed some strange things in very simple test cases. I think this boils down to the fact that you enforce a cubic spline, which might not actually be an ideal fit for many trajectories.

For instead a very 1-DOF trajectory of 0,10 with max_vel and max_accel of 3 and 4, it takes 4.07 seconds. Reasonable. If a make this a 3 points trajectory of 0,5,10, I get the same answer. Now, If I make it 0,1,10, I get not only a different time (5.05s), but now the system moves negative in velocity and position at the start and goes very negative (-0.5 in position is 5% the wrong direction) and stays at a negative position for 1.2 seconds of the entire motion. This just seems wrong. After careful planning, it may be that the system simply cannot go negative here (which brings up another point of why there aren't max/min joint position conditions?). I realize that this is due to the original cubic spline which makes a dip down. I've looked and unfortunately for cases like this, where we might want a different model, I haven't found anything suitable in scipy (though PYthon really isn't my strong suit, so I don't know the libraries well). Also I tried the PPoly and Univariate models, with the simple case above but these both result in Python runtime errors. Any help would be appreciated.

Trajectory Violates Constraints

I've been trying to use toppra to generate a trajectory for a 7 DOF robot but the trajectories generated violate the velocity and acceleration limits.
Note that this issue follows the conversation from #41

Here is a minimal example to demonstrate the issue.
Code and data for the test can be found here

Test description

The toppra_test.npy file contains data for a trajectory that has 384 points for a 7 DOF robot.
Most of the code in toppra_example.py is taken from the toppra tutorial here

After sampling the generated trajectory to get the velocity and acceleration data, the maximum acc and vel per robot joint is found. Since the acc and vel limits are symmetric, the maximum acc and vel values are found by taking the max over their absolute values.

Expected results

It is expected that the maximum velocity and acceleration per joint is lower than their corresponding limits

Results

The results below show that the limits are not respected:

Joint 1:
Vel limit: 0.87, Acc limit: 0.8
Max Vel: 0.72, Max Acc: 1.02

Joint 2:
Vel limit: 0.87, Acc limit: 0.8
Max Vel: 0.57, Max Acc: 1.04

Joint 3:
Vel limit: 0.87, Acc limit: 0.8
Max Vel: 0.59, Max Acc: 1.30

Joint 4:
Vel limit: 0.87, Acc limit: 1.0
Max Vel: 0.62, Max Acc: 1.48

Joint 5:
Vel limit: 0.87, Acc limit: 1.0
Max Vel: 0.88, Max Acc: 1.40

Joint 6:
Vel limit: 0.87, Acc limit: 1.0
Max Vel: 0.85, Max Acc: 1.20

Joint 7:
Vel limit: 0.87, Acc limit: 1.0
Max Vel: 0.82, Max Acc: 1.41

Final Trajectory Violates Acceleration Limits

I've noticed that the optimized trajectories coming out of instance.compute_trajectory don't respect the acceleration limits. When I evaluate the trajectory at every point on ss_waypoints, at times the acceleration is almost double the limit.

Can TOPPRA solve the dynamic model considering friction?

If I want to consider Coulomb and Viscous friction into dynamic model:

M(q) * q_ddot + q_dot^T * C(q, q_dot) * q_dot + g(q) + Dv * q_dot + Fc *sign(q_dot) = torque

where M(q) , C(q) and g(q) are the same representation in tutorial of Second-Order Constraint.
Dv and Fc are coefficients of viscous and Coulomb friction, respectively.

Can TOPPRA solve it?