diff --git a/.changeset/lucky-donuts-tell.md b/.changeset/lucky-donuts-tell.md new file mode 100644 index 000000000..914e87b59 --- /dev/null +++ b/.changeset/lucky-donuts-tell.md @@ -0,0 +1,5 @@ +--- +'@viamrobotics/motion-tools': patch +--- + +Add `interpolateTrajectory`, expanding a planned trajectory into preview playback frames budgeted per joint unit (degrees for revolute joints, millimetres for prismatic) diff --git a/.prettierignore b/.prettierignore index 96ddae943..5ccb42eec 100644 --- a/.prettierignore +++ b/.prettierignore @@ -24,5 +24,9 @@ static/test-fixtures # cannot round-trip them. src/lib/plugins/MotionPlanReplayer/__tests__/__fixtures__/ +# A 225-step capture, stored minified because it is 2,000 lines pretty-printed +# and nothing reads it by eye. Its sibling fixtures are small and stay formatted. +src/lib/motion/__tests__/__fixtures__/plan-linear-constrained.json + # IDE .cursor \ No newline at end of file diff --git a/src/lib/motion/__tests__/__fixtures__/plan-free-space.json b/src/lib/motion/__tests__/__fixtures__/plan-free-space.json new file mode 100644 index 000000000..8128a875e --- /dev/null +++ b/src/lib/motion/__tests__/__fixtures__/plan-free-space.json @@ -0,0 +1,19 @@ +{ + "plan": [ + { + "arm": [0, 0, 0, 0, 0, 0], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + 1.1050027617288314, -1.8778538694900169, -1.5693986191251539, -4.406731791553699, + 4.712389313270464, -0.46579159318886737 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + } + ] +} diff --git a/src/lib/motion/__tests__/__fixtures__/plan-gantry.json b/src/lib/motion/__tests__/__fixtures__/plan-gantry.json new file mode 100644 index 000000000..026cd6724 --- /dev/null +++ b/src/lib/motion/__tests__/__fixtures__/plan-gantry.json @@ -0,0 +1,16 @@ +{ + "plan": [ + { + "arm-1": [0, 0, 0, 0, 0, 0], + "arm-1_origin": [], + "gantry-1": [50], + "gantry-1_origin": [] + }, + { + "arm-1": [0, 0, 0, 0, 0, 0], + "arm-1_origin": [], + "gantry-1": [90.00000000000001], + "gantry-1_origin": [] + } + ] +} diff --git a/src/lib/motion/__tests__/__fixtures__/plan-linear-constrained.json b/src/lib/motion/__tests__/__fixtures__/plan-linear-constrained.json new file mode 100644 index 000000000..6357d52b9 --- /dev/null +++ b/src/lib/motion/__tests__/__fixtures__/plan-linear-constrained.json @@ -0,0 +1 @@ +{"plan":[{"arm":[0,0,0,0,0,0],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.002363,-0.065416,0.131886,-0.070124,-0.012278,-0.006263],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.00472,-0.094668,0.188379,-0.101806,-0.024554,-0.011741],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.007073,-0.117096,0.230622,-0.125741,-0.036823,-0.017539],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.009421,-0.135977,0.265438,-0.145418,-0.049085,-0.023724],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.011765,-0.153169,0.296694,-0.163609,-0.061342,-0.029519],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.014103,-0.168814,0.324689,-0.179971,-0.073591,-0.035436],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.016436,-0.183348,0.350331,-0.195069,-0.085834,-0.041381],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.018765,-0.19704,0.374177,-0.209249,-0.098069,-0.047297],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.021088,-0.210026,0.396515,-0.222628,-0.110297,-0.053217],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.023407,-0.222439,0.417619,-0.235371,-0.122513,-0.05912],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.025719,-0.234328,0.437593,-0.247473,-0.134722,-0.065067],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.028027,-0.24582,0.456697,-0.259157,-0.146922,-0.070969],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.030332,-0.256921,0.474946,-0.270354,-0.159114,-0.076897],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.03263,-0.267689,0.492463,-0.281154,-0.171296,-0.082841],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.034923,-0.278178,0.509356,-0.291642,-0.183466,-0.088764],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.037212,-0.288399,0.52565,-0.301792,-0.195625,-0.094704],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.039496,-0.298385,0.541414,-0.311652,-0.207773,-0.10065],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.041775,-0.308158,0.556692,-0.32125,-0.21991,-0.106602],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.044049,-0.317738,0.571527,-0.330607,-0.232036,-0.11256],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.046318,-0.327141,0.585954,-0.339741,-0.244149,-0.118524],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.048583,-0.336383,0.600002,-0.348668,-0.256249,-0.124496],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.050842,-0.345477,0.6137,-0.357402,-0.268337,-0.130475],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.053097,-0.354435,0.627073,-0.365957,-0.280412,-0.136462],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.055348,-0.363266,0.64014,-0.374345,-0.292473,-0.142458],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.057594,-0.371981,0.652923,-0.382575,-0.30452,-0.148463],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.059835,-0.380588,0.665439,-0.390657,-0.316553,-0.154476],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.062072,-0.389095,0.677703,-0.398601,-0.328571,-0.1605],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.064305,-0.397508,0.689731,-0.406413,-0.340575,-0.166534],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.066534,-0.405835,0.701536,-0.414102,-0.352563,-0.172579],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.068758,-0.414081,0.71313,-0.421673,-0.364536,-0.178635],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.070979,-0.422252,0.724524,-0.429134,-0.376493,-0.184703],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.073195,-0.430352,0.735728,-0.436489,-0.388434,-0.190782],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.075408,-0.438386,0.746753,-0.443745,-0.400359,-0.196875],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.077617,-0.446359,0.757607,-0.450905,-0.412267,-0.202981],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.079822,-0.454275,0.768298,-0.457975,-0.424157,-0.2091],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.082024,-0.462137,0.778833,-0.464958,-0.43603,-0.215234],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.084223,-0.469948,0.789221,-0.47186,-0.447886,-0.221382],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.086418,-0.477713,0.799468,-0.478683,-0.459724,-0.227546],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.088611,-0.485434,0.80958,-0.485433,-0.471543,-0.233725],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.0908,-0.493114,0.819563,-0.49211,-0.483344,-0.23992],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.092987,-0.500755,0.829423,-0.49872,-0.495125,-0.246132],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.095171,-0.508362,0.839166,-0.505267,-0.506881,-0.252361],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.097352,-0.515933,0.848792,-0.511749,-0.51863,-0.258608],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.099531,-0.523474,0.858311,-0.518173,-0.530353,-0.264874],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.101708,-0.530986,0.867725,-0.52454,-0.542055,-0.271158],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.103883,-0.538471,0.87704,-0.530853,-0.553738,-0.277462],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.106056,-0.545932,0.886258,-0.537115,-0.565399,-0.283786],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.108228,-0.553368,0.895384,-0.543327,-0.577039,-0.29013],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.110398,-0.560784,0.90442,-0.549492,-0.588658,-0.296495],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.112567,-0.56818,0.913371,-0.555611,-0.600255,-0.302881],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.114734,-0.575557,0.922239,-0.561688,-0.61183,-0.30929],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.116901,-0.582917,0.931028,-0.567724,-0.623383,-0.315722],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.119068,-0.590262,0.93974,-0.57372,-0.634913,-0.322177],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.121233,-0.597594,0.948378,-0.579678,-0.64642,-0.328655],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.123399,-0.604912,0.956945,-0.585601,-0.657904,-0.335159],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.125565,-0.612219,0.965443,-0.59149,-0.669364,-0.341687],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.12773,-0.619516,0.973875,-0.597346,-0.680801,-0.348241],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.129897,-0.626803,0.982242,-0.603172,-0.692213,-0.354822],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.132064,-0.634082,0.990548,-0.608968,-0.7036,-0.361429],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.134231,-0.641355,0.998793,-0.614736,-0.714963,-0.368064],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.1364,-0.648621,1.006981,-0.620477,-0.7263,-0.374727],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.138571,-0.655882,1.015113,-0.626194,-0.737612,-0.381419],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.140743,-0.663139,1.023191,-0.631886,-0.748898,-0.38814],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.142917,-0.670392,1.031216,-0.637557,-0.760157,-0.394892],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.145094,-0.677644,1.039192,-0.643203,-0.77139,-0.401672],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.147272,-0.684893,1.047116,-0.648836,-0.782596,-0.408487],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.149453,-0.692141,1.054993,-0.654445,-0.793775,-0.415334],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.151638,-0.699389,1.062825,-0.660038,-0.804925,-0.422212],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.153826,-0.706638,1.070612,-0.665614,-0.816048,-0.429125],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.156017,-0.713888,1.078355,-0.671175,-0.827143,-0.436071],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.158212,-0.721139,1.086057,-0.676722,-0.838208,-0.443053],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.160412,-0.728394,1.093717,-0.682256,-0.849244,-0.45007],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.162616,-0.735651,1.101338,-0.687778,-0.860251,-0.457123],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.164825,-0.742912,1.10892,-0.693289,-0.871228,-0.464214],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.167038,-0.750177,1.116465,-0.698789,-0.882174,-0.471343],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.169258,-0.757447,1.123974,-0.70428,-0.893089,-0.47851],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.171483,-0.764722,1.131447,-0.709763,-0.903973,-0.485716],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.173714,-0.772003,1.138886,-0.71524,-0.914825,-0.492964],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.175952,-0.77929,1.146292,-0.720711,-0.925645,-0.500252],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.178196,-0.786583,1.153664,-0.726176,-0.936433,-0.507582],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.180448,-0.793884,1.161005,-0.731637,-0.947188,-0.514955],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.182707,-0.801192,1.168315,-0.737095,-0.957909,-0.52237],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.184974,-0.808508,1.175595,-0.74255,-0.968597,-0.52983],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.187249,-0.815833,1.182845,-0.748004,-0.97925,-0.537336],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.189532,-0.823165,1.190067,-0.753457,-0.989867,-0.544887],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.191825,-0.830507,1.19726,-0.75891,-1.00045,-0.552485],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.194127,-0.837858,1.204426,-0.764364,-1.010996,-0.56013],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.196438,-0.845219,1.211566,-0.76982,-1.021506,-0.567825],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.19876,-0.852589,1.218679,-0.775279,-1.031979,-0.575568],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.201092,-0.85997,1.225766,-0.780741,-1.042415,-0.583362],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.203435,-0.867361,1.232828,-0.786207,-1.052812,-0.591208],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.20579,-0.874762,1.239866,-0.791678,-1.06317,-0.599106],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.208156,-0.882175,1.246879,-0.797155,-1.07349,-0.607058],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.210534,-0.889598,1.253868,-0.802638,-1.083769,-0.615064],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.212924,-0.897032,1.260834,-0.808128,-1.094008,-0.623125],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.215328,-0.904478,1.267777,-0.813627,-1.104205,-0.631242],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.217745,-0.911937,1.274699,-0.819137,-1.11436,-0.639414],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.220176,-0.919407,1.2816,-0.824658,-1.124473,-0.647645],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.222622,-0.926889,1.288478,-0.830188,-1.134542,-0.655934],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.225082,-0.934383,1.295334,-0.83573,-1.144568,-0.664284],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.227558,-0.941894,1.302178,-0.841296,-1.154546,-0.672688],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.230049,-0.949413,1.308994,-0.846864,-1.164481,-0.68116],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.232556,-0.956945,1.31579,-0.852447,-1.174369,-0.689695],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.23508,-0.96449,1.322566,-0.858046,-1.18421,-0.698294],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.237621,-0.972047,1.329321,-0.863659,-1.194004,-0.706959],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.240179,-0.979616,1.336054,-0.869284,-1.203751,-0.715692],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.242756,-0.987197,1.342767,-0.874925,-1.213449,-0.724494],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.245351,-0.994792,1.349462,-0.880587,-1.223095,-0.733361],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.247966,-1.002399,1.356139,-0.886268,-1.23269,-0.742299],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.250601,-1.010019,1.362797,-0.891965,-1.242231,-0.751309],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.253255,-1.017653,1.369436,-0.897687,-1.251721,-0.760388],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.25593,-1.0253,1.376055,-0.903429,-1.261158,-0.769539],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.258626,-1.032957,1.382653,-0.909184,-1.270538,-0.778768],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.261344,-1.04063,1.389236,-0.914972,-1.279864,-0.78807],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.264084,-1.048315,1.395801,-0.920782,-1.289132,-0.797448],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.266848,-1.056012,1.402346,-0.926614,-1.298343,-0.806904],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.269635,-1.063721,1.408871,-0.932469,-1.307494,-0.81644],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.272445,-1.071442,1.415379,-0.938349,-1.316587,-0.826055],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.275281,-1.079176,1.421867,-0.944255,-1.325617,-0.835751],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.278142,-1.086922,1.428338,-0.950188,-1.334584,-0.845531],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.281028,-1.094681,1.434789,-0.956148,-1.343488,-0.855394],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.283942,-1.10245,1.441221,-0.962136,-1.352326,-0.865342],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.286882,-1.110233,1.447635,-0.968154,-1.361099,-0.875377],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.289849,-1.118025,1.45403,-0.9742,-1.369805,-0.885499],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.292845,-1.12583,1.460406,-0.980275,-1.378441,-0.895709],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.295872,-1.133646,1.466764,-0.98638,-1.387,-0.906008],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.298926,-1.141471,1.473101,-0.992518,-1.395497,-0.916399],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.302011,-1.149308,1.479418,-0.998688,-1.403921,-0.926883],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.305126,-1.157154,1.485716,-1.00489,-1.412271,-0.93746],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.308273,-1.165011,1.491994,-1.011125,-1.420545,-0.948131],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.311452,-1.172877,1.498252,-1.017395,-1.428742,-0.958899],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.314664,-1.180752,1.50449,-1.023698,-1.43686,-0.969764],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.317909,-1.188636,1.510708,-1.030037,-1.444898,-0.980727],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.321189,-1.196529,1.516904,-1.03641,-1.452854,-0.99179],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.324504,-1.20443,1.52308,-1.04282,-1.460726,-1.002953],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.327855,-1.212338,1.529234,-1.049265,-1.468513,-1.014219],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.331242,-1.220254,1.535367,-1.055748,-1.476213,-1.025588],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.334666,-1.228177,1.541478,-1.062268,-1.483825,-1.037062],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.338128,-1.236106,1.547566,-1.068826,-1.491346,-1.04864],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.341628,-1.244041,1.553632,-1.075423,-1.498775,-1.060326],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.345168,-1.251982,1.559675,-1.082057,-1.50611,-1.072119],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.348748,-1.259927,1.565694,-1.088731,-1.51335,-1.084021],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.352368,-1.267877,1.571689,-1.095444,-1.520491,-1.096033],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.356031,-1.275831,1.57766,-1.102196,-1.527533,-1.108156],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.359735,-1.283787,1.583606,-1.108988,-1.534473,-1.120391],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.363483,-1.291746,1.589527,-1.115819,-1.54131,-1.13274],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.367275,-1.299707,1.595422,-1.122691,-1.54804,-1.145202],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.371112,-1.30767,1.601291,-1.129601,-1.554663,-1.157779],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.374995,-1.315632,1.607133,-1.136552,-1.561175,-1.170471],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.378923,-1.323595,1.612947,-1.143543,-1.567576,-1.183281],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.382899,-1.331557,1.618734,-1.150573,-1.573866,-1.196206],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.386923,-1.339517,1.624492,-1.157643,-1.580035,-1.209251],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.390996,-1.347474,1.63022,-1.164753,-1.586085,-1.222414],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.395119,-1.355428,1.635918,-1.171901,-1.592013,-1.235698],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.399292,-1.363377,1.641583,-1.179084,-1.597822,-1.249096],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.403517,-1.371323,1.647221,-1.186313,-1.603497,-1.262626],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.407795,-1.379262,1.652825,-1.193574,-1.609048,-1.276272],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.412125,-1.387194,1.658396,-1.200872,-1.614468,-1.29004],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.41651,-1.395118,1.663933,-1.208207,-1.619755,-1.30393],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.420949,-1.403035,1.669438,-1.215582,-1.624908,-1.317947],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.425445,-1.410941,1.674904,-1.222983,-1.629921,-1.332082],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.429997,-1.418835,1.680334,-1.230418,-1.634793,-1.34634],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.434607,-1.426718,1.685727,-1.237886,-1.639522,-1.360722],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.439275,-1.434588,1.691082,-1.245385,-1.644105,-1.375227],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.444003,-1.442445,1.696397,-1.252913,-1.64854,-1.389855],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.448791,-1.450286,1.701673,-1.26047,-1.652824,-1.404607],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.453641,-1.45811,1.706905,-1.268053,-1.656952,-1.419479],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.458553,-1.465919,1.712098,-1.275662,-1.660925,-1.434479],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.463528,-1.473708,1.717247,-1.283294,-1.664738,-1.449599],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.468567,-1.481478,1.722351,-1.290945,-1.668392,-1.464841],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.47367,-1.489227,1.727409,-1.298615,-1.671881,-1.480204],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.47884,-1.496953,1.73242,-1.306302,-1.675204,-1.495688],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.484076,-1.504656,1.737384,-1.314003,-1.678357,-1.511292],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.489379,-1.512335,1.742298,-1.321715,-1.681339,-1.527015],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.494752,-1.519987,1.747161,-1.329437,-1.684146,-1.542857],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.500193,-1.527613,1.751973,-1.337163,-1.68678,-1.558813],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.505704,-1.535211,1.756735,-1.344903,-1.689226,-1.574887],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.511288,-1.542776,1.761436,-1.352627,-1.691494,-1.591083],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.516944,-1.550311,1.766085,-1.360355,-1.693578,-1.607388],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.522672,-1.557814,1.770676,-1.368078,-1.695474,-1.623806],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.528474,-1.565283,1.775209,-1.37579,-1.697181,-1.640336],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.534351,-1.572717,1.779682,-1.383489,-1.698696,-1.656975],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.540303,-1.580113,1.784093,-1.391169,-1.700016,-1.673723],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.546332,-1.587472,1.788442,-1.39883,-1.701143,-1.690576],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.552438,-1.594791,1.792727,-1.406468,-1.702068,-1.707534],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.558621,-1.602069,1.796946,-1.414074,-1.702793,-1.724595],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.564884,-1.609305,1.801098,-1.421646,-1.703315,-1.741757],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.571226,-1.616497,1.80518,-1.42918,-1.703633,-1.759017],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.577647,-1.623644,1.809192,-1.436669,-1.703745,-1.776373],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.584152,-1.630744,1.813133,-1.444115,-1.703645,-1.793828],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.590737,-1.637796,1.817001,-1.451507,-1.703335,-1.811374],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.597404,-1.644799,1.820794,-1.458841,-1.702815,-1.829008],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.604155,-1.65175,1.824509,-1.466113,-1.702081,-1.846731],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.610989,-1.65865,1.828147,-1.473317,-1.701132,-1.86454],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.617907,-1.665496,1.831706,-1.480448,-1.699966,-1.882431],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.62491,-1.672288,1.835182,-1.487503,-1.698582,-1.900405],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.631999,-1.679022,1.838577,-1.494471,-1.69698,-1.918456],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.639173,-1.685697,1.841888,-1.501349,-1.695159,-1.936581],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.646433,-1.692314,1.845112,-1.508132,-1.693117,-1.95478],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.65378,-1.69887,1.848249,-1.514815,-1.690853,-1.97305],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.661214,-1.705364,1.851297,-1.521391,-1.688369,-1.991386],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.668737,-1.711797,1.854259,-1.527867,-1.685653,-2.009802],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.676346,-1.718163,1.857127,-1.534217,-1.68272,-2.028271],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.684043,-1.724464,1.859899,-1.540437,-1.679567,-2.046799],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.691828,-1.730696,1.86258,-1.546537,-1.676186,-2.065387],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.699701,-1.73686,1.865162,-1.552494,-1.672585,-2.084033],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.70766,-1.742952,1.867643,-1.558305,-1.668769,-2.102727],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.715709,-1.748974,1.870031,-1.563978,-1.664721,-2.121478],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.723847,-1.754925,1.87232,-1.569499,-1.660449,-2.140282],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.732072,-1.760799,1.874503,-1.574857,-1.655963,-2.159127],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.740384,-1.766599,1.876585,-1.580051,-1.651252,-2.178021],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.748784,-1.772321,1.878563,-1.585075,-1.646325,-2.196957],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.757271,-1.777965,1.880435,-1.589922,-1.641179,-2.215934],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.765844,-1.78353,1.8822,-1.594587,-1.635818,-2.234951],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.774503,-1.789014,1.883856,-1.599064,-1.630243,-2.254006],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.783247,-1.794414,1.885404,-1.603348,-1.624455,-2.273096],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.792076,-1.799732,1.886842,-1.607435,-1.618455,-2.292223],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.800988,-1.804964,1.888166,-1.611313,-1.612252,-2.311379],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.809983,-1.81011,1.889378,-1.614983,-1.605841,-2.330569],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.819061,-1.815168,1.890476,-1.618439,-1.599226,-2.34979],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.828219,-1.820136,1.891459,-1.621676,-1.592409,-2.369043],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.837456,-1.825015,1.892325,-1.624681,-1.5854,-2.38832],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.846773,-1.8298,1.893072,-1.627458,-1.578198,-2.407623],"arm_origin":[],"table":[],"table_origin":[]},{"arm":[-0.856167,-1.834494,1.893705,-1.630008,-1.570795,-2.426964],"arm_origin":[],"table":[],"table_origin":[]}]} diff --git a/src/lib/motion/__tests__/__fixtures__/plan-synthetic-obstacle-routed.json b/src/lib/motion/__tests__/__fixtures__/plan-synthetic-obstacle-routed.json new file mode 100644 index 000000000..073d1dd2e --- /dev/null +++ b/src/lib/motion/__tests__/__fixtures__/plan-synthetic-obstacle-routed.json @@ -0,0 +1,187 @@ +{ + "plan": [ + { + "arm": [0, 0, 0, 0, 0, 0], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [0, 0, 0, 0, 0, 0], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.026296498218373508, -0.15470611393736317, 0.07280992487199799, 0.12735964055862564, + 0.24431249119848666, -0.08898409746564592 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.026855998180466557, -0.15799773338283896, 0.07435907220970006, 0.13006942014497938, + 0.24951062930909274, -0.09087737613512774 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.04028399727069984, -0.2369966000742585, 0.1115386083145501, 0.19510413021746909, + 0.37426594396363916, -0.13631606420269163 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.047557496777909526, -0.279787652865444, 0.1316775237046772, 0.23033126484006763, + 0.44184173940151833, -0.16092868690595535 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.12868499128140226, -0.7570724724594368, 0.35630388767147947, 0.6232493048613595, + 1.1955717654394027, -0.4354540939808204 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.20571028488356596, -0.7822346502344796, 0.3987269923759556, 0.6309936839796001, + 1.1951027447273148, -0.4255465941728487 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -0.8475877315682634, -0.991919465026503, 0.7522528649132567, 0.6955301766316049, + 1.1911942387932488, -0.34298409577308436 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.216926284495618, -1.4392470699161528, 1.5064413929928322, 0.8332080276225484, + 1.1828560928005747, -0.16685076585358719 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.3239058589430677, -1.4741945390481568, 1.565362371749049, 0.8439641097312159, + 1.182204675144897, -0.15309034945362646 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.3453017738325572, -1.4811840328745576, 1.5771465675002925, 0.8461153261529494, + 1.1820743916137615, -0.15033826617363433 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.3202934038379324, -1.411438640525238, 1.592742600247066, 0.8238222471704858, + 1.218405987284108, -0.19107643316238532 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.317660943838498, -1.404097020277941, 1.594384287904621, 0.8214756072775949, + 1.2222303657757234, -0.19536466126646437 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.236054683856038, -1.17650679261174, 1.645276605288829, 0.7487297705979771, + 1.3407860990158014, -0.328299732492915 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.1873541738665048, -1.0406868180367492, 1.6756478269535984, 0.7053169325794953, + 1.4115371011106868, -0.4076319524183775 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.1807730238679195, -1.0223327674185072, 1.6797520460974862, 0.6994503328472682, + 1.4210980473397252, -0.4183525226785751 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.1794567938682023, -1.0186619572948588, 1.6805728899262637, 0.6982770129008227, + 1.4230102365855328, -0.42049663673061466 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.1583971138727285, -0.9599289953164845, 1.6937063911867045, 0.6795038937576955, + 1.4536052645184563, -0.45480246156324705 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.116277753881781, -0.8424630713597355, 1.719973393707586, 0.6419576554714411, + 1.514795320384303, -0.5234141112285119 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + }, + { + "arm": [ + -2.1559561769087057, -0.8733141758332765, 1.738883351746668, 0.7052277081276631, + 1.5707951402863713, -0.5851599053467483 + ], + "arm_origin": [], + "table": [], + "table_origin": [] + } + ] +} diff --git a/src/lib/motion/__tests__/interpolateTrajectory.spec.ts b/src/lib/motion/__tests__/interpolateTrajectory.spec.ts new file mode 100644 index 000000000..10213944c --- /dev/null +++ b/src/lib/motion/__tests__/interpolateTrajectory.spec.ts @@ -0,0 +1,526 @@ +import { describe, expect, it } from 'vitest' + +import { Pose } from '$lib/math' + +import type { FrameDescriptor } from '../frameDescriptors' +import type { JointMotion, JointMotions } from '../interpolateTrajectory' +import type { TrajectoryStep } from '../jointPose' + +import { + DEFAULT_DEGREES_PER_FRAME, + interpolatedFrames, + jointMotionsOf, + jointTravelRadians, + lerpTrajectoryStep, + segmentFrameCost, + waypointFrames, +} from '../interpolateTrajectory' +import freeSpacePlan from './__fixtures__/plan-free-space.json' +import gantryPlan from './__fixtures__/plan-gantry.json' +import linearPlan from './__fixtures__/plan-linear-constrained.json' +import obstaclePlan from './__fixtures__/plan-synthetic-obstacle-routed.json' + +const RAD_TO_DEG = 180 / Math.PI + +const plans = { + freeSpace: freeSpacePlan.plan as TrajectoryStep[], + linear: linearPlan.plan as TrajectoryStep[], + obstacle: obstaclePlan.plan as TrajectoryStep[], + gantry: gantryPlan.plan as TrajectoryStep[], +} + +/** The gantry rig's one prismatic axis; everything else in these fixtures is revolute. */ +const GANTRY_MOTIONS: JointMotions = new Map([['gantry-1', ['translational'] as const]]) + +const segmentDegrees = (trajectory: TrajectoryStep[]) => + trajectory + .slice(1) + .map((step, index) => jointTravelRadians(trajectory[index]!, step) * RAD_TO_DEG) + +describe('captured plans still have the shape these rules were built for', () => { + it.each([ + ['freeSpace', 2, 269, 271], + ['linear', 225, 7, 8], + ['obstacle', 21, 78, 79], + ] as const)('%s: %i steps, worst segment between %i° and %i°', (key, steps, lower, upper) => { + const trajectory = plans[key] + expect(trajectory).toHaveLength(steps) + + const worst = Math.max(...segmentDegrees(trajectory)) + expect(worst).toBeGreaterThan(lower) + expect(worst).toBeLessThan(upper) + }) + + // Dividing a segment by its own length would be NaN here. Structural rather than incidental: a + // CBiRRT-solved goal returns a path whose first node is the segment's start configuration. + it('obstacle plans contain a zero-length segment', () => { + expect(segmentDegrees(plans.obstacle).filter((d) => d === 0)).not.toHaveLength(0) + }) + + it('free-space plans do not', () => { + expect(segmentDegrees(plans.freeSpace).filter((d) => d === 0)).toHaveLength(0) + }) + + it('every step of a plan carries the same columns', () => { + for (const trajectory of Object.values(plans)) { + const keys = trajectory.map((step) => Object.keys(step).toSorted().join(',')) + expect(new Set(keys).size).toBe(1) + } + }) +}) + +describe('jointTravelRadians', () => { + it('reports the largest single-joint change, not the sum', () => { + const travel = jointTravelRadians({ arm: [0, 0, 0] }, { arm: [0.1, 0.5, -0.2] }) + expect(travel).toBeCloseTo(0.5) + }) + + it('spans every component in the step', () => { + const travel = jointTravelRadians({ arm: [0.1], gantry: [2] }, { arm: [0.2], gantry: [0] }) + expect(travel).toBeCloseTo(2) + }) + + it('ignores the zero-DoF frames a real trajectory carries', () => { + const travel = jointTravelRadians({ arm: [1], table: [] }, { arm: [1.5], table: [] }) + expect(travel).toBeCloseTo(0.5) + }) +}) + +/** + * Neither guard can fire on a real reply, since `ToFrameSystemInputs` serializes every node of a + * plan from one schema. They are pinned so a malformed one degrades instead of throwing or costing + * `NaN`. + */ +describe('a step that does not carry the same columns as its neighbour', () => { + it.each([ + ['segmentFrameCost', segmentFrameCost], + ['jointTravelRadians', jointTravelRadians], + ])('%s skips a component the next step drops', (_label, cost) => { + expect(cost({ arm: [0], gripper: [0] }, { arm: [0] })).toBe(0) + }) + + it.each([ + ['segmentFrameCost', segmentFrameCost], + ['jointTravelRadians', jointTravelRadians], + ])('%s skips a joint the next step drops', (_label, cost) => { + expect(cost({ arm: [0, 0] }, { arm: [0] })).toBe(0) + }) + + it('charges nothing for a component that appears only in the later step', () => { + expect(segmentFrameCost({ arm: [0] }, { arm: [0], gripper: [Math.PI] })).toBe(0) + }) +}) + +describe('lerpTrajectoryStep', () => { + const from: TrajectoryStep = { arm: [0, 10] } + const to: TrajectoryStep = { arm: [1, -10] } + + it.each([ + [0, [0, 10]], + [0.5, [0.5, 0]], + [1, [1, -10]], + ])('blends each joint at t=%s', (t, expected) => { + expect(lerpTrajectoryStep(from, to, t).arm).toEqual(expected) + }) + + it('leaves the endpoints untouched', () => { + lerpTrajectoryStep(from, to, 0.5) + expect(from.arm).toEqual([0, 10]) + expect(to.arm).toEqual([1, -10]) + }) + + it.each([ + ['only the start', { arm: [5] }, {}], + ['only the end', {}, { arm: [5] }], + ])('holds a component present in %s', (_label, start, end) => { + expect(lerpTrajectoryStep(start, end, 0.5).arm).toEqual([5]) + }) + + // Handing back an array the input plan still owns means an in-place edit of a frame rewrites the + // plan itself. + it.each([ + ['only the start', true], + ['only the end', false], + ])('copies a component present in %s rather than aliasing it', (_label, onStart) => { + const held = [5] + const start: TrajectoryStep = onStart ? { arm: held } : {} + const end: TrajectoryStep = onStart ? {} : { arm: held } + + expect(lerpTrajectoryStep(start, end, 0.5).arm).not.toBe(held) + }) + + it('carries joints a component gained between steps', () => { + expect(lerpTrajectoryStep({ arm: [0, 0] }, { arm: [1, 1, 9] }, 0.5).arm).toEqual([0.5, 0.5, 9]) + }) + + it('keeps joints a component lost between steps', () => { + expect(lerpTrajectoryStep({ arm: [0, 10, 20] }, { arm: [1] }, 0.5).arm).toEqual([0.5, 10, 20]) + }) + + it('blends straight across ±π rather than taking the short way round', () => { + expect(lerpTrajectoryStep({ arm: [3] }, { arm: [3.4] }, 0.5).arm![0]).toBeCloseTo(3.2) + }) +}) + +describe('waypointFrames', () => { + it("plays the planner's waypoints and nothing else", () => { + const frames = waypointFrames(plans.obstacle) + expect(frames.steps).toBe(plans.obstacle) + expect(frames.waypoints).toHaveLength(plans.obstacle.length) + }) + + // Length alone leaves the contents free: every waypoint could point at frame 0 and the assertion + // above would still hold. + it('indexes every frame as a waypoint, in order', () => { + expect(waypointFrames(plans.gantry).waypoints).toEqual([0, 1]) + expect(waypointFrames(plans.obstacle).waypoints).toEqual( + plans.obstacle.map((_, index) => index) + ) + }) + + it('reports no coarsening, since it never subdivides', () => { + expect(waypointFrames(plans.obstacle).coarsening).toBe(1) + expect(waypointFrames([]).coarsening).toBe(1) + }) +}) + +describe('interpolatedFrames', () => { + it.each(['freeSpace', 'linear', 'obstacle'] as const)( + 'keeps every planned waypoint of the %s plan, in order', + (key) => { + const planned = plans[key] + const { steps, waypoints } = interpolatedFrames(planned) + + expect(waypoints).toHaveLength(planned.length) + // By reference, not by value: `toEqual` also passes for a blend that lands on the waypoint, + // which is what dropping the explicit `steps.push(to)` produces. + for (const [index, frame] of waypoints.entries()) { + expect(steps[frame]).toBe(planned[index]) + } + expect(waypoints[0]).toBe(0) + expect(waypoints.at(-1)).toBe(steps.length - 1) + } + ) + + it.each(['freeSpace', 'linear', 'obstacle'] as const)( + 'holds the %s plan to the requested resolution', + (key) => { + const { steps } = interpolatedFrames(plans[key]) + const worst = Math.max(...segmentDegrees(steps)) + + // Allowing a hair over: a segment is split into a whole number of frames, so the last one + // lands on or under the target rather than exactly at it. + expect(worst).toBeLessThanOrEqual(DEFAULT_DEGREES_PER_FRAME + 0.001) + } + ) + + it('turns the free-space teleport into a sweep', () => { + expect(plans.freeSpace).toHaveLength(2) + expect(interpolatedFrames(plans.freeSpace).steps.length).toBeGreaterThan(150) + }) + + it('leaves an already-dense plan essentially alone', () => { + // Frames added, not a fraction of the plan's length: 6 of its 224 segments exceed the budget + // and contribute all 11, so a `× 1.1` bound would read as slack it does not have. + const { steps } = interpolatedFrames(plans.linear) + expect(steps.length - plans.linear.length).toBe(11) + }) + + it('spends frames where the motion is, not where the waypoints are', () => { + const planned = plans.obstacle + const { waypoints } = interpolatedFrames(planned) + + const segments = segmentDegrees(planned).map((degrees, index) => ({ + degrees, + frames: waypoints[index + 1]! - waypoints[index]!, + })) + + const longest = segments.toSorted((a, b) => b.degrees - a.degrees)[0]! + const zeroLength = segments.find((segment) => segment.degrees === 0)! + + expect(longest.frames).toBeGreaterThan(40) + expect(zeroLength.frames).toBe(1) + }) + + it('coarsens rather than truncates when the frame cap binds', () => { + const { steps, waypoints, coarsening } = interpolatedFrames(plans.linear, { degrees: 0.0001 }) + + expect(coarsening).toBeGreaterThan(1) + // The cap bounds interpolation; each planned waypoint keeps its own frame regardless, so the + // ceiling is the budget plus the plan's own length. + expect(steps.length).toBeLessThanOrEqual(2000 + plans.linear.length) + expect(steps.at(-1)).toEqual(plans.linear.at(-1)) + expect(waypoints).toHaveLength(plans.linear.length) + }) + + // The cap-binding case above asserts how many waypoints there are and never where they point. + it('still points each waypoint at its own frame when the cap binds', () => { + const { steps, waypoints, coarsening } = interpolatedFrames(plans.linear, { degrees: 0.0001 }) + + expect(coarsening).toBeGreaterThan(1) + for (const [index, frame] of waypoints.entries()) { + expect(steps[frame]).toBe(plans.linear[index]) + } + }) + + // Nothing else here pins `coarsening` to a number, only to 1, so a scaled or offset value would + // be wrong on every plan with every other assertion green. + it('reports a coarsening of exactly 1 when the budget is met', () => { + expect(interpolatedFrames(plans.linear).coarsening).toBe(1) + expect(interpolatedFrames(plans.freeSpace).coarsening).toBe(1) + }) + + it('reports coarsening as the factor the cap stretched each frame by', () => { + const budget = { degrees: 0.0001 } + const total = plans.linear + .slice(1) + .reduce((sum, step, index) => sum + segmentFrameCost(plans.linear[index]!, step, budget), 0) + + expect(interpolatedFrames(plans.linear, budget).coarsening).toBeCloseTo(total / 2000, 6) + }) + + it.each([ + ['an empty plan', []], + ['a single-step plan', [{ arm: [0] }]], + ])('returns %s unchanged', (_label, planned) => { + expect(interpolatedFrames(planned).steps).toBe(planned) + }) +}) + +/** + * `plan-gantry.json` is a 40 mm slide with the arm held still. Costed in radians it comes to 1,529 + * frames, and a longer stroke alongside real arm motion collapses every arm segment to one frame. + */ +describe('a plan with a prismatic joint in it', () => { + /** + * Exact, not a band: a band wide enough to read as sane accepts a millimetre budget anywhere from + * 2 to 20. Ten and not nine because the stroke is `90.00000000000001 - 50`, which `Math.ceil` + * rounds up. + */ + it('spends exactly the budgeted frames on a 40 mm slide', () => { + const { steps } = interpolatedFrames(plans.gantry, { motions: GANTRY_MOTIONS }) + + expect(steps).toHaveLength(10) + }) + + // The stroke and the arm motion in separate segments, which is where the damage is: read as + // radians, 500 mm is 28,648°, enough on its own to blow the frame cap and coarsen what follows. + it('lets the arm keep its own resolution when a long stroke shares the plan', () => { + const together: TrajectoryStep[] = [ + { 'arm-1': [0], 'gantry-1': [0] }, + { 'arm-1': [0], 'gantry-1': [500] }, + { 'arm-1': [(12 * Math.PI) / 180], 'gantry-1': [500] }, + ] + + const { steps, waypoints } = interpolatedFrames(together, { motions: GANTRY_MOTIONS }) + // `segmentDegrees` is unit-blind by design, so read the arm's column on its own. + const armDegrees = segmentDegrees(steps.map((step) => ({ 'arm-1': step['arm-1']! }))) + + expect(waypoints[2]! - waypoints[1]!).toBeGreaterThan(1) + expect(Math.max(...armDegrees)).toBeLessThanOrEqual(DEFAULT_DEGREES_PER_FRAME + 0.001) + }) + + it('reads an unlabelled joint as revolute', () => { + const unlabelled = interpolatedFrames(plans.gantry) + const labelled = interpolatedFrames(plans.gantry, { motions: GANTRY_MOTIONS }) + + expect(unlabelled.steps.length).toBeGreaterThan(labelled.steps.length) + }) +}) + +describe('segmentFrameCost', () => { + it('costs a revolute joint by the degree budget', () => { + const cost = segmentFrameCost({ arm: [0] }, { arm: [Math.PI / 2] }, { degrees: 1.5 }) + expect(cost).toBeCloseTo(60) + }) + + it('costs a prismatic joint by the millimetre budget', () => { + const motions: JointMotions = new Map([['gantry', ['translational'] as const]]) + const cost = segmentFrameCost({ gantry: [0] }, { gantry: [40] }, { millimetres: 5, motions }) + + expect(cost).toBeCloseTo(8) + }) + + it('takes the costliest joint, so the two units never add up', () => { + const motions: JointMotions = new Map([['gantry', ['translational'] as const]]) + const cost = segmentFrameCost( + { arm: [0], gantry: [0] }, + { arm: [Math.PI / 2], gantry: [40] }, + { degrees: 1.5, millimetres: 5, motions } + ) + + expect(cost).toBeCloseTo(60) + }) + + // Every other prismatic assertion here passes `millimetres: 5`, which is the default, so ignoring + // the option entirely would look correct. + it('reads the millimetre budget rather than always defaulting it', () => { + const motions: JointMotions = new Map([['gantry', ['translational'] as const]]) + const cost = segmentFrameCost({ gantry: [0] }, { gantry: [40] }, { millimetres: 20, motions }) + + expect(cost).toBeCloseTo(2) + }) + + it('costs a segment that moves nothing at zero', () => { + expect(segmentFrameCost({ arm: [1, 2] }, { arm: [1, 2] })).toBe(0) + }) + + it.each([ + ['zero', 0], + ['negative', -1.5], + ['NaN', Number.NaN], + ['Infinity', Number.POSITIVE_INFINITY], + ])('falls back to the default for a %s degree budget', (_label, degrees) => { + const from = { arm: [0] } + const to = { arm: [Math.PI / 2] } + + expect(segmentFrameCost(from, to, { degrees })).toBe(segmentFrameCost(from, to)) + }) + + it.each([ + ['zero', 0], + ['NaN', Number.NaN], + ])('falls back to the default for a %s millimetre budget', (_label, millimetres) => { + const motions: JointMotions = new Map([['gantry', ['translational'] as const]]) + const from = { gantry: [0] } + const to = { gantry: [40] } + + expect(segmentFrameCost(from, to, { millimetres, motions })).toBe( + segmentFrameCost(from, to, { motions }) + ) + }) + + it('still fills in a free-space plan when handed a zero budget', () => { + expect(interpolatedFrames(plans.freeSpace, { degrees: 0 }).steps.length).toBeGreaterThan(150) + }) +}) + +// `constructor` and `__proto__` are legal RDK resource names. Only `lerpTrajectoryStep` is hurt; the +// cost functions' joint guard absorbs a prototype value. +describe('a component named like an Object.prototype member', () => { + const proto = (json: string) => JSON.parse(json) as TrajectoryStep + + it.each(['constructor', 'toString', 'hasOwnProperty', '__proto__'])( + 'costs a segment that drops %s without reading the prototype', + (name) => { + const from = proto(`{"arm": [0], ${JSON.stringify(name)}: []}`) + const to = proto('{"arm": [1]}') + + expect(segmentFrameCost(from, to)).toBeCloseTo((1 * RAD_TO_DEG) / DEFAULT_DEGREES_PER_FRAME) + expect(jointTravelRadians(from, to)).toBeCloseTo(1) + } + ) + + it.each(['constructor', 'toString', 'hasOwnProperty'])('blends a plan containing %s', (name) => { + const from = proto(`{"arm": [0], ${JSON.stringify(name)}: []}`) + const to = proto('{"arm": [1]}') + + const blended = lerpTrajectoryStep(from, to, 0.5) + expect(blended.arm).toEqual([0.5]) + expect(blended[name]).toEqual([]) + }) + + it('keeps a component named __proto__ as a key rather than a setter', () => { + const blended = lerpTrajectoryStep( + proto('{"__proto__": [0]}'), + proto('{"__proto__": [2]}'), + 0.5 + ) + + expect(Object.keys(blended)).toEqual(['__proto__']) + expect(blended['__proto__']).toEqual([1]) + }) + + it('interpolates a plan containing one end to end', () => { + const planned = [proto('{"arm": [0], "constructor": []}'), proto('{"arm": [1]}')] + expect(interpolatedFrames(planned).steps.length).toBeGreaterThan(30) + }) +}) + +describe('jointMotionsOf', () => { + const joint = ( + componentName: string, + jointIndex: number, + motion: JointMotion, + mimic?: { multiplier: number; offset: number } + ): FrameDescriptor => ({ + kind: 'joint', + motion, + name: `${componentName}:joint-${jointIndex}`, + parent: 'world', + axis: { X: 0, Y: 0, Z: 1 }, + componentName, + jointIndex, + mimic, + uuid: new Uint8Array(16) as Uint8Array, + }) + + it('indexes each joint the way a trajectory step does', () => { + const motions = jointMotionsOf([ + joint('gantry', 0, 'translational'), + joint('arm', 1, 'rotational'), + ]) + + expect(motions.get('gantry')).toEqual(['translational']) + expect(motions.get('arm')?.[1]).toBe('rotational') + }) + + // The tests around this one use a single joint per component, which leaves the accumulation + // untested: rebuilding the array rather than reading it back keeps only the last axis. + it('keeps every joint of a component, not just the last', () => { + const motions = jointMotionsOf([ + joint('gantry', 0, 'translational'), + joint('gantry', 1, 'translational'), + joint('gantry', 2, 'translational'), + ]) + + expect(motions.get('gantry')).toEqual(['translational', 'translational', 'translational']) + }) + + it('ignores frames that are not joints', () => { + const motions = jointMotionsOf([ + { + kind: 'static', + name: 'table', + parent: 'world', + localPose: new Pose(), + geometry: null, + uuid: new Uint8Array(16) as Uint8Array, + }, + joint('arm', 0, 'rotational'), + ]) + + expect([...motions.keys()]).toEqual(['arm']) + }) + + // Both orders, because the winner is decided by frame-system key order: pinning only the order + // that happens to fail leaves half of it green. + it.each([ + [ + 'source first', + [ + joint('gantry', 0, 'translational'), + joint('gantry', 0, 'rotational', { multiplier: 1, offset: 0 }), + ], + ], + [ + 'mimic first', + [ + joint('gantry', 0, 'rotational', { multiplier: 1, offset: 0 }), + joint('gantry', 0, 'translational'), + ], + ], + ] as const)('lets the column owner win over a mimic of it, %s', (_label, descriptors) => { + expect(jointMotionsOf([...descriptors]).get('gantry')).toEqual(['translational']) + }) + + // 60 anchors the whole-plan consequence: the same slide costed in degrees is 1,529 frames. + it('spends a sane number of frames on a slide whose column is mimicked by a revolute joint', () => { + const motions = jointMotionsOf([ + joint('gantry-1', 0, 'translational'), + joint('gantry-1', 0, 'rotational', { multiplier: 1, offset: 0 }), + ]) + + expect(interpolatedFrames(plans.gantry, { motions }).steps.length).toBeLessThan(60) + }) +}) diff --git a/src/lib/motion/interpolateTrajectory.ts b/src/lib/motion/interpolateTrajectory.ts new file mode 100644 index 000000000..3e0add129 --- /dev/null +++ b/src/lib/motion/interpolateTrajectory.ts @@ -0,0 +1,228 @@ +/** + * Turns a planned trajectory into frames to play back, allocating them in proportion to joint + * travel so playback reads at a constant speed whether the planner returned 2 waypoints or 225. + */ + +import type { FrameDescriptor } from './frameDescriptors' +import type { TrajectoryStep } from './jointPose' + +const RAD_TO_DEG = 180 / Math.PI + +/** + * Joint degrees per frame. At the preview's frame interval this works out to a plausible arm speed, + * which is the only claim being made — the real duration is unknowable from a trajectory. + */ +export const DEFAULT_DEGREES_PER_FRAME = 1.5 + +/** + * Millimetres per frame: the arc {@link DEFAULT_DEGREES_PER_FRAME} sweeps at a 191 mm mid-link + * radius. Erring short only over-samples a prismatic axis; erring long under-samples it into a jump. + */ +export const DEFAULT_MILLIMETRES_PER_FRAME = 5 + +/** How a joint's values are measured — the same two kinds a `JointFrameDescriptor` distinguishes. */ +export type JointMotion = 'rotational' | 'translational' + +/** + * Which joints are prismatic, indexed the way a trajectory step is: `motions.get('gantry')[0]`. + * Anything absent reads as rotational, which is what every joint in the captured plans is. + */ +export type JointMotions = ReadonlyMap + +export interface FrameBudget { + /** Degrees of revolute travel one frame represents. */ + degrees?: number + /** Millimetres of prismatic travel one frame represents. */ + millimetres?: number + motions?: JointMotions +} + +/** + * The joint kinds a plan's descriptors describe, in the shape {@link FrameBudget} wants. Mimics are + * skipped: a mimic's `jointIndex` addresses its source's column, so its motion would mislabel it. + */ +export const jointMotionsOf = (descriptors: readonly FrameDescriptor[]): JointMotions => { + const motions = new Map() + + for (const descriptor of descriptors) { + if (descriptor.kind !== 'joint' || descriptor.mimic) continue + + const kinds = motions.get(descriptor.componentName) ?? [] + kinds[descriptor.jointIndex] = descriptor.motion + motions.set(descriptor.componentName, kinds) + } + + return motions +} + +/** + * Backstop for a plan beyond anything observed; the worst real plan came to 236 frames. It bounds + * interpolation only, since every planned waypoint keeps its own frame whatever this is set to. + */ +const MAX_INTERPOLATED_FRAMES = 2000 + +export interface PreviewFrames { + /** What to render, in order. Contains every planned waypoint, plus any frames added between. */ + steps: TrajectoryStep[] + /** + * Where each planned waypoint landed in `steps`. Always starts at 0 and ends at the last index, + * so a scrubber can mark which frames are real data and which are drawn between them. + */ + waypoints: number[] + /** + * How much the cap stretched each frame: 1 when the requested budget was met, higher when a plan + * long enough to exceed {@link MAX_INTERPOLATED_FRAMES} had to be coarsened to fit. + */ + coarsening: number +} + +/** + * The largest single-joint change between two steps, unit-blind and with no production caller: the + * spec measures captured plans against it, while {@link segmentFrameCost} budgets the frames. + */ +export const jointTravelRadians = (from: TrajectoryStep, to: TrajectoryStep): number => { + let worst = 0 + + for (const [component, start] of Object.entries(from)) { + const end = to[component] + if (!end) continue + + for (const [index, value] of start.entries()) { + const target = end[index] + if (target === undefined) continue + worst = Math.max(worst, Math.abs(target - value)) + } + } + + return worst +} + +/** + * `from` and `to` blended at `t`, over the union of both steps' components. Not angle-aware: RDK + * collision-checks with this same expression, so wrapping would draw a path it never validated. + */ +export const lerpTrajectoryStep = ( + from: TrajectoryStep, + to: TrajectoryStep, + t: number +): TrajectoryStep => { + // `constructor` and `__proto__` are legal RDK resource names. On a plain object the first reads a + // function off the prototype and the second hits the prototype setter, so neither stays data. + const blended = Object.create(null) as TrajectoryStep + + for (const component of new Set([...Object.keys(from), ...Object.keys(to)])) { + const start = Object.hasOwn(from, component) ? from[component] : undefined + const end = Object.hasOwn(to, component) ? to[component] : undefined + + if (!start) { + if (end) blended[component] = [...end] + continue + } + if (!end) { + blended[component] = [...start] + continue + } + + blended[component] = start.map((value, index) => { + const target = end[index] + return target === undefined ? value : value + (target - value) * t + }) + + // A component that gained joints between steps would otherwise lose them here. + if (end.length > start.length) blended[component].push(...end.slice(start.length)) + } + + return blended +} + +/** + * A budget figure that can be divided by, or the default. `?? fallback` only catches null and + * undefined: a zero pins every plan to the frame cap, and a `NaN` collapses every segment to one + * frame. + */ +const perFrame = (value: number | undefined, fallback: number): number => + value !== undefined && Number.isFinite(value) && value > 0 ? value : fallback + +/** + * What a segment costs in frames: each joint's change over the budget for its own kind, largest + * quotient wins. Normalizing before the max is the point: a raw max lets a millimetre outweigh a + * radian 57×. + */ +export const segmentFrameCost = ( + from: TrajectoryStep, + to: TrajectoryStep, + budget: FrameBudget = {} +): number => { + const degrees = perFrame(budget.degrees, DEFAULT_DEGREES_PER_FRAME) + const millimetres = perFrame(budget.millimetres, DEFAULT_MILLIMETRES_PER_FRAME) + + let worst = 0 + + for (const [component, start] of Object.entries(from)) { + const end = to[component] + if (!end) continue + const kinds = budget.motions?.get(component) + + for (const [index, value] of start.entries()) { + const target = end[index] + if (target === undefined) continue + + const delta = Math.abs(target - value) + const cost = + kinds?.[index] === 'translational' ? delta / millimetres : (delta * RAD_TO_DEG) / degrees + worst = Math.max(worst, cost) + } + } + + return worst +} + +/** One frame per planned waypoint — the trajectory exactly as the planner returned it. */ +export const waypointFrames = (trajectory: TrajectoryStep[]): PreviewFrames => ({ + steps: trajectory, + waypoints: trajectory.map((_, index) => index), + coarsening: 1, +}) + +/** + * Frames spread across `trajectory` at roughly one budget's worth of joint travel each. A segment + * shorter than that, a zero-length one included, still gets its frame from `steps.push(to)` below. + */ +export const interpolatedFrames = ( + trajectory: TrajectoryStep[], + budget: FrameBudget = {} +): PreviewFrames => { + if (trajectory.length < 2) return waypointFrames(trajectory) + + const travel: number[] = [] + let total = 0 + for (let step = 1; step < trajectory.length; step += 1) { + const segment = segmentFrameCost(trajectory[step - 1]!, trajectory[step]!, budget) + travel.push(segment) + total += segment + } + + // Coarsen rather than truncate: every segment still gets its frames, just fewer of them. + const resolution = Math.max(1, total / MAX_INTERPOLATED_FRAMES) + + const steps: TrajectoryStep[] = [trajectory[0]!] + const waypoints: number[] = [0] + + for (const [index, segment] of travel.entries()) { + const from = trajectory[index]! + const to = trajectory[index + 1]! + // `max(1, …)` keeps this a usable divisor below rather than granting the minimum frame, which + // `steps.push(to)` already does — a zero-length segment would otherwise `ceil` to 0. + const divisions = Math.max(1, Math.ceil(segment / resolution)) + + // The segment's own start is already in `steps` as the previous segment's end, so this walks + // the interior and finishes on `to` exactly rather than on a blend that rounds to it. + for (let division = 1; division < divisions; division += 1) { + steps.push(lerpTrajectoryStep(from, to, division / divisions)) + } + steps.push(to) + waypoints.push(steps.length - 1) + } + + return { steps, waypoints, coarsening: resolution } +}