TOPP
TOPP copied to clipboard
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 https://github.com/quangounet/TOPP/issues/6 https://github.com/quangounet/TOPP/issues/7 https://github.com/quangounet/TOPP/issues/9
I just checked your trajectory, actually it's pretty complex and generates a lot of singularities that TOPP was not able to handle, see the MVC attached. The computation of the CLC failed and was not detected by VIP (VIP currently does not check for the continuity of the CLC, I should add that), which falsely reported success. However, when running RunComputeProfiles, the non-continuity of the CLC was detected and failure was reported.
May I ask where the trajectory comes from? Any way to make it a bit smoother before feeding to TOPP?
Here's the code to generate the MVC and CLC
from TOPP import Utilities
import TOPP
import numpy as np
from TOPP import TOPPpy
from pylab import *
ion()
Ndim = 4
discrtimestep= 1e-3
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")
I will work on detecting the non-continuity of the CLC and patching the CLC (even non-optimally).