Closed EdsterG closed 5 years ago
Totals | |
---|---|
Change from base Build 159: | 0.01% |
Covered Lines: | 1165 |
Relevant Lines: | 1407 |
Thanks! Can you also provide an example/test-case whereby re-scaling is necessary? and also the case whereby re-scaling help to improve numerical stability?
Yes, here's one:
import numpy as np
import toppra
ws = [0.0, 0.04295308046937736, 0.08581756676459122, 0.1111111111111111, 0.12859047417912278, 0.17126877612957553, 0.21384940553807, 0.2222222222222222, 0.2563292562570282, 0.2987051845355027, 0.3333333333333333, 0.3409740105261145, 0.38313251983149993, 0.4251774650890799, 0.4444444444444444, 0.4671055675928987, 0.5089135189511323, 0.5505979827777779, 0.5555555555555556, 0.5921555964169503, 0.6335829726980746, 0.6666666666666666, 0.67487670172022, 0.7160333526637366, 0.7570494756271962, 0.7777777777777777, 0.7979216034875934, 0.8386462537817871, 0.8792199306068587, 0.8888888888888888, 0.9196391265372732, 0.9599003245563777, 1.0]
dof_values = [[1.570795727583652, 0.8589726756743996, 1.8730406382277955, 5.992112463573123e-07, 0.40956793428868077, 1.5707953680569031], [1.570795727583652, 0.8514344344768539, 1.8751945375954273, 5.918788143489779e-07, 0.4149522761185933, 1.5707953760598117], [1.570795727583652, 0.8439117415064956, 1.877283269244709, 5.8467550644168e-07, 0.42038623743966885, 1.5707953839405457], [1.570795727583652, 0.8394727388565967, 1.8784798331191392, 5.804912188526208e-07, 0.42362867621513706, 1.5707953885295238], [1.570795727583652, 0.8364051205776217, 1.879306730848801, 5.775996240271529e-07, 0.42586939676445007, 1.5707953917007842], [1.570795727583652, 0.8289151028538972, 1.881264821690949, 5.706494514204904e-07, 0.4314013236460252, 1.5707953993422221], [1.570795727583652, 0.821442226605746, 1.8831574428140774, 5.63823259206485e-07, 0.43698157877104765, 1.5707954068665682], [1.570795727583652, 0.8199728020387939, 1.8835175568660207, 5.625019042832871e-07, 0.4380908892860562, 1.5707954083268834], [1.570795727583652, 0.8139870369603015, 1.8849844971669225, 5.571193073583037e-07, 0.44260971406364613, 1.5707954142755411], [1.570795727583652, 0.8065500856440675, 1.886745889746594, 5.505358481401499e-07, 0.4482852728002073, 1.5707954215708666], [1.570795727583652, 0.8004728652209909, 1.888135017626974, 5.452397165864454e-07, 0.4529733653429035, 1.5707954274557745], [1.570795727583652, 0.7991319307184508, 1.8884415277374464, 5.440711288051981e-07, 0.45400778973497125, 1.5707954287542754], [1.570795727583652, 0.7917331363083608, 1.8900713206461432, 5.37723394099677e-07, 0.45977679123636306, 1.5707954358274996], [1.570795727583652, 0.7843542723240837, 1.891635180432797, 5.314908885835857e-07, 0.46559179543398554, 1.5707954427922717], [1.570795727583652, 0.780972928403188, 1.89232347477135, 5.286790460747049e-07, 0.4682848450163281, 1.570795445943712], [1.570795727583652, 0.7769959141766503, 1.8931330216380717, 5.253718587781108e-07, 0.47145231237614377, 1.5707954496503216], [1.570795727583652, 0.7696586424869443, 1.8945647615061283, 5.193645551494163e-07, 0.47735784419779237, 1.570795456403375], [1.570795727583652, 0.762343042788812, 1.8959303201033015, 5.134672339380729e-07, 0.4833078852987504, 1.570795463053151], [1.570795727583652, 0.7614729915853852, 1.8960853187997522, 5.127766321785719e-07, 0.48402293780572664, 1.5707954638343131], [1.570795727583652, 0.7550497052264493, 1.8972296204323944, 5.076781588429942e-07, 0.4893019225320192, 1.5707954696013613], [1.570795727583652, 0.7477792242463659, 1.8984625885424804, 5.019956025683694e-07, 0.4953394354020163, 1.5707954760497083], [1.570795727583652, 0.7419730547675822, 1.899397216714835, 4.975268194977618e-07, 0.5002109767084447, 1.570795481137339], [1.570795727583652, 0.7405321982842361, 1.8996291536341026, 4.964178482416728e-07, 0.501419896272523, 1.5707954823998833], [1.570795727583652, 0.7333092294469576, 1.9007292481597686, 4.909431907104745e-07, 0.5075427705841349, 1.570795488653566], [1.570795727583652, 0.7261109231902694, 1.9017628079196325, 4.855699377254027e-07, 0.5137075170809584, 1.5707954948124223], [1.570795727583652, 0.7224731179497794, 1.9022532038895068, 4.828954683315023e-07, 0.5168549263515738, 1.5707954978886334], [1.570795727583652, 0.7189378879922872, 1.9027297721522705, 4.802964110162626e-07, 0.519913588046302, 1.5707955008781045], [1.570795727583652, 0.7117907350233121, 1.9036300836204512, 4.7512094726794006e-07, 0.5261604295470954, 1.5707955068522494], [1.570795727583652, 0.7046700778123185, 1.9044636886918078, 4.700418990024241e-07, 0.5324474816867318, 1.5707955127364779], [1.570795727583652, 0.7029731811319765, 1.9046471319329716, 4.6884957847276257e-07, 0.5339609351259099, 1.5707955141229595], [1.570795727583652, 0.6975765319104932, 1.9052305374143312, 4.650576353729273e-07, 0.538774178866033, 1.5707955185323939], [1.570795727583652, 0.6905107145522589, 1.9059305835865932, 4.601665428757887e-07, 0.5451399500520047, 1.5707955242415836], [1.570795727583652, 0.6834732443141736, 1.9065637848226284, 4.5536702598553013e-07, 0.5515442190540538, 1.5707955298656155]]
gridpoints = [0.0, 0.015151515151515152, 0.030303030303030304, 0.04295308046937736, 0.045454545454545456, 0.06060606060606061, 0.07575757575757576, 0.08581756676459122, 0.09090909090909091, 0.10606060606060606, 0.1111111111111111, 0.12121212121212122, 0.12859047417912278, 0.13636363636363635, 0.15151515151515152, 0.16666666666666669, 0.17126877612957553, 0.18181818181818182, 0.19696969696969696, 0.21212121212121213, 0.21384940553807, 0.2222222222222222, 0.2272727272727273, 0.24242424242424243, 0.2563292562570282, 0.25757575757575757, 0.2727272727272727, 0.2878787878787879, 0.2987051845355027, 0.30303030303030304, 0.3181818181818182, 0.3333333333333333, 0.33333333333333337, 0.3409740105261145, 0.3484848484848485, 0.36363636363636365, 0.3787878787878788, 0.38313251983149993, 0.3939393939393939, 0.4090909090909091, 0.42424242424242425, 0.4251774650890799, 0.4393939393939394, 0.4444444444444444, 0.4545454545454546, 0.4671055675928987, 0.4696969696969697, 0.48484848484848486, 0.5, 0.5089135189511323, 0.5151515151515151, 0.5303030303030303, 0.5454545454545454, 0.5505979827777779, 0.5555555555555556, 0.5606060606060607, 0.5757575757575758, 0.5909090909090909, 0.5921555964169503, 0.6060606060606061, 0.6212121212121212, 0.6335829726980746, 0.6363636363636364, 0.6515151515151515, 0.6666666666666666, 0.6666666666666667, 0.67487670172022, 0.6818181818181819, 0.696969696969697, 0.7121212121212122, 0.7160333526637366, 0.7272727272727273, 0.7424242424242424, 0.7570494756271962, 0.7575757575757576, 0.7727272727272727, 0.7777777777777777, 0.7878787878787878, 0.7979216034875934, 0.8030303030303031, 0.8181818181818182, 0.8333333333333334, 0.8386462537817871, 0.8484848484848485, 0.8636363636363636, 0.8787878787878788, 0.8792199306068587, 0.8888888888888888, 0.8939393939393939, 0.9090909090909092, 0.9196391265372732, 0.9242424242424243, 0.9393939393939394, 0.9545454545454546, 0.9599003245563777, 0.9696969696969697, 0.9848484848484849, 1.0]
path = toppra.SplineInterpolator(ws, dof_values, bc_type="clamped")
vlim = np.asarray([8.011061266653973, 6.408849013323178, 7.712609964562942, 8.482300164692441, 6.53451271946677, 13.571680263507906])
alim = np.asarray([33.37942194439155, 26.703537555513243, 32.138492846223585, 35.34291735288517, 27.22190034335556, 56.548667764616276])
pc_vel = toppra.constraint.JointVelocityConstraint(np.vstack((-vlim, vlim)).T)
pc_acc = toppra.constraint.JointAccelerationConstraint(
np.vstack((-alim, alim)).T, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation
)
instance = toppra.algorithm.TOPPRA(
[pc_vel, pc_acc], path,
gridpoints=gridpoints,
scaling=1,
solver_wrapper="seidel"
)
jnt_traj, _ = instance.compute_trajectory(0, 0, bc_type="clamped")
I see. Look like problems appear when the motion is way too short. This is similar to what others have been reporting. Thanks.
Scaling seems like a great way to deal with numerical stability issues. However, the returned trajectory isn't correct unless the scaling is reverted right before returning. This PR reverts scaling before returning the final trajectory.