CLC discontinuity

[aorthey]

Sorry for opening yet another issue, but i faced another problem regarding a CLC discontinuity. After your answer on Issue https://github.com/quangounet/TOPP/issues/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)).

[quangounet]

In the zoomed figure, I can see a singular switch point (green) around 0.56. My guess is that the algorithm was not able to go through this switch point because it was too low: any small numerical integration error will lead to hitting either sdot = 0 or the MVC, causing termination.

Looking at the path in the XY-plane, this low singular switch point might indeed correspond to the sharp cusp in the top right corner. One idea could be to avoid too sharp turns at the path planning stage, if possible.