├── LICENSE ├── README.md ├── matlab ├── CubicInterpolation.m ├── LinearInterpolation.m ├── ParabolicInterpolation.m ├── Polynomial5Interpolation.m ├── main.m └── problem_def.m ├── python └── PolynomialInterpolation.py ├── qrcode.jpg ├── qrcode.png └── results ├── comparison.png ├── cubic.png ├── linear.png ├── parabolic.png └── poly_5.png /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Polynomial Interpolation（多项式曲线插值） 2 | In this project, we provide the source code of polynomial interpolation methods for smooth trajectory interpolation, including the linear interpolation, parabolic interpolation, cubic interpolation and interpolation of polynomial with five degrees. There are two branchs for both Matlab and Python. The experimental results are shown in following pictures. 3 | 4 | 在这个项目里面，我们提供多项式插值的源代码，包括线性插值、抛物线插值、三次多项式插值和五次多项式插值，提供Matlab和Python两个版本的代码。实验结果如下图所示： 5 | 6 | 7 | 8 | ## 1. 线性插值（Linear trajectory） 9 | 10 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/results/linear.png) 11 | 12 | ## 2. 抛物线插值（Parabolic trajectory） 13 | 14 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/results/parabolic.png) 15 | 16 | ## 3. 三次多项式插值（Cubic trajectory） 17 | 18 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/results/cubic.png) 19 | 20 | ## 4. 五次多项式插值（Polynomial of degree five） 21 | 22 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/results/poly_5.png) 23 | 24 | ## 5. 结果对比（Comparison） 25 | 26 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/results/comparison.png) 27 | 28 | --- 29 | 30 | 获取本项目代码附带的文章教程有以下3个途径（For tutorials）： 31 | 32 | 1. 知乎专栏【AI与机器人】：https://zhuanlan.zhihu.com/c_1212783320150577152 33 | 2. CSDN博客【沙漏】：https://blog.csdn.net/shakehands2012 34 | 3. 微信公众号：沙漏AI机器人。 35 | 36 | ![image](https://github.com/chauby/PolynomialInterpolation/blob/main/qrcode.png) 37 | 38 | 39 | 40 | 更多联系方式（More contact information）： 41 | 42 | - Email: chaubyZou@163.com 43 | -------------------------------------------------------------------------------- /matlab/CubicInterpolation.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Cubic Interpolation 3 | % *************************************************************************** 4 | % Author: Chaobin 5 | % Email: chaubyZou@163.com 6 | % Date: October 2020 7 | % *************************************************************************** 8 | % Language: Matlab 9 | % Also available in: Python 10 | % Required library: None 11 | % *************************************************************************** 12 | 13 | classdef CubicInterpolation < handle 14 | properties 15 | name = 'cubic interpolation'; 16 | q_via = []; 17 | t_via = []; 18 | end 19 | 20 | methods 21 | % crate the objective 22 | % name: string 23 | % q_via: N x 3 array 24 | % t_via: N x 1 array 25 | function obj = CubicInterpolation(name, q_via, t_via) 26 | obj.name = name; 27 | obj.q_via = q_via; 28 | obj.t_via = t_via; 29 | if length(q_via(:,1)) ~= length(t_via) 30 | error('The q_via and t_via must have a same length'); 31 | end 32 | end 33 | 34 | % cubic interpolation with two data points 35 | % q0: the first data point 36 | % q1: the second data point 37 | % v0: the velocity of the first data point 38 | % v1: the velocity of the second data point 39 | % t0: the time of the first data point 40 | % t1: the time of the second data point 41 | % a0~a5: parameters 42 | function [a0, a1, a2, a3] = cubic(obj, q0, q1, v0, v1, t0, t1) 43 | if abs(t0 - t1) < 1e-6 44 | error('t0 and t1 must be different'); 45 | end 46 | 47 | T = t1 - t0; 48 | h = q1 - q0; 49 | 50 | a0 = q0; 51 | a1 = v0; 52 | a2 = (3*h - (2*v0 + v1)*T) / (T^2); 53 | a3 = (-2*h + (v0 + v1)*T) / (T^3); 54 | end 55 | 56 | % cubic interpolation for all data points 57 | % t: time 58 | % q: 1 x 3 array, output of the interpolation at time t 59 | function q = getPosition(obj, t) 60 | if (t < obj.t_via(1)) || (t > obj.t_via(end)) 61 | error('The specific time error, time ranges error'); 62 | end 63 | 64 | j = find(obj.t_via >= t, 1, 'first'); % find the index of t1 65 | if j == 1 66 | i = 1; 67 | j = 2; 68 | else 69 | i = j-1; 70 | end 71 | 72 | % get given position 73 | q0 = obj.q_via(i, 1); 74 | v0 = obj.q_via(i, 2); 75 | t0 = obj.t_via(i); 76 | 77 | q1 = obj.q_via(j, 1); 78 | v1 = obj.q_via(j, 2); 79 | t1 = obj.t_via(j); 80 | 81 | [a0, a1, a2, a3] = obj.cubic(q0, q1, v0, v1, t0, t1); 82 | 83 | q(1, 1) = a0 + a1*(t - t0) + a2*(t-t0)^2 + a3*(t - t0)^3; % position 84 | q(1, 2) = a1 + 2*a2*(t - t0) + 3*a3*(t - t0)^2; % velocity 85 | q(1, 3) = 2*a2 + 6*a3*(t - t0); % acceleration 86 | end 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /matlab/LinearInterpolation.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Linear Interpolation 3 | % *************************************************************************** 4 | % Author: Chaobin 5 | % Email: chaubyZou@163.com 6 | % Date: October 2020 7 | % *************************************************************************** 8 | % Language: Matlab 9 | % Also available in: Python 10 | % Required library: None 11 | % *************************************************************************** 12 | 13 | classdef LinearInterpolation < handle 14 | properties 15 | name = 'linear interpolation'; 16 | q_via = []; 17 | t_via = []; 18 | end 19 | 20 | methods 21 | % crate the objective 22 | % name: string 23 | % q_via: N x 3 array 24 | % t_via: N x 1 array 25 | function obj = LinearInterpolation(name, q_via, t_via) 26 | obj.name = name; 27 | obj.q_via = q_via; 28 | obj.t_via = t_via; 29 | if size(q_via(:,1)) ~= length(t_via) 30 | error('The q_via and t_via must have a same length'); 31 | end 32 | end 33 | 34 | % linar interpolation with two data points 35 | % q0: the first data point 36 | % q1: the second data point 37 | % t0: the time of the first data point 38 | % t1: the time of the second data point 39 | % a0, a1: parameters 40 | function [a0, a1] = linear(obj, q0, q1, t0, t1) 41 | if abs(t0 - t1) < 1e-6 42 | error('t0 and t1 must be different'); 43 | end 44 | a0 = q0; 45 | a1 = (q1 - q0)/(t1 - t0); 46 | end 47 | 48 | % linar interpolation for all data points 49 | % t: specified time 50 | % q: 1 x 3 array, output of the interpolation at time t 51 | function q = getPosition(obj, t) 52 | if (t < obj.t_via(1)) || (t > obj.t_via(end)) 53 | error('The specific time error, time ranges error'); 54 | end 55 | 56 | j = find(obj.t_via >= t, 1, 'first'); % find the index of t1 57 | if j == 1 58 | i = 1; 59 | j = 2; 60 | else 61 | i = j-1; 62 | end 63 | 64 | % position 65 | q0 = obj.q_via(i,1); 66 | t0 = obj.t_via(i); 67 | q1 = obj.q_via(j,1); 68 | t1 = obj.t_via(j); 69 | [a0, a1] = obj.linear(q0, q1, t0, t1); 70 | q(1, 1) = a0 + a1*(t - t0); 71 | 72 | % velocity 73 | q(1, 2) = a1; 74 | 75 | % acceleration 76 | q(1, 3) = 0; % for linear model, the acceleration is infinite, here we set to zero 77 | end 78 | end 79 | end 80 | -------------------------------------------------------------------------------- /matlab/ParabolicInterpolation.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Parabolic Interpolation 3 | % *************************************************************************** 4 | % Author: Chaobin 5 | % Email: chaubyZou@163.com 6 | % Date: October 2020 7 | % *************************************************************************** 8 | % Language: Matlab 9 | % Also available in: Python 10 | % Required library: None 11 | % *************************************************************************** 12 | 13 | classdef ParabolicInterpolation < handle 14 | properties 15 | name = 'parabolic interpolation'; 16 | q_via = []; 17 | t_via = []; 18 | end 19 | 20 | methods 21 | % crate the objective 22 | % name: string 23 | % q_via: N x 3 array 24 | % t_via: N x 1 array 25 | function obj = ParabolicInterpolation(name, q_via, t_via) 26 | obj.name = name; 27 | obj.q_via = q_via; 28 | obj.t_via = t_via; 29 | if length(q_via(:,1)) ~= length(t_via) 30 | error('The q_via and t_via must have a same length'); 31 | end 32 | end 33 | 34 | % parabolic interpolation with two data points 35 | % q0: the first data point 36 | % q1: the second data point 37 | % v0: the velocity of the first data point 38 | % v1: the velocity of the second data point 39 | % t0: the time of the first data point 40 | % t1: the time of the second data point 41 | % tf: the time of the flex point 42 | % qf: the position of the flex point 43 | % a0~a5: parameters 44 | function [a0, a1, a2, a3, a4, a5] = parabolic(obj, q0, q1, v0, v1, t0, t1, tf, qf) 45 | if abs(t0 - t1) < 1e-6 46 | error('t0 and t1 must be different'); 47 | end 48 | 49 | if ((tf <= t0) || (tf >= t1)) 50 | error('tf must satisfy t0 < tf < t1'); 51 | end 52 | 53 | if ((qf <= min(q0, q1)) || (qf >= max(q0, q1))) 54 | error('qf must satisfy min(q0, q1) < qf < max(q0, q1)'); 55 | end 56 | 57 | T = t1 - t0; 58 | h = q1 - q0; 59 | Ta = tf - t0; 60 | Td = t1 - tf; 61 | 62 | a0 = q0; 63 | a1 = v0; 64 | a2 = (2*h - v0*(T + Ta) - v1*Td)/(2*T*Ta); 65 | a3 = (2*q1*Ta + Td*(2*q0 + Ta*(v0 - v1)))/(2*T); 66 | a4 = (2*h - v0*Ta - v1*Td)/T; 67 | a5 = -(2*h - v0*Ta - v1*(T+Td))/(2*T*Td); 68 | end 69 | 70 | % parabolic interpolation for all data points 71 | % t: time 72 | % q: 1 x 3 array, output of the interpolation at time t 73 | function q = getPosition(obj, t) 74 | if (t < obj.t_via(1)) || (t > obj.t_via(end)) 75 | error('The specific time error, time ranges error'); 76 | end 77 | 78 | j = find(obj.t_via >= t, 1, 'first'); % find the index of t1 79 | if j == 1 80 | i = 1; 81 | j = 2; 82 | else 83 | i = j-1; 84 | end 85 | 86 | % get given position 87 | q0 = obj.q_via(i, 1); 88 | v0 = obj.q_via(i, 2); 89 | t0 = obj.t_via(i); 90 | 91 | q1 = obj.q_via(j, 1); 92 | v1 = obj.q_via(j, 2); 93 | t1 = obj.t_via(j); 94 | 95 | % symmetric acceleration 96 | tf = (t0 + t1)/2; 97 | qf = (q0 + q1)/2; 98 | 99 | % asymmetric acceleration, specify tf and qf by users 100 | % tf = ? 101 | % qf = ? 102 | 103 | [a0, a1, a2, a3, a4, a5] = obj.parabolic(q0, q1, v0, v1, t0, t1, tf, qf); 104 | 105 | if t <= tf 106 | q(1, 1) = a0 + a1*(t - t0) + a2*(t-t0)^2; 107 | q(1, 2) = a1 + 2*a2*(t - t0); 108 | q(1, 3) = 2*a2; 109 | else 110 | q(1, 1) = a3 + a4*(t - tf) + a5*(t-tf)^2; 111 | q(1, 2) = a4 + 2*a5*(t - tf); 112 | q(1, 3) = 2*a5; 113 | end 114 | end 115 | end 116 | end 117 | -------------------------------------------------------------------------------- /matlab/Polynomial5Interpolation.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Polynomial of Degree Five Interpolation 3 | % *************************************************************************** 4 | % Author: Chaobin 5 | % Email: chaubyZou@163.com 6 | % Date: October 2020 7 | % *************************************************************************** 8 | % Language: Matlab 9 | % Also available in: Python 10 | % Required library: None 11 | % *************************************************************************** 12 | 13 | classdef Polynomial5Interpolation < handle 14 | properties 15 | name = 'cubic interpolation'; 16 | q_via = []; 17 | t_via = []; 18 | end 19 | 20 | methods 21 | % crate the objective 22 | % name: string 23 | % q_via: N x 3 array 24 | % t_via: N x 1 array 25 | function obj = Polynomial5Interpolation(name, q_via, t_via) 26 | obj.name = name; 27 | obj.q_via = q_via; 28 | obj.t_via = t_via; 29 | if length(q_via(:,1)) ~= length(t_via) 30 | error('The q_via and t_via must have a same length'); 31 | end 32 | end 33 | 34 | % polynomial of degree 5 interpolation with two data points 35 | % q0: the first data point 36 | % q1: the second data point 37 | % v0: the velocity of the first data point 38 | % v1: the velocity of the second data point 39 | % acc0: the acceleration of the first data point 40 | % acc1: the acceleration of the second data point 41 | % t0: the time of the first data point 42 | % t1: the time of the second data point 43 | % a0~a5: parameters 44 | function [a0, a1, a2, a3, a4, a5] = polynomial(obj, q0, q1, v0, v1, acc0, acc1, t0, t1) 45 | if abs(t0 - t1) < 1e-6 46 | error('t0 and t1 must be different'); 47 | end 48 | 49 | T = t1 - t0; 50 | h = q1 - q0; 51 | 52 | a0 = q0; 53 | a1 = v0; 54 | a2 = acc0/2; 55 | a3 = (20*h - (8*v1 + 12*v0)*T - (3*acc0 - acc1)*T^2) / (2*T^3); 56 | a4 = (-30*h + (14*v1 + 16*v0)*T + (3*acc0 - 2*acc1)*T^2) / (2*T^4); 57 | a5 = (12*h - 6*(v1 + v0)*T + (acc1 - acc0)*T^2) / (2*T^5); 58 | end 59 | 60 | % polynomial of degree 5 interpolation for all data points 61 | % t: time 62 | % q: 1 x 3 array, output of the interpolation at time t 63 | function q = getPosition(obj, t) 64 | if (t < obj.t_via(1)) || (t > obj.t_via(end)) 65 | error('The specific time error, time ranges error'); 66 | end 67 | 68 | j = find(obj.t_via >= t, 1, 'first'); % find the index of t1 69 | if j == 1 70 | i = 1; 71 | j = 2; 72 | else 73 | i = j-1; 74 | end 75 | 76 | % get given position 77 | q0 = obj.q_via(i, 1); 78 | v0 = obj.q_via(i, 2); 79 | acc0 = obj.q_via(i, 3); 80 | t0 = obj.t_via(i); 81 | 82 | q1 = obj.q_via(j, 1); 83 | v1 = obj.q_via(j, 2); 84 | acc1 = obj.q_via(j, 3); 85 | t1 = obj.t_via(j); 86 | 87 | [a0, a1, a2, a3, a4, a5] = obj.polynomial(q0, q1, v0, v1, acc0, acc1, t0, t1); 88 | 89 | q(1, 1) = a0 + a1*(t - t0) + a2*(t-t0)^2 + a3*(t - t0)^3 + a4*(t - t0)^4 + a5*(t - t0)^5; % position 90 | q(1, 2) = a1 + 2*a2*(t - t0) + 3*a3*(t - t0)^2 + 4*a4*(t - t0)^3 + 5*a5*(t - t0)^4; % velocity 91 | q(1, 3) = 2*a2 + 6*a3*(t - t0) + 12*a4*(t- t0)^2 + 20*a5*(t - t0)^3; % acceleration 92 | end 93 | end 94 | end 95 | -------------------------------------------------------------------------------- /matlab/main.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Polynomial Interpolation 3 | % Code Library 4 | % *************************************************************************** 5 | % Author: Chaobin 6 | % Email: chaubyZou@163.com 7 | % Date: October 2020 8 | % *************************************************************************** 9 | % Language: Matlab 10 | % Also available in: Python 11 | % Required library: None 12 | % *************************************************************************** 13 | 14 | clear; 15 | close all; 16 | 17 | %% Specify the data point 18 | % q_given = [0, 1.6, 3.2, 2, 4, 0.2, 1.2; % position 19 | % 0, 0, 0, 0, 0, 0, 0; % zero velocity 20 | % 0, 0, 0, 0, 0, 0, 0]'; % zero acceleration 21 | 22 | q_given = [0, 1.6, 3.2, 2, 4, 0.2, 1.2; % position 23 | 0, 1.0, 2.0, -2.0, -1.0, 0, 0; % varying velocity 24 | 0, 1, 2, 3, 2, 1, 0]'; % varying acceleration 25 | 26 | t_given = [0, 1, 3, 4.5, 6, 8, 10]'; % time 27 | 28 | % time for interpolation 29 | t = t_given(1):0.01:t_given(end); 30 | 31 | 32 | %% ************************ Linear interpolation ******************************* 33 | linear_interpolation = LinearInterpolation('Linear', q_given, t_given); 34 | linear_trajectory = zeros(length(t), 3); % N x 3 array: position, velocity, acceleration 35 | 36 | for i = 1:length(t) 37 | linear_trajectory(i,:) = linear_interpolation.getPosition(t(i)); 38 | end 39 | 40 | figure('position', [200, 200, 800, 600]) 41 | subplot(3,1,1) 42 | hold on 43 | plot(t_given, q_given(:, 1), 'ro') 44 | plot(t, linear_trajectory(:,1), 'k') 45 | hold off 46 | grid on 47 | title('Linear interpolation') 48 | xlabel('time') 49 | ylabel('position') 50 | xlim([t_given(1)-1, t_given(end)+1]) 51 | ylim([min(q_given(:,1)) - 1, max(q_given(:,1)) + 1]) 52 | 53 | subplot(3,1,2) 54 | hold on 55 | plot(t_given, q_given(:, 2), 'ro') 56 | plot(t, linear_trajectory(:,2), 'k') 57 | hold off 58 | grid on 59 | xlabel('time') 60 | ylabel('velocity') 61 | xlim([t_given(1)-1, t_given(end)+1]) 62 | 63 | subplot(3,1,3) 64 | hold on 65 | plot(t_given, q_given(:, 3), 'ro') 66 | plot(t, linear_trajectory(:,3), 'k') 67 | hold off 68 | grid on 69 | xlabel('time') 70 | ylabel('acceleration') 71 | xlim([t_given(1)-1, t_given(end)+1]) 72 | 73 | 74 | %% *********************** Parabolic interpolation ***************************** 75 | parabolic_interpolation = ParabolicInterpolation('Parabolic', q_given, t_given); 76 | parabolic_trajectory = zeros(length(t), 3); % N x 3 array: position, velocity, acceleration 77 | 78 | for i = 1:length(t) 79 | parabolic_trajectory(i,:) = parabolic_interpolation.getPosition(t(i)); 80 | end 81 | 82 | figure('position', [230, 200, 800, 600]) 83 | subplot(3,1,1) 84 | hold on 85 | plot(t_given, q_given(:, 1), 'ro') 86 | plot(t, parabolic_trajectory(:,1), 'k') 87 | hold off 88 | grid on 89 | title('Parabolic interpolation') 90 | xlabel('time') 91 | ylabel('position') 92 | xlim([t_given(1)-1, t_given(end)+1]) 93 | ylim([min(q_given(:,1)) - 1, max(q_given(:,1)) + 1]) 94 | 95 | subplot(3,1,2) 96 | hold on 97 | plot(t_given, q_given(:, 2), 'ro') 98 | plot(t, parabolic_trajectory(:,2), 'k') 99 | hold off 100 | grid on 101 | xlabel('time') 102 | ylabel('velocity') 103 | xlim([t_given(1)-1, t_given(end)+1]) 104 | 105 | subplot(3,1,3) 106 | hold on 107 | plot(t_given, q_given(:, 3), 'ro') 108 | plot(t, parabolic_trajectory(:,3), 'k') 109 | hold off 110 | grid on 111 | xlabel('time') 112 | ylabel('acceleration') 113 | xlim([t_given(1)-1, t_given(end)+1]) 114 | 115 | 116 | %% ************************ Cubic interpolation ****************************** 117 | cubic_interpolation = CubicInterpolation('Cubic', q_given, t_given); 118 | cubic_trajectory = zeros(length(t), 3); % N x 3 array: position, velocity, acceleration 119 | 120 | for i = 1:length(t) 121 | cubic_trajectory(i,:) = cubic_interpolation.getPosition(t(i)); 122 | end 123 | 124 | figure('position', [260, 200, 800, 600]) 125 | subplot(3,1,1) 126 | hold on 127 | plot(t_given, q_given(:, 1), 'ro') 128 | plot(t, cubic_trajectory(:,1), 'k') 129 | hold off 130 | grid on 131 | title('Cubic interpolation') 132 | xlabel('time') 133 | ylabel('position') 134 | xlim([t_given(1)-1, t_given(end)+1]) 135 | ylim([min(q_given(:,1)) - 1, max(q_given(:,1)) + 1]) 136 | 137 | subplot(3,1,2) 138 | hold on 139 | plot(t_given, q_given(:, 2), 'ro') 140 | plot(t, cubic_trajectory(:,2), 'k') 141 | hold off 142 | grid on 143 | xlabel('time') 144 | ylabel('velocity') 145 | xlim([t_given(1)-1, t_given(end)+1]) 146 | 147 | subplot(3,1,3) 148 | hold on 149 | plot(t_given, q_given(:, 3), 'ro') 150 | plot(t, cubic_trajectory(:,3), 'k') 151 | hold off 152 | grid on 153 | xlabel('time') 154 | ylabel('acceleration') 155 | xlim([t_given(1)-1, t_given(end)+1]) 156 | 157 | 158 | %% *********************** Polynomial 5 interpolation **************************** 159 | polynomial5_interpolation = Polynomial5Interpolation('Polynomial 5', q_given, t_given); 160 | polynomial5_trajectory = zeros(length(t), 3); % N x 3 array: position, velocity, acceleration 161 | 162 | for i = 1:length(t) 163 | polynomial5_trajectory(i,:) = polynomial5_interpolation.getPosition(t(i)); 164 | end 165 | 166 | figure('position', [290, 200, 800, 600]) 167 | subplot(3,1,1) 168 | hold on 169 | plot(t_given, q_given(:, 1), 'ro') 170 | plot(t, polynomial5_trajectory(:,1), 'k') 171 | hold off 172 | grid on 173 | title('Polynomial of degree 5 interpolation') 174 | xlabel('time') 175 | ylabel('position') 176 | xlim([t_given(1)-1, t_given(end)+1]) 177 | ylim([min(q_given(:,1)) - 1, max(q_given(:,1)) + 1]) 178 | 179 | subplot(3,1,2) 180 | hold on 181 | plot(t_given, q_given(:, 2), 'ro') 182 | plot(t, polynomial5_trajectory(:,2), 'k') 183 | hold off 184 | grid on 185 | xlabel('time') 186 | ylabel('velocity') 187 | xlim([t_given(1)-1, t_given(end)+1]) 188 | 189 | subplot(3,1,3) 190 | hold on 191 | plot(t_given, q_given(:, 3), 'ro') 192 | plot(t, polynomial5_trajectory(:,3), 'k') 193 | hold off 194 | grid on 195 | xlabel('time') 196 | ylabel('acceleration') 197 | xlim([t_given(1)-1, t_given(end)+1]) 198 | 199 | 200 | %% ***************************** Comparison *********************************** 201 | figure('position', [350, 200, 800, 600]) 202 | subplot(3,1,1) 203 | hold on 204 | plot(t_given, q_given(:, 1), 'ro') 205 | plot(t, linear_trajectory(:,1), 'k') 206 | plot(t, parabolic_trajectory(:,1), 'b') 207 | plot(t, cubic_trajectory(:,1), 'g') 208 | plot(t, polynomial5_trajectory(:,1), 'm') 209 | hold off 210 | grid on 211 | title('Comparison') 212 | xlabel('time') 213 | ylabel('position') 214 | legend('given pos', 'linear', 'parabolic', 'cubic', 'poly 5') 215 | xlim([t_given(1)-1, t_given(end)+3]) 216 | ylim([min(q_given(:,1)) - 1, max(q_given(:,1)) + 1]) 217 | 218 | subplot(3,1,2) 219 | hold on 220 | plot(t_given, q_given(:,2), 'ro') 221 | plot(t, linear_trajectory(:,2), 'k') 222 | plot(t, parabolic_trajectory(:,2), 'b') 223 | plot(t, cubic_trajectory(:,2), 'g') 224 | plot(t, polynomial5_trajectory(:,2), 'm') 225 | hold off 226 | grid on 227 | xlabel('time') 228 | ylabel('velocity') 229 | legend('given vel', 'linear', 'parabolic', 'cubic', 'poly 5') 230 | xlim([t_given(1)-1, t_given(end)+3]) 231 | 232 | subplot(3,1,3) 233 | hold on 234 | plot(t_given, q_given(:,3), 'ro') 235 | plot(t, linear_trajectory(:,3), 'k') 236 | plot(t, parabolic_trajectory(:,3), 'b') 237 | plot(t, cubic_trajectory(:,3), 'g') 238 | plot(t, polynomial5_trajectory(:,3), 'm') 239 | hold off 240 | grid on 241 | xlabel('time') 242 | ylabel('acceleration') 243 | legend('given acc', 'linear', 'parabolic', 'cubic', 'poly 5') 244 | xlim([t_given(1)-1, t_given(end)+3]) 245 | -------------------------------------------------------------------------------- /matlab/problem_def.m: -------------------------------------------------------------------------------- 1 | % *************************************************************************** 2 | % Problem definition 3 | % *************************************************************************** 4 | % Author: Chaobin 5 | % Email: chaubyZou@163.com 6 | % Date: October 2020 7 | % *************************************************************************** 8 | % Language: Matlab 9 | % Also available in: Python 10 | % Required library: None 11 | % *************************************************************************** 12 | 13 | clear; 14 | 15 | q = [0, 1.6, 2.5, 2, 4, 1.5, 0]'; 16 | t = [0, 1, 3, 4.5, 6, 8, 10]'; 17 | 18 | figure() 19 | plot(t, q, 'ro') 20 | xlabel('time') 21 | ylabel('position') 22 | xlim([-1, 11]) 23 | ylim([-1, 5]) 24 | -------------------------------------------------------------------------------- /python/PolynomialInterpolation.py: -------------------------------------------------------------------------------- 1 | #-*-coding:UTF-8 -*- 2 | # *************************************************************************** 3 | # Polynomial Interpolation 4 | # Code Library 5 | # *************************************************************************** 6 | # Author: Chaobin 7 | # Email: chaubyZou@163.com 8 | # Date: October 2020 9 | # *************************************************************************** 10 | # Language: Python 11 | # Also available in: Matlab 12 | # Required library: Numpy, Matplotlib 13 | # *************************************************************************** 14 | 15 | import numpy as np 16 | import matplotlib.pyplot as plt 17 | 18 | class LinearInterpolation(): 19 | def __init__(self, name='Linear', q_via=None, t_via=None): 20 | """ 21 | :param: name: string 22 | name of objective 23 | :param: q_via: N x 3 array 24 | given q array 25 | :param: t_via: N x 1 array 26 | given t array 27 | """ 28 | super(self.__class__, self).__init__() 29 | self.name = name 30 | self.q_via = q_via 31 | self.t_via = t_via 32 | 33 | try: 34 | q_via.shape[1] != t_via.shape[0] 35 | except ValueError: 36 | print('The q_via and t_via must have a same length') 37 | 38 | def linear(self, q0, q1, t0, t1): 39 | """ 40 | :param: q0: float 41 | the first data point 42 | :param: q1: float 43 | the second data point 44 | :param: t0: float 45 | the time of the first data point 46 | :param: t1: float 47 | the time of the second data point 48 | """ 49 | try: 50 | abs(t0 - t1) < 1e-6 51 | except ValueError: 52 | print('t0 and t1 must be different') 53 | 54 | a0 = q0 55 | a1 = (q1 - q0)/(t1 - t0) 56 | return a0, a1 57 | 58 | def getPosition(self, t): 59 | """ 60 | :param: t: float 61 | specified time 62 | :return: q: float 63 | output of the interpolation at time t 64 | """ 65 | try: 66 | (t < self.t_via[0]) or (t > self.t_via[-1]) 67 | except ValueError: 68 | print('The specific time error, time ranges error') 69 | 70 | j_array = np.where(self.t_via >= t) # find the index of t1 71 | j = j_array[0][0] 72 | if j == 0: 73 | i = 0 74 | j = 1 75 | else: 76 | i = j-1 77 | 78 | q = np.zeros((1, 3)) 79 | 80 | # position 81 | q0 = self.q_via[i,0] 82 | t0 = self.t_via[i] 83 | q1 = self.q_via[j,0] 84 | t1 = self.t_via[j] 85 | a0, a1 = self.linear(q0, q1, t0, t1) 86 | q[0, 0] = a0 + a1*(t - t0) 87 | 88 | # velocity 89 | q[0, 1] = a1 90 | 91 | # acceleration 92 | q[0, 2] = 0 # for linear model, the acceleration is infinite, here we set to zero 93 | return q 94 | 95 | 96 | class ParabolicInterpolation(): 97 | def __init__(self, name='Parabolic', q_via=None, t_via=None): 98 | """ 99 | :param: name: string 100 | name of objective 101 | :param: q_via: N x 3 array 102 | given q array 103 | :param: t_via: N x 1 array 104 | given t array 105 | """ 106 | super(self.__class__, self).__init__() 107 | self.name = name 108 | self.q_via = q_via 109 | self.t_via = t_via 110 | 111 | try: 112 | q_via.shape[1] != t_via.shape[0] 113 | except ValueError: 114 | print('The q_via and t_via must have a same length') 115 | 116 | def parabolic(self, q0, q1, v0, v1, t0, t1, tf, qf): 117 | """ 118 | :param: q0: float 119 | the first data point 120 | :param: q1: float 121 | the second data point 122 | :param: v0: float 123 | the velocity of the first data point 124 | :param: v1: float 125 | the velocity of the second data point 126 | :param: t0: float 127 | the time of the first data point 128 | :param: t1: float 129 | the time of the second data point 130 | :param: tf: float 131 | the time of the flex point 132 | :param: qf: float 133 | the position of the flex point 134 | """ 135 | 136 | try: 137 | abs(t0 - t1) < 1e-6 138 | except ValueError: 139 | print('t0 and t1 must be different') 140 | 141 | try: 142 | ((tf <= t0) or (tf >= t1)) 143 | except ValueError: 144 | print('tf must satisfy t0 < tf < t1') 145 | 146 | try: 147 | ((qf <= min(q0, q1)) or (qf >= max(q0, q1))) 148 | except ValueError: 149 | print('qf must satisfy min(q0, q1) < qf < max(q0, q1)') 150 | 151 | T = t1 - t0 152 | h = q1 - q0 153 | Ta = tf - t0 154 | Td = t1 - tf 155 | 156 | a0 = q0 157 | a1 = v0 158 | a2 = (2*h - v0*(T + Ta) - v1*Td)/(2*T*Ta) 159 | a3 = (2*q1*Ta + Td*(2*q0 + Ta*(v0 - v1)))/(2*T) 160 | a4 = (2*h - v0*Ta - v1*Td)/T 161 | a5 = -(2*h - v0*Ta - v1*(T+Td))/(2*T*Td) 162 | return a0, a1, a2, a3, a4, a5 163 | 164 | def getPosition(self, t): 165 | """ 166 | :param: t: float 167 | specified time 168 | :return: q: float 169 | output of the interpolation at time t 170 | """ 171 | try: 172 | (t < self.t_via[0]) or (t > self.t_via[-1]) 173 | except ValueError: 174 | print('The specific time error, time ranges error') 175 | 176 | j_array = np.where(self.t_via >= t) # find the index of t1 177 | j = j_array[0][0] 178 | if j == 0: 179 | i = 0 180 | j = 1 181 | else: 182 | i = j-1 183 | 184 | q = np.zeros((1, 3)) 185 | 186 | # get given position 187 | q0 = self.q_via[i,0] 188 | v0 = self.q_via[i,1] 189 | t0 = self.t_via[i] 190 | 191 | q1 = self.q_via[j,0] 192 | v1 = self.q_via[j,1] 193 | t1 = self.t_via[j] 194 | 195 | # symmetric acceleration 196 | tf = (t0 + t1)/2 197 | qf = (q0 + q1)/2 198 | 199 | # asymmetric acceleration, specify tf and qf by users 200 | # tf = ? 201 | # qf = ? 202 | 203 | a0, a1, a2, a3, a4, a5 = self.parabolic(q0, q1, v0, v1, t0, t1, tf, qf) 204 | 205 | if t <= tf: 206 | q[0, 0] = a0 + a1*(t - t0) + a2*(t-t0)**2 207 | q[0, 1] = a1 + 2*a2*(t - t0) 208 | q[0, 2] = 2*a2 209 | else: 210 | q[0, 0] = a3 + a4*(t - tf) + a5*(t-tf)**2 211 | q[0, 1] = a4 + 2*a5*(t - tf) 212 | q[0, 2] = 2*a5 213 | 214 | return q 215 | 216 | 217 | class CubicInterpolation(): 218 | def __init__(self, name='Cubic', q_via=None, t_via=None): 219 | """ 220 | :param: name: string 221 | name of objective 222 | :param: q_via: N x 3 array 223 | given q array 224 | :param: t_via: N x 1 array 225 | given t array 226 | """ 227 | super(self.__class__, self).__init__() 228 | self.name = name 229 | self.q_via = q_via 230 | self.t_via = t_via 231 | 232 | try: 233 | q_via.shape[1] != t_via.shape[0] 234 | except ValueError: 235 | print('The q_via and t_via must have a same length') 236 | 237 | def cubic(self, q0, q1, v0, v1, t0, t1): 238 | """ 239 | :param: q0: float 240 | the first data point 241 | :param: q1: float 242 | the second data point 243 | :param: v0: float 244 | the velocity of the first data point 245 | :param: v1: float 246 | the velocity of the second data point 247 | :param: t0: float 248 | the time of the first data point 249 | :param: t1: float 250 | the time of the second data point 251 | """ 252 | try: 253 | abs(t0 - t1) < 1e-6 254 | except ValueError: 255 | print('t0 and t1 must be different') 256 | 257 | 258 | T = t1 - t0 259 | h = q1 - q0 260 | 261 | a0 = q0 262 | a1 = v0 263 | a2 = (3*h - (2*v0 + v1)*T) / (T**2) 264 | a3 = (-2*h + (v0 + v1)*T) / (T**3) 265 | return a0, a1, a2, a3 266 | 267 | def getPosition(self, t): 268 | """ 269 | :param: t: float 270 | specified time 271 | :return: q: float 272 | output of the interpolation at time t 273 | """ 274 | try: 275 | (t < self.t_via[0]) or (t > self.t_via[-1]) 276 | except ValueError: 277 | print('The specific time error, time ranges error') 278 | 279 | j_array = np.where(self.t_via >= t) # find the index of t1 280 | j = j_array[0][0] 281 | if j == 0: 282 | i = 0 283 | j = 1 284 | else: 285 | i = j-1 286 | 287 | q = np.zeros((1, 3)) 288 | 289 | # get given position 290 | q0 = self.q_via[i,0] 291 | v0 = self.q_via[i,1] 292 | t0 = self.t_via[i] 293 | 294 | q1 = self.q_via[j,0] 295 | v1 = self.q_via[j,1] 296 | t1 = self.t_via[j] 297 | 298 | a0, a1, a2, a3 = self.cubic(q0, q1, v0, v1, t0, t1) 299 | 300 | q[0, 0] = a0 + a1*(t - t0) + a2*(t-t0)**2 + a3*(t - t0)**3 # position 301 | q[0, 1] = a1 + 2*a2*(t - t0) + 3*a3*(t - t0)**2 # velocity 302 | q[0, 2] = 2*a2 + 6*a3*(t - t0) # acceleration 303 | 304 | return q 305 | 306 | 307 | class Polynomial5Interpolation(): 308 | def __init__(self, name='Polynomial 5', q_via=None, t_via=None): 309 | """ 310 | :param: name: string 311 | name of objective 312 | :param: q_via: N x 3 array 313 | given q array 314 | :param: t_via: N x 1 array 315 | given t array 316 | """ 317 | super(self.__class__, self).__init__() 318 | self.name = name 319 | self.q_via = q_via 320 | self.t_via = t_via 321 | 322 | try: 323 | q_via.shape[1] != t_via.shape[0] 324 | except ValueError: 325 | print('The q_via and t_via must have a same length') 326 | 327 | def polynomial(self, q0, q1, v0, v1, acc0, acc1, t0, t1): 328 | """ 329 | :param: q0: float 330 | the first data point 331 | :param: q1: float 332 | the second data point 333 | :param: v0: float 334 | the velocity of the first data point 335 | :param: v1: float 336 | the velocity of the second data point 337 | :param: acc0: float 338 | the acceleration of the first data point 339 | :param: acc1: float 340 | the acceleration of the second data point 341 | :param: t0: float 342 | the time of the first data point 343 | :param: t1: float 344 | the time of the second data point 345 | """ 346 | try: 347 | abs(t0 - t1) < 1e-6 348 | except ValueError: 349 | print('t0 and t1 must be different') 350 | 351 | 352 | T = t1 - t0 353 | h = q1 - q0 354 | 355 | a0 = q0 356 | a1 = v0 357 | a2 = acc0/2 358 | a3 = (20*h - (8*v1 + 12*v0)*T - (3*acc0 - acc1)*T**2) / (2*T**3) 359 | a4 = (-30*h + (14*v1 + 16*v0)*T + (3*acc0 - 2*acc1)*T**2) / (2*T**4) 360 | a5 = (12*h - 6*(v1 + v0)*T + (acc1 - acc0)*T**2) / (2*T**5) 361 | return a0, a1, a2, a3, a4, a5 362 | 363 | def getPosition(self, t): 364 | """ 365 | :param: t: float 366 | specified time 367 | :return: q: float 368 | output of the interpolation at time t 369 | """ 370 | try: 371 | (t < self.t_via[0]) or (t > self.t_via[-1]) 372 | except ValueError: 373 | print('The specific time error, time ranges error') 374 | 375 | j_array = np.where(self.t_via >= t) # find the index of t1 376 | j = j_array[0][0] 377 | if j == 0: 378 | i = 0 379 | j = 1 380 | else: 381 | i = j-1 382 | 383 | q = np.zeros((1, 3)) 384 | 385 | # get given position 386 | q0 = self.q_via[i,0] 387 | v0 = self.q_via[i,1] 388 | acc0 = self.q_via[i,2] 389 | t0 = self.t_via[i] 390 | 391 | q1 = self.q_via[j,0] 392 | v1 = self.q_via[j,1] 393 | acc1 = self.q_via[j,2] 394 | t1 = self.t_via[j] 395 | 396 | a0, a1, a2, a3, a4, a5 = self.polynomial(q0, q1, v0, v1, acc0, acc1, t0, t1) 397 | 398 | q[0, 0] = a0 + a1*(t - t0) + a2*(t-t0)**2 + a3*(t - t0)**3 + a4*(t - t0)**4 + a5*(t - t0)**5 # position 399 | q[0, 1] = a1 + 2*a2*(t - t0) + 3*a3*(t - t0)**2 + 4*a4*(t - t0)**3 + 5*a5*(t - t0)**4 # velocity 400 | q[0, 2] = 2*a2 + 6*a3*(t - t0) + 12*a4*(t- t0)**2 + 20*a5*(t - t0)**3 # acceleration 401 | 402 | return q 403 | 404 | 405 | 406 | if __name__ == "__main__": 407 | q_given = np.array([[0, 1.6, 3.2, 2, 4, 0.2, 1.2], 408 | [0, 1.0, 2.0, -2.0, -1.0, 0, 0], 409 | [0, 1, 2, 3, 2, 1, 0]]).transpose() 410 | t_given = np.array([0, 1, 3, 4.5, 6, 8, 10]).transpose() 411 | 412 | # time for interpolation 413 | t = np.linspace(t_given[0], t_given[-1], 1000) 414 | 415 | #%% ************************ Linear interpolation ******************************* 416 | linear_interpolation = LinearInterpolation('Linear', q_given, t_given) 417 | linear_trajectory = np.zeros((t.shape[0], 3)) # N x 3 array: position, velocity, acceleration 418 | 419 | for i in range(t.shape[0]): 420 | linear_trajectory[i,:] = linear_interpolation.getPosition(t[i]) 421 | 422 | plt.figure(figsize=(8, 6)) 423 | plt.subplot(3,1,1) 424 | plt.plot(t_given, q_given[:, 0], 'ro') 425 | plt.plot(t, linear_trajectory[:,0], 'k') 426 | plt.grid('on') 427 | plt.title('Linear interpolation') 428 | plt.xlabel('time') 429 | plt.ylabel('position') 430 | plt.xlim(t_given[0]-1, t_given[-1]+1) 431 | plt.ylim(min(q_given[:,0]) - 1, max(q_given[:,0]) + 1) 432 | 433 | plt.subplot(3,1,2) 434 | plt.plot(t_given, q_given[:, 1], 'ro') 435 | plt.plot(t, linear_trajectory[:,1], 'k') 436 | plt.grid('on') 437 | plt.xlabel('time') 438 | plt.ylabel('velocity') 439 | plt.xlim(t_given[0]-1, t_given[-1]+1) 440 | 441 | plt.subplot(3,1,3) 442 | plt.plot(t_given, q_given[:, 2], 'ro') 443 | plt.plot(t, linear_trajectory[:,2], 'k') 444 | plt.grid('on') 445 | plt.xlabel('time') 446 | plt.ylabel('acceleration') 447 | plt.xlim(t_given[0]-1, t_given[-1]+1) 448 | 449 | 450 | #%% ************************ Parabolic interpolation ******************************* 451 | parabolic_interpolation = ParabolicInterpolation('Parabolic', q_given, t_given) 452 | parabolic_trajectory = np.zeros((t.shape[0], 3)) # N x 3 array: position, velocity, acceleration 453 | 454 | for i in range(t.shape[0]): 455 | parabolic_trajectory[i,:] = parabolic_interpolation.getPosition(t[i]) 456 | 457 | plt.figure(figsize=(8, 6)) 458 | plt.subplot(3,1,1) 459 | plt.plot(t_given, q_given[:, 0], 'ro') 460 | plt.plot(t, parabolic_trajectory[:,0], 'k') 461 | plt.grid('on') 462 | plt.title('Parabolic interpolation') 463 | plt.xlabel('time') 464 | plt.ylabel('position') 465 | plt.xlim(t_given[0]-1, t_given[-1]+1) 466 | plt.ylim(min(q_given[:,0]) - 1, max(q_given[:,0]) + 1) 467 | 468 | plt.subplot(3,1,2) 469 | plt.plot(t_given, q_given[:, 1], 'ro') 470 | plt.plot(t, parabolic_trajectory[:,1], 'k') 471 | plt.grid('on') 472 | plt.xlabel('time') 473 | plt.ylabel('velocity') 474 | plt.xlim(t_given[0]-1, t_given[-1]+1) 475 | 476 | plt.subplot(3,1,3) 477 | plt.plot(t_given, q_given[:, 2], 'ro') 478 | plt.plot(t, parabolic_trajectory[:,2], 'k') 479 | plt.grid('on') 480 | plt.xlabel('time') 481 | plt.ylabel('acceleration') 482 | plt.xlim(t_given[0]-1, t_given[-1]+1) 483 | 484 | 485 | #%% ************************ Cubic interpolation ******************************* 486 | cubic_interpolation = CubicInterpolation('Cubic', q_given, t_given) 487 | cubic_trajectory = np.zeros((t.shape[0], 3)) # N x 3 array: position, velocity, acceleration 488 | 489 | for i in range(t.shape[0]): 490 | cubic_trajectory[i,:] = cubic_interpolation.getPosition(t[i]) 491 | 492 | plt.figure(figsize=(8, 6)) 493 | plt.subplot(3,1,1) 494 | plt.plot(t_given, q_given[:, 0], 'ro') 495 | plt.plot(t, cubic_trajectory[:,0], 'k') 496 | plt.grid('on') 497 | plt.title('Cubic interpolation') 498 | plt.xlabel('time') 499 | plt.ylabel('position') 500 | plt.xlim(t_given[0]-1, t_given[-1]+1) 501 | plt.ylim(min(q_given[:,0]) - 1, max(q_given[:,0]) + 1) 502 | 503 | plt.subplot(3,1,2) 504 | plt.plot(t_given, q_given[:, 1], 'ro') 505 | plt.plot(t, cubic_trajectory[:,1], 'k') 506 | plt.grid('on') 507 | plt.xlabel('time') 508 | plt.ylabel('velocity') 509 | plt.xlim(t_given[0]-1, t_given[-1]+1) 510 | 511 | plt.subplot(3,1,3) 512 | plt.plot(t_given, q_given[:, 2], 'ro') 513 | plt.plot(t, cubic_trajectory[:,2], 'k') 514 | plt.grid('on') 515 | plt.xlabel('time') 516 | plt.ylabel('acceleration') 517 | plt.xlim(t_given[0]-1, t_given[-1]+1) 518 | 519 | 520 | #%% *************** Polynomial of degree five interpolation ********************* 521 | polynomial5_interpolation = Polynomial5Interpolation('Polynomial5', q_given, t_given) 522 | polynomial5_trajectory = np.zeros((t.shape[0], 3)) # N x 3 array: position, velocity, acceleration 523 | 524 | for i in range(t.shape[0]): 525 | polynomial5_trajectory[i,:] = polynomial5_interpolation.getPosition(t[i]) 526 | 527 | plt.figure(figsize=(8, 6)) 528 | plt.subplot(3,1,1) 529 | plt.plot(t_given, q_given[:, 0], 'ro') 530 | plt.plot(t, polynomial5_trajectory[:,0], 'k') 531 | plt.grid('on') 532 | plt.title('Polynomial of degree 5 interpolation') 533 | plt.xlabel('time') 534 | plt.ylabel('position') 535 | plt.xlim(t_given[0]-1, t_given[-1]+1) 536 | plt.ylim(min(q_given[:,0]) - 1, max(q_given[:,0]) + 1) 537 | 538 | plt.subplot(3,1,2) 539 | plt.plot(t_given, q_given[:, 1], 'ro') 540 | plt.plot(t, polynomial5_trajectory[:,1], 'k') 541 | plt.grid('on') 542 | plt.xlabel('time') 543 | plt.ylabel('velocity') 544 | plt.xlim(t_given[0]-1, t_given[-1]+1) 545 | 546 | plt.subplot(3,1,3) 547 | plt.plot(t_given, q_given[:, 2], 'ro') 548 | plt.plot(t, polynomial5_trajectory[:,2], 'k') 549 | plt.grid('on') 550 | plt.xlabel('time') 551 | plt.ylabel('acceleration') 552 | plt.xlim(t_given[0]-1, t_given[-1]+1) 553 | 554 | 555 | #%% ************************** Comparison *************************** 556 | plt.figure(figsize=(8, 6)) 557 | plt.subplot(3,1,1) 558 | plt.plot(t_given, q_given[:, 0], 'ro', label='given pos') 559 | plt.plot(t, linear_trajectory[:,0], 'k', label='linear') 560 | plt.plot(t, parabolic_trajectory[:,0], 'b', label='parabolic') 561 | plt.plot(t, cubic_trajectory[:,0], 'g', label='cubic') 562 | plt.plot(t, polynomial5_trajectory[:,0], 'm', label='poly 5') 563 | plt.grid('on') 564 | plt.legend(loc='upper right') 565 | plt.title('Comparison') 566 | plt.xlabel('time') 567 | plt.ylabel('position') 568 | plt.xlim(t_given[0]-1, t_given[-1]+3) 569 | plt.ylim(min(q_given[:,0]) - 1, max(q_given[:,0]) + 1) 570 | 571 | plt.subplot(3,1,2) 572 | plt.plot(t_given, q_given[:, 1], 'ro', label='given vel') 573 | plt.plot(t, linear_trajectory[:,1], 'k', label='linear') 574 | plt.plot(t, parabolic_trajectory[:,1], 'b', label='parabolic') 575 | plt.plot(t, cubic_trajectory[:,1], 'g', label='cubic') 576 | plt.plot(t, polynomial5_trajectory[:,1], 'm', label='poly 5') 577 | plt.grid('on') 578 | plt.legend(loc='upper right') 579 | plt.xlabel('time') 580 | plt.ylabel('velocity') 581 | plt.xlim(t_given[0]-1, t_given[-1]+3) 582 | 583 | plt.subplot(3,1,3) 584 | plt.plot(t_given, q_given[:, 2], 'ro', label='given acc') 585 | plt.plot(t, linear_trajectory[:,2], 'k', label='linear') 586 | plt.plot(t, parabolic_trajectory[:,2], 'b', label='parabolic') 587 | plt.plot(t, cubic_trajectory[:,2], 'g', label='cubic') 588 | plt.plot(t, polynomial5_trajectory[:,2], 'm', label='poly 5') 589 | plt.grid('on') 590 | plt.legend(loc='upper right') 591 | plt.xlabel('time') 592 | plt.ylabel('acceleration') 593 | plt.xlim(t_given[0]-1, t_given[-1]+3) 594 | 595 | 596 | plt.show() 597 | -------------------------------------------------------------------------------- /qrcode.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/qrcode.jpg -------------------------------------------------------------------------------- /qrcode.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/qrcode.png -------------------------------------------------------------------------------- /results/comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/results/comparison.png -------------------------------------------------------------------------------- /results/cubic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/results/cubic.png -------------------------------------------------------------------------------- /results/linear.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/results/linear.png -------------------------------------------------------------------------------- /results/parabolic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/results/parabolic.png -------------------------------------------------------------------------------- /results/poly_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chauby/PolynomialInterpolation/2e375b32f796fca8a60f91749e3a89848a5bccee/results/poly_5.png --------------------------------------------------------------------------------