quangounet / topp

Time-Optimal Path Parameterization (à la Bobrow)

TOPP

NOTE: Starting June 2018, TOPP will no longer be actively maintained. Please use TOPP-RA, which provides all functionalities supported by TOPP while being faster and more robust (100% success rate guaranteed).

This is TOPP, the Time-Optimal Path Parameterization library by Quang-Cuong Pham ([email protected]).

If you use this library for your research, please reference the accompanying paper «A general, fast, and robust implementation of the time-optimal path parameterization algorithm», IEEE Transactions on Robotics, vol. 30(6), pp. 1533–1540, 2014.

If you use Admissible Velocity Propagation, please reference also «Admissible Velocity Propagation: Beyond quasi-static path planning for high-dimensional robots», The International Journal of Robotics Research, vol. 36(1), pp. 44–67, 2017.

Many thanks to Stéphane Caron, Rosen Diankov, Puttichai Lertkultanon, and others, for their contributions!

Requirements

The following software is required to install TOPP:

• Python (2.7 or above), with numpy, scipy and matplotlib
• Boost (1.46 or above), with Boost.Python

If you need OpenRAVE support (for dynamics computations), the following software is also required:

• OpenRAVE (installed from source, "master" branch, see http://openrave.org/docs/latest_stable/coreapihtml/installation_linux.html NB: one must install from the "master" branch, not the "latest stable" branch, so change git clone --branch latest_stable https://github.com/rdiankov/openrave.git into git clone --branch master https://github.com/rdiankov/openrave.git)

Installation

Follow the standard installation procedure: from the TOPP directory,

mkdir build
cd build
cmake ..
make
sudo make install

TOPP will be compiled with OpenRAVE support if the latter is found on your system.

Examples

Please try the test files in the tests/ folder or copy-paste test cases from https://github.com/quangounet/TOPP/wiki/Quick-examples

Documentation, Tutorials...

See the wiki https://github.com/quangounet/TOPP/wiki

topp's Issues

Use Cython for passing variables between C++ and Python

En passant : au labo, on utilise Cython pour wrapper du C++ au Python.
Ca permet de passer facilement des nombres/listes/tableaux, et c'est
complètement transparent pour l'utilisateur :

http://cython.readthedocs.io/en/latest/src/tutorial/cython_tutorial.html

Je ne vais pas coder ça maintenant, mais dans l'absolu je me dis que ce
serait bien pour TOPP (rapide à implémenter et mieux que les strings).

Automatic discrtimestep?

We need to find a way to compute a reasonable ds (discrtimestep) dynamically, since good values for this parameter are both system and trajectory dependent. For instance, in torques_pendulum_rave.py:

sdbeg_min, sdbeg_max = 0.0 , 0.0001
trajectorystring = """1.000000
2
0.0 0.0 -1.04335890092 0.547353298841
0.0 0.0 -2.24856384056 1.36915743417"""

Says "no solution" for discrtimestep=1e-3, but finds solutions (sd_end = [0.0,
2.80]) for discrtimestep=1e-4.

AVP ok but PP fails

On the following test case:

tuningsstring = "0.005000 0.005000 0.000000 10"
constraintstring = """-11.0 -7.0
11.0 7.0
0 0"""
trajectorystring = """1.000000
2
0.0 0.62309887553 1.66533453694e-16 0.0
0.0 -1.14223493975 3.33066907388e-16 -4.4408920985e-16
1.000000
2
0.623098875592 0.62309887553 -3.93787583145 2.48652201003
-1.14223493986 -1.14223493975 7.52345920063 -3.80261000559"""

AVP finds a solution but PP fails:

RunComputeProfiles: 0
RunVIP: 1
sdendmin = 0.0
sdendmax = 1.81565719434

Boost Compile Error

When I compile I get the error on Ubuntu 14.04:

TOPP/src/SO3Constraints.h:7:1: error: ‘multi_array’ in namespace ‘boost’ does not name a type
 boost::multi_array<dReal, 2> SkewFromVect(const std::vector<dReal>& vect);

I do not have OpenRave installed.

Thanks!

TrimSwitchPoints incrementing iterator at the end of the list

Hi Cuong,

I think there's a bug here:

if(scur>=0) {
    it++; // it may point to the end of the list and cannot be incremented

because the iterator can point at the end of the list switchpointslist in case it contains a single SwitchPoint of type SP_SINGULAR.

A solution can be either to change the if condition:

if(scur>=0 && it!=switchpointslist.end()) 

or to set scur=-1 before line 398

The same issue may probably happen also at line 358

How to use it in anaconda

How to build it in anaconda

RunVIP vs RunComputeProfiles

I ran into a problem where VIP (using RunVIP) was successful, but computing a valid speed profile using any of the end-speeds inside the interval obtained from VIP is failing.

(1) Running VIP starting with zero speed produces a valid sd_end interval:
discrtimestep 0.001 TOPP RunVIP (0,0) code: 1 (Success) sd_end: [ 0.0 , 0.187701511064 ]

(2) trying to obtain a speed profile via RunComputeProfiles fails for any of the speeds inside the sd_end speed interval:
discrtimestep 0.001 TOPP RunComputeProfiles (0, 0.0 ) code: 0 (Failure)
discrtimestep 0.001 TOPP RunComputeProfiles (0, 0.046925377766 ) code: 0 (Failure)
discrtimestep 0.001 TOPP RunComputeProfiles (0, 0.093850755532 ) code: 0 (Failure)
discrtimestep 0.001 TOPP RunComputeProfiles (0, 0.140776133298 ) code: 0 (Failure)
discrtimestep 0.001 TOPP RunComputeProfiles (0, 0.187701511064 ) code: 0 (Failure)

To reproduce the results:

wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/a
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/b
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/c
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/traj0
wget https://github.com/orthez/openrave-forcefields/blob/master/topp/test_topp_standalone3.py
python test_topp_standalone3.py

Edit: related to
#6
#7
#9

MotionPlanning.py

I think the motion planning script does not really fit into TOPP, the purpose of which is time-optimal path parametrization only. If we need it for testing, then it should go into the tests/ directory; preferably, every test should have its own sub-directory, and we would copy the motion planner in each of these. (The benefit of code duplication is that the tests would stay consistent with their motion planner, vs having future updates in the motion planner break up previous tests.)

Degree flexibility for trajectory chunks

The following trajectory:

trajectorystring = """1.000000
2
-0.537652888623 0.234321204132 0.0
0.183981719702 0.488818919925 3.33066907388e-16
1.000000
2
-0.303331684468 0.234321204132 0.0315593379862 0.507751607858
0.672800639677 0.488818919925 -2.25421681376 1.50275249907"""

Raises an assertion error in the current version because the two chunks do not have the same degree. We need to lift this limitation.

Warning

When compiling I get:

[ 10%] Building CXX object src/CMakeFiles/TOPPbindings.dir/TOPPbindings.cpp.o
/home/dave/ros/standalone_software/TOPP/src/TOPPbindings.cpp: In function ‘boost::python::list RunComputeSO3Constraints(std::string, std::string)’:
/home/dave/ros/standalone_software/TOPP/src/TOPPbindings.cpp:345:1: warning: control reaches end of non-void function [-Wreturn-type]
 };

Not sure what to return outside the if statement, but seems like an easy thing to fix.

Segmentation Fault

I ran into a problem where a small timestep size causes a segfault.
The code below is an example which is the same code as in Issue #23, but if with a timestep size of 1e-4 instead of 1e-3, leading to a segfault.
If i choose a stepsize of 1e-5 i get a memory error, does it mean that there is an upper limit on the number of waypoints or a lower limit on the timestep size?

Here is the code to reproduce the error (note that it works with discrtimestep = 1e-3)

from TOPP import Utilities
import TOPP
import numpy as np
from TOPP import TOPPpy
from pylab import *
ion()

Ndim = 4
discrtimestep= 1e-4
a = np.loadtxt('topp/a')
b = np.loadtxt('topp/b')
c = np.loadtxt('topp/c')
with open("topp/traj0", "r") as fh:
        trajectorystring = "%s" % fh.read()

vmax = 1e5*np.ones(Ndim) ## no velocity constraints
topp_inst = TOPP.QuadraticConstraints(trajectorystring, discrtimestep, vmax, list(a), list(b), list(c))
x = topp_inst.solver
ret = x.RunVIP(0,0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
raw_input("Press enter")

and the data:

wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/a
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/b
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/c
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/traj0

chunk duration is negative

when extracting the re-timed result itchunk->duration is negative (-7.8285737144529086e-06)

reparamtimestep

Need to document the fact that, by default, reparamtimestep = discrtimestep

To use another value s, use Reparameterize(s)

Division by zero in function StraightProfile

Hi Cuong.

I think there's a bug here:

TOPP.cpp Line 1701~1706:
// Address Switch Point
if (!AddressSwitchPoint(constraints, switchpoint, sbackward,
sdbackward, sforward, sdforward))
continue;

    // Add middle part
    constraints.resprofileslist.push_back(StraightProfile(sbackward,sforward,sdbackward,sdforward));

In function AddressSwitchPoint, when switchpoint's type is Tangent, Discontinuous or Zlajpah, sforward == sbackward.

TOPP.cpp Line 1666~1667:
dReal dtmod = 2 * (sforward - sbackward) / (sdforward + sdbackward);
dReal sdd = (sdforward - sdbackward) / dtmod;

sforward == sbackward means dtmod ==0 , and there is a Division by zero, then sdd will be a weird value.

Please forgive my poor English

RunComputeProfiles and RunVIP concurrence

In the current version,

ret = x.RunComputeProfiles(0, 1e-4)
print "RunComputeProfiles:", ret
ret = x.RunVIP(sdbeg_min, sdbeg_max)
print "RunVIP:", ret

Yields incorrect VIP results (because RunComputeProfiles affects x in a way that RunVIP does not detect). We should make the two independent in the future.

example segfaults

the torque limits example at https://github.com/quangounet/TOPP/wiki/Quick-examples#wiki-torque-limits

segfaults at:

    at /home/rdiankov/mujin/kawasaki_hi/weldingdynamics/TOPP/src/TOPPbindings.cpp:72
72          pconstraints->bisectionprecision = 0.01;

looks like pconstraints is NULL

Fwd integration starts above the MVC

Maybe this is a variant of issue #4 ?

discrtimestep = 0.005000
integrationtimestep = discrtimestep
sdbeg_min, sdbeg_max = 0.000000, 2.963261
tuningsstring = """0.005000 0.005000 0.000000 10"""
constraintstring = """-11.0 -7.0
11.0 7.0
0 0"""
trajectorystring = """1.000000
2
0.0598839777986 -0.767543731454 17.0567694943 -13.207517087
0.43299721927 1.80131573114 -5.85537754605 3.62106459564"""

The integration profile starts above the MVC, then fails. However, AVP propagation is still successfull. (Looking at the (alpha, beta) plots, it should not be.)

Problem in running "Bimanual manipulation (redundant actuation)"

Hello
I'm going to execute "Bimanual manipulation (redundant actuation)" as you explained in quick example pages. Everything is ok till line 85 where we you are going to use Bimanual.IK3 function to find start configuration of second robot. After spending two hours for calculating q_start2 the solution is still unknown. I think something is wrong in this line, e.g. desired goal position is not in robot task space or something. Would you please help me to fix this issue?

Line 85 : q_start2, error = Bimanual.IK3(robot1,desiredpose2,q_start=array([pi,0,0])-q_start1)

Same problem exist in line 103
Line 103: q_goal2, error = Bimanual.IK3(robot1,desiredpose2,q_start=array([pi,0,0])-q_goal1)

Thanks

Backward integration does not start

On the following trajectory (torques_pendulum_rave.py format):

sdbeg_min, sdbeg_max = 0.0, 0.0001
trajectorystring = """1.000000
2
0.0 -0.316841821141 -0.517973306853 0.413832196233
0.0 -1.83079350169 1.68090295997 -0.480535237087"""

On my machine, it seems the backward integration does not start: the integration profile (blue in the plot below) goes straight up to the MVC for s>0.8, then hits it and says there is no solution. However, I can get solutions using the RRT checker (red).

Repar

I'm noticing several errors with the Reparameterize function; it appears to be very sensitive to the reparameterization step size and there appears to be a lot of introduced noise. Specifically, valid velocity profiles tend to have a fair deal of noise at what appears to be the intersection point between profiles. Often times this will lead to torque profiles that violate constraints at these points despite the profile never entering the inadmissible region above the MVC.

From my robot over a very long, complex, and heavily constrained path:

The blue curve should not go above or below 0.75 in this scenario, all of those spikes above the flat part are violations. The MVC is not violated and the velocity profiles in the phase plane look good.

One can see this even in the simple test example torque_limits.py:

We can see that the constraint is violated by the black curve right at the start.

Additionally, decreasing the reparameterization step size can cause another issue. The SPieceToChunks function will throw an exception when SolveQuadraticEquation is called, usually when the reparameterization step size is set very small. Looking at SolveQuadraticEquation, it appears to be incomplete.

The error is reproducible on the examples provided with TOPP. Taking the torque_limits.py, if you set the time step to 0.0001 as in: x.ReparameterizeTrajectory(0.0001) then the failure will occur. Changing this to 0.001 as in: x.ReparameterizeTrajectory(0.001) then it will succeed.

CLC discontinuity

Sorry for opening yet another issue, but i faced another problem regarding a CLC discontinuity. After your answer on Issue #23, i fixed some problems on my path to make sure it will be smooth (some waypoints were too close to each other). The resulting path is shown below. It is basically a path where the robot (SE(2) holonomic point robot + fixed z-coordinate) first moves backwards then reaches a turn-around point with almost zero speed and then moves forward again, see MVC/CLC curve picture. The path is, however, smooth, and it can be seen that the MVC is never hitting zero exactly. So from my understanding i should be able to execute this path with a near-zero speed profile. However, the CLC is discontinuous which i don't understand. Do you have some idea what could cause that? Is the turn-around motion to difficult to handle maybe?

wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/clc_a
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/clc_b
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/clc_c
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/clc_W
wget https://raw.githubusercontent.com/orthez/openrave-forcefields/master/topp/clc_traj

from TOPP import Utilities,Trajectory
import TOPP
import numpy as np
from TOPP import TOPPpy
from pylab import *
from scipy import interpolate
ion()

Ndim = 4
discrtimestep= 1e-3
a = np.loadtxt('clc_a')
b = np.loadtxt('clc_b')
c = np.loadtxt('clc_c')
with open("clc_traj", "r") as fh:
        trajectorystring = "%s" % fh.read()
W = np.loadtxt('clc_W')
trajectorystring = Utilities.InterpolateViapoints(W)

vmax = 1e5*np.ones(Ndim) ## no velocity constraints
topp_inst = TOPP.QuadraticConstraints(trajectorystring, discrtimestep, vmax, list(a), list(b), list(c))
x = topp_inst.solver
ret = x.RunVIP(0,0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
x.WriteResultTrajectory()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist,switchpointslist,4)

ret = x.RunComputeProfiles(0,0)

MVC/CLC curves:

Zoom in at discontinuity:

Path in time domain:

Zoom in at turn-around point. blue is the input to TOPP, red is just concat of waypoints, showing that the path is smooth.

Path in XY-Plane (start: (-2,0), goal (-5.5,0)).

openrave exception in ZMP trajectory

With the following trajectory (zmp_hrp4 format):

trajectorystring = """1.500000
22
0.0 0.0 1.99242076297 -1.32221433021
0.0 0.0 2.53280855643 -1.67759722295
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.967382109132 -0.642750868261
0.0 0.0 -0.519933641176 0.348891374481
0.0 0.0 2.89830573159 -1.93488398449
0.0 0.0 -2.14170672114 1.44939278452
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 3.13256805344 -2.08537050225
0.0 0.0 4.59613512542 -3.04279209114
0.0 0.0 4.81882377745 -3.21312086562
0.0 0.0 -3.18430860851 2.12838441106
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0
0.0 0.0 0.0 0.0"""

I get the following openrave exception:

openrave_exception: openrave (Assert): [/home/caron/Software/openrave/include
/openrave/geometry.h:449] -> OpenRAVE::geometry::RaveTransform<T> 
OpenRAVE::geometry::RaveTransform<T>::operator*(const 
OpenRAVE::geometry::RaveTransform<T>&) const [with T = double], expr: 
fnorm > 0.99f && fnorm < 1.01f

ReparameterizeTrajectory

OK compris, il fallait lancer

x.ReparameterizeTrajectory()

Intuitivité de l'API, hum hum !... ;p

(Par exemple, tu pourrais attraper le débutant égaré en vérifiant dans
WriteResultTrajectory() si la trajectoire est vide, et le cas échéant en
mettant un petit warning du genre : "did you forget to run
ReparameterizeTrajectory()?".)

Can I apply this in cartesian space trajectory?

I am trying to apply on cartesian space trajectory for the robot tool to move at a constant speed. Can I apply for cartesian space?

Bug in RunComputeProfiles

In the following setting:

discrtimestep = 1e-4
sdbeg_min, sdbeg_max = 0., 1e-4
trajectorystring = """1.000000
2
1.02380743853 -0.621211430375 6.66133814775e-16 -4.4408920985e-16
-0.294007923442 -0.228798693785 1.66533453694e-16 0.0"""

(TOPPbindings.)RunVIP finds the correct sd_end, but RunComputeProfiles returns 0 (i.e. no solution).

Forward profile crossing the MVC

I guess this is related to the "variable ds" enhancement proposal, but since it results in an incorrect answer I file it as a bug here. With the following settings in torques_pendulum_rave.py:

discrtimestep = 5e-3
sdbeg_min, sdbeg_max = 0.0 , 0.0001
trajectorystring = """1.000000
2
0.0 -0.000441411097554 -0.16346942929 0.109925764182
0.0 -0.0902570804637 -0.811688277595 0.600976616276"""

The forward profile crosses the MVC, then drops to zero. The good answer (obtained with e.g. discrtimestep=1e-4) is that sd_end=(0., 4.39101), with solutions going below the singular point.

How