search

hungpham2511 / toppra

robotic motion planning library
https://hungpham2511.github.io/toppra/index.html
MIT License
625 stars 170 forks source link

Undo Scaling Before Returning #35

Closed EdsterG closed 5 years ago

EdsterG commented 5 years ago

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.

coveralls commented 5 years ago

Pull Request Test Coverage Report for Build 164

Totals Coverage Status
Change from base Build 159: 0.01%
Covered Lines: 1165
Relevant Lines: 1407
💛 - Coveralls
hungpham2511 commented 5 years ago

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?

EdsterG commented 5 years ago

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")
hungpham2511 commented 5 years ago

I see. Look like problems appear when the motion is way too short. This is similar to what others have been reporting. Thanks.