hungpham2511 / toppra Goto Github PK
View Code? Open in Web Editor NEWrobotic motion planning library
Home Page: https://hungpham2511.github.io/toppra/index.html
License: MIT License
robotic motion planning library
Home Page: https://hungpham2511.github.io/toppra/index.html
License: MIT License
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.
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).
Support for per way-point velocity constraints.
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?
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
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?
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.
The parametrization algorithm might fail when:
Users should be notified when any of these errors occur.
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.
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] _
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.
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
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?
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()
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
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.
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
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.
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?
Feature added.
TODO: Add doc.
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?
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?
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]
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.
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!
Have you considered releasing this module on pypi?
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.
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
?
We will use this issue to flesh out some key designs of the C++ interface.
There are a few core components that are required.
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.
The ability to add time varying acceleration limits similar to the time varying velocity limit would be very useful
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?
Is it possible to plan trajectories with a specific duration?
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
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.
The current trajectory classes do not allow the specification of trajectory.
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.
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]
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..
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 ?
Importing toppra after running python setup.py install
raises this error:
ModuleNotFoundError: No module named 'toppra._CythonUtils'
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?
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.
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`
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).
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
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.
It is expected that the maximum velocity and acceleration per joint is lower than their corresponding limits
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
I implemented TOPP-RA on my robot. Can I know how can I check Cartesian velocity profile after constraining Cartesian accelerations ?
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.