利用matlab计算多项式样条曲线的末端状态与偏导数

摘要

利用3次或者5次样条曲线表示连接两个状态的运动；根据曲线参数p，可求得末端状态，也可求出末端状态相对于曲线参数p的偏导数。本文的公式可以用在优化算法中，用来求解符合两端状态约束的曲线方程。

三次样条曲线

三次样条曲线的m程序：

% 3_curvatrue.m
% Replace inv(A)*b with A\b
% Replace b*inv(A) with b/A
clear all;
clc;
format long;
syms p0 p1 p2 p3 sf s; % k0 k1 k2 kf sf;
% k(s) = a + b*s + c*s*s + d*s*s*s;
% [k0,k1,k2,kf] = A * [a,b,c,d]
A = [1,0,0,0; 
     1,sf/3,(sf/3)^2,(sf/3)^3;
     1,sf*2/3,(sf*2/3)^2,(sf*2/3)^3;
     1,sf,(sf^2),(sf^3)];
iA = inv(A);
p = [p0, p1, p2, p3].';
Coeff = A\p;
a = Coeff(1);
b = Coeff(2);
c = Coeff(3);
d = Coeff(4);

k_coeff = [1, s, s^2, s^3]/A;
ks = k_coeff*p;

theta_coeff = [s, s^2, s^3, s^4]/A;
thetas = theta_coeff*p;

% theta_coeff = zeros(8,4);
for k = 1:8    
    sk = sf*k/8;
    theta_coeff(k,:) = [sk, (sk^2)/2, (sk^3)/3, (sk^4)/4]/A;
end

theta_0 = 0;
theta_1 = theta_coeff(1,:)*p;
theta_2 = theta_coeff(2,:)*p;
theta_3 = theta_coeff(3,:)*p;
theta_4 = theta_coeff(4,:)*p;
theta_5 = theta_coeff(5,:)*p;
theta_6 = theta_coeff(6,:)*p;
theta_7 = theta_coeff(7,:)*p;
theta_8 = theta_coeff(8,:)*p;

% 末端状态
thetaf = simplify(theta_8);
xf = sf/24*(cos(theta_0) ...,
            + 4*(cos(theta_1) + cos(theta_3) + cos(theta_5) + cos(theta_7)) ...,
            + 2*(cos(theta_2) + cos(theta_4) + cos(theta_6)) ...,
            + cos(theta_8));
xf = simplify(xf);
yf = sf/24*(sin(theta_0) ...,
            + 4*(sin(theta_1) + sin(theta_3) + sin(theta_5) + sin(theta_7)) ...,
            + 2*(sin(theta_2) + sin(theta_4) + sin(theta_6)) ...,
            + sin(theta_8));
fy = simplify(yf);
% 弯曲能量
bending = sf*int(ks*ks,s,0,sf);
bending = simplify(bending);

% 偏导数
dthetaf_dp1 = diff(thetaf, p1); dthetaf_dp1 = simplify(dthetaf_dp1);
dthetaf_dp2 = diff(thetaf, p2); dthetaf_dp2 = simplify(dthetaf_dp2);
dthetaf_dsf = diff(thetaf, sf); dthetaf_dsf = simplify(dthetaf_dsf);

dxf_dp1 = diff(xf, p1); dxf_dp1 = simplify(dxf_dp1);
dxf_dp2 = diff(xf, p2); dxf_dp2 = simplify(dxf_dp2);
dxf_dsf = diff(xf, sf); dxf_dsf = simplify(dxf_dsf);

dyf_dp1 = diff(yf, p1); dyf_dp1 = simplify(dyf_dp1);
dyf_dp2 = diff(yf, p2); dyf_dp2 = simplify(dyf_dp2);
dyf_dsf = diff(yf, sf); dyf_dsf = simplify(dyf_dsf);

dbending_dp1 = diff(bending, p1); dbending_dp1 = simplify(dbending_dp1);
dbending_dp2 = diff(bending, p2); dbending_dp2 = simplify(dbending_dp2);
dbending_dsf = diff(bending, sf); dbending_dsf = simplify(dbending_dsf);

三次样条曲线的运算结果：

// ====================================================
thetaf = 
(sf*(p0 + 3*p1 + 3*p2 + p3))/8;

 
xf = 
(sf*(cos((sf*(p0 + 3*p1 + 3*p2 + p3))/8) 
+ 2*cos((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128) 
+ 2*cos((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048) 
+ 2*cos((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048) 
+ 4*cos((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768) 
+ 4*cos((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768) 
+ 4*cos((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768) 
+ 4*cos((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768) + 1))/24;

  
yf = 
(sf*(sin((p0*sf)/8 + (3*p1*sf)/8 + (3*p2*sf)/8 + (p3*sf)/8) 
+ 2*sin((15*p0*sf)/128 + (51*p1*sf)/128 - (3*p2*sf)/128 + (p3*sf)/128) 
+ 2*sin((247*p0*sf)/2048 + (363*p1*sf)/2048 - (123*p2*sf)/2048 + (25*p3*sf)/2048) 
+ 2*sin((231*p0*sf)/2048 + (891*p1*sf)/2048 + (405*p2*sf)/2048 + (9*p3*sf)/2048) 
+ 4*sin((2871*p0*sf)/32768 + (1851*p1*sf)/32768 - (795*p2*sf)/32768 + (169*p3*sf)/32768) 
+ 4*sin((4071*p0*sf)/32768 + (9963*p1*sf)/32768 - (2187*p2*sf)/32768 + (441*p3*sf)/32768) 
+ 4*sin((3655*p0*sf)/32768 + (14475*p1*sf)/32768 + (2325*p2*sf)/32768 + (25*p3*sf)/32768) 
+ 4*sin((3927*p0*sf)/32768 + (13083*p1*sf)/32768 + (10437*p2*sf)/32768 + (1225*p3*sf)/32768)))/24;

bending =
(sf^2*(64*p0^2 + 99*p0*p1 - 36*p0*p2 + 19*p0*p3 + 324*p1^2 - 81*p1*p2 - 36*p1*p3 + 324*p2^2 + 99*p2*p3 + 64*p3^2))/840;
// ==========================================================
dthetaf_dp1 = (3*sf)/8;
dthetaf_dp2 = (3*sf)/8;
dthetaf_dsf = p0/8 + (3*p1)/8 + (3*p2)/8 + p3/8;

dxf_dp1 = 
-(sf^2*(1024*sin((sf*(p0 + 3*p1 + 3*p2 + p3))/8) 
+ 2176*sin((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128) 
+ 2376*sin((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048) 
+ 968*sin((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048) 
+ 4825*sin((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768) 
+ 4361*sin((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768) 
+ 3321*sin((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768) 
+ 617*sin((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)))/65536;
 
dxf_dp2 =  
-(sf^2*(1024*sin((sf*(p0 + 3*p1 + 3*p2 + p3))/8) 
- 128*sin((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128) 
+ 1080*sin((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048) 
- 328*sin((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048) 
+ 775*sin((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768) 
+ 3479*sin((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768) 
- 729*sin((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768) 
- 265*sin((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)))/65536;

dxf_dsf = 
cos((sf*(p0 + 3*p1 + 3*p2 + p3))/8)/24 
+ cos((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128)/12 
+ cos((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048)/12 
+ cos((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048)/12 
+ cos((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768)/6 
+ cos((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768)/6 
+ cos((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768)/6 
+ cos((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)/6 
- (sf*(sin((sf*(p0 + 3*p1 + 3*p2 + p3))/8)*(p0/8 + (3*p1)/8 + (3*p2)/8 + p3/8) 
+ sin((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128)*((15*p0)/64 + (51*p1)/64 - (3*p2)/64 + p3/64) 
+ sin((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048)*((247*p0)/1024 + (363*p1)/1024 - (123*p2)/1024 + (25*p3)/1024) 
+ sin((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048)*((231*p0)/1024 + (891*p1)/1024 + (405*p2)/1024 + (9*p3)/1024) 
+ sin((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)*((2871*p0)/8192 + (1851*p1)/8192 - (795*p2)/8192 + (169*p3)/8192) 
+ sin((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768)*((4071*p0)/8192 + (9963*p1)/8192 - (2187*p2)/8192 + (441*p3)/8192) 
+ sin((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768)*((3655*p0)/8192 + (14475*p1)/8192 + (2325*p2)/8192 + (25*p3)/8192) 
+ sin((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768)*((3927*p0)/8192 + (13083*p1)/8192 + (10437*p2)/8192 + (1225*p3)/8192)))/24 
+ 1/24;

dyf_dp1 = 
(sf^2*(1024*cos((sf*(p0 + 3*p1 + 3*p2 + p3))/8) 
+ 2176*cos((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128) 
+ 2376*cos((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048) 
+ 968*cos((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048) 
+ 4825*cos((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768) 
+ 4361*cos((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768) 
+ 3321*cos((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768) 
+ 617*cos((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)))/65536;

dyf_dp2 = 
(sf^2*(1024*cos((sf*(p0 + 3*p1 + 3*p2 + p3))/8) 
- 128*cos((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128) 
+ 1080*cos((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048) 
- 328*cos((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048) 
+ 775*cos((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768) 
+ 3479*cos((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768) 
- 729*cos((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768) 
- 265*cos((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)))/65536;

dyf_dsf = 
sin((sf*(p0 + 3*p1 + 3*p2 + p3))/8)/24 
+ sin((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128)/12 
+ sin((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048)/12 
+ sin((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048)/12 
+ sin((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768)/6 
+ sin((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768)/6 
+ sin((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768)/6 
+ sin((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)/6 
+ (sf*(cos((sf*(p0 + 3*p1 + 3*p2 + p3))/8)*(p0/8 + (3*p1)/8 + (3*p2)/8 + p3/8) 
+ cos((sf*(15*p0 + 51*p1 - 3*p2 + p3))/128)*((15*p0)/64 + (51*p1)/64 - (3*p2)/64 + p3/64) 
+ cos((sf*(247*p0 + 363*p1 - 123*p2 + 25*p3))/2048)*((247*p0)/1024 + (363*p1)/1024 - (123*p2)/1024 + (25*p3)/1024) 
+ cos((3*sf*(77*p0 + 297*p1 + 135*p2 + 3*p3))/2048)*((231*p0)/1024 + (891*p1)/1024 + (405*p2)/1024 + (9*p3)/1024) 
+ cos((sf*(2871*p0 + 1851*p1 - 795*p2 + 169*p3))/32768)*((2871*p0)/8192 + (1851*p1)/8192 - (795*p2)/8192 + (169*p3)/8192) 
+ cos((3*sf*(1357*p0 + 3321*p1 - 729*p2 + 147*p3))/32768)*((4071*p0)/8192 + (9963*p1)/8192 - (2187*p2)/8192 + (441*p3)/8192) 
+ cos((5*sf*(731*p0 + 2895*p1 + 465*p2 + 5*p3))/32768)*((3655*p0)/8192 + (14475*p1)/8192 + (2325*p2)/8192 + (25*p3)/8192) 
+ cos((7*sf*(561*p0 + 1869*p1 + 1491*p2 + 175*p3))/32768)*((3927*p0)/8192 + (13083*p1)/8192 + (10437*p2)/8192 + (1225*p3)/8192)))/24;

dbending_dp1 = (sf^2*(99*p0 + 648*p1 - 81*p2 - 36*p3))/840;
dbending_dp2 = -(sf^2*(36*p0 + 81*p1 - 648*p2 - 99*p3))/840;
dbending_dsf = (sf*(64*p0^2 + 99*p0*p1 - 36*p0*p2 + 19*p0*p3 + 324*p1^2 - 81*p1*p2 - 36*p1*p3 + 324*p2^2 + 99*p2*p3 + 64*p3^2))/420;

五次样条曲线

五次样条曲线的m程序：

% five_curvature.m
clear all;
clc;
format long;
syms p0 p1 p2 p3 p4 p5 sf s; % k0 k1 k2 kf sf;
% k(s) = a + b*s + c*s^2 + d*s^3 + e*s^4 + f*s^5;
% [k0, dk0, ddk0, k1, k2, kf] = A * [a,b,c,d,e,f]
A = [1,0,0,0,0,0;
     0,1,0,0,0,0;
     0,0,2,0,0,0;
     1,sf/3,(sf/3)^2,(sf/3)^3,(sf/3)^4,(sf/3)^5;
     1,sf*2/3,(sf*2/3)^2,(sf*2/3)^3,(sf*2/3)^4,(sf*2/3)^5;
     1,sf,(sf^2),(sf^3),(sf^4),(sf^5)];
p = [p0, p1, p2, p3, p4, p5].';

% 求解多项式系数
Coeff = A\p; 
a = Coeff(1);
b = Coeff(2);
c = Coeff(3);
d = Coeff(4);
e = Coeff(5);
f = Coeff(6);

% 辛普森数值积分法求取末端状态
% theta_coeff = zeros(8,6);
for k = 1:8
    sk = sf*k/8;
    theta_coeff(k,:) = [sk, (sk^2)/2, (sk^3)/3, (sk^4)/4, (sk^5)/5, (sk^6)/6]/A;
end

theta_0 = 0;
theta_1 = theta_coeff(1,:)*p;
theta_2 = theta_coeff(2,:)*p;
theta_3 = theta_coeff(3,:)*p;
theta_4 = theta_coeff(4,:)*p;
theta_5 = theta_coeff(5,:)*p;
theta_6 = theta_coeff(6,:)*p;
theta_7 = theta_coeff(7,:)*p;
theta_8 = theta_coeff(8,:)*p;

% 末端状态
thetaf = simplify(theta_8);
xf = sf/24*(cos(theta_0) ...,
            + 4*(cos(theta_1) + cos(theta_3) + cos(theta_5) + cos(theta_7)) ...,
            + 2*(cos(theta_2) + cos(theta_4) + cos(theta_6)) ...,
            + cos(theta_8));
xf = simplify(xf);
yf = sf/24*(sin(theta_0) ...,
            + 4*(sin(theta_1) + sin(theta_3) + sin(theta_5) + sin(theta_7)) ...,
            + 2*(sin(theta_2) + sin(theta_4) + sin(theta_6)) ...,
            + sin(theta_8));
yf = simplify(yf);

% 弯曲能量
k_coeff = [1, s, s^2, s^3, s^4, s^5]/A;
ks = k_coeff*p;
bending = sf*int(ks*ks,s,0,sf);
bending = simplify(bending);

% 偏导数
dthetaf_dp3 = diff(thetaf, p3); dthetaf_dp3 = simplify(dthetaf_dp3);
dthetaf_dp4 = diff(thetaf, p4); dthetaf_dp4 = simplify(dthetaf_dp4);
dthetaf_dp5 = diff(thetaf, p5); dthetaf_dp5 = simplify(dthetaf_dp5);
dthetaf_dsf = diff(thetaf, sf); dthetaf_dsf = simplify(dthetaf_dsf);

dxf_dp3 = diff(xf, p3); dxf_dp3 = simplify(dxf_dp3);
dxf_dp4 = diff(xf, p4); dxf_dp4 = simplify(dxf_dp4);
dxf_dp5 = diff(xf, p5); dxf_dp5 = simplify(dxf_dp5);
dxf_dsf = diff(xf, sf); dxf_dsf = simplify(dxf_dsf);

dyf_dp3 = diff(yf, p3); dyf_dp3 = simplify(dyf_dp3);
dyf_dp4 = diff(yf, p4); dyf_dp4 = simplify(dyf_dp4);
dyf_dp5 = diff(yf, p5); dyf_dp5 = simplify(dyf_dp5);
dyf_dsf = diff(yf, sf); dyf_dsf = simplify(dyf_dsf);

dbending_dp3 = diff(bending, p3); dbending_dp3 = simplify(dbending_dp3);
dbending_dp4 = diff(bending, p4); dbending_dp4 = simplify(dbending_dp4);
dbending_dp5 = diff(bending, p5); dbending_dp5 = simplify(dbending_dp5);
dbending_dsf = diff(bending, sf); dbending_dsf = simplify(dbending_dsf);

五次样条曲线的运行结果：

thetaf = (sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480;

xf = 
(sf*(2*cos((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360) 
+ 2*cos((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680) 
+ 2*cos((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040) 
+ 4*cos((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912) 
+ 4*cos((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520) 
+ 4*cos((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560) 
+ 4*cos((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560) 
+ cos((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480) 
+ 1))/24;
 

yf = 
(sf*(2*sin((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360) 
+ 2*sin((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680) 
+ 2*sin((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040) 
+ 4*sin((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912) 
+ 4*sin((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520) 
+ 4*sin((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560) 
+ 4*sin((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560) 
+ sin((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480)))/24;
 

bending = 
(sf^2*(41367*p0^2 + 14784*p0*p1*sf + 1010*p0*p2*sf^2 - 85131*p0*p3 + 46008*p0*p4 + 14601*p0*p5 
+ 1540*p1^2*sf^2 + 220*p1*p2*sf^3 - 19602*p1*p3*sf + 10692*p1*p4*sf + 3366*p1*p5*sf 
+ 8*p2^2*sf^4 - 1458*p2*p3*sf^2 + 810*p2*p4*sf^2 + 254*p2*p5*sf^2 
+ 87480*p3^2 - 67797*p3*p4 - 22032*p3*p5 
+ 41553*p4^2 + 13527*p4*p5 + 4344*p5^2))/73920;
 

// ==================
dthetaf_dp3 = 0;
dthetaf_dp4 = (81*sf)/160;
dthetaf_dp5 = sf/10;
dthetaf_dsf = (p2*sf^2)/80 + (p1*sf)/8 + (63*p0)/160 + (81*p4)/160 + p5/10;

dxf_dp3 = 
-(sf*((81*sf*sin((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360))/128 
+ (6561*sf*sin((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680))/8192 
+ (729*sf*sin((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040))/8192 
+ (455625*sf*sin((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912))/262144 
+ (164025*sf*sin((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520))/262144 
+ (194481*sf*sin((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560))/262144 
+ (3969*sf*sin((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560))/262144))/24;
 

dxf_dp4 = 
-(sf*((81*sf*sin((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360))/2560 
+ (72171*sf*sin((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680))/163840 
- (1053*sf*sin((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040))/163840 
+ (354375*sf*sin((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912))/1048576 
- (85293*sf*sin((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520))/5242880 
+ (8362683*sf*sin((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560))/5242880 
- (8181*sf*sin((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560))/5242880 
+ (81*sf*sin((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480))/160))/24;
  

dxf_dp5 = 
-(sf*((23*sf*sin((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040))/40960 
- (81*sf*sin((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680))/40960 
- (sf*sin((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360))/640 
- (3125*sf*sin((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912))/262144 
+ (1863*sf*sin((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520))/1310720 
+ (112847*sf*sin((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560))/1310720 
+ (191*sf*sin((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560))/1310720 
+ (sf*sin((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480))/10))/24;

dxf_dsf = 
cos((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360)/12 
+ cos((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680)/12 
+ cos((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040)/12 
+ cos((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912)/6 
+ cos((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520)/6 
+ cos((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560)/6 
+ cos((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560)/6 
+ cos((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480)/24 
- (sf*(sin((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480)*((p2*sf^2)/80 + (p1*sf)/8 + (63*p0)/160 + (81*p4)/160 + p5/10) 
+ sin((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360)*((p2*sf^2)/1280 + (5*p1*sf)/128 + (863*p0)/2560 + (81*p3)/128 + (81*p4)/2560 - p5/640) 
+ sin((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680)*((42693*p0)/163840 + (6561*p3)/8192 + (72171*p4)/163840 - (81*p5)/40960 + (63*p1*sf)/8192 - (189*p2*sf^2)/81920) 
+ sin((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040)*((347*p2*sf^2)/81920 + (599*p1*sf)/8192 + (68301*p0)/163840 + (729*p3)/8192 - (1053*p4)/163840 + (23*p5)/40960) 
+ sin((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912)*((457065*p0)/1048576 + (455625*p3)/262144 + (354375*p4)/1048576 - (3125*p5)/262144 - (6025*p1*sf)/262144 - (4625*p2*sf^2)/524288) 
+ sin((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520)*((25947*p2*sf^2)/2621440 + (43047*p1*sf)/262144 + (4661661*p0)/5242880 + (164025*p3)/262144 - (85293*p4)/5242880 + (1863*p5)/1310720) 
+ sin((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560)*((5779*p2*sf^2)/2621440 + (14207*p1*sf)/262144 + (2549477*p0)/5242880 + (3969*p3)/262144 - (8181*p4)/5242880 + (191*p5)/1310720) 
+ sin((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560)*((62083*p2*sf^2)/2621440 + (70511*p1*sf)/262144 + (5646389*p0)/5242880 + (194481*p3)/262144 + (8362683*p4)/5242880 + (112847*p5)/1310720)))/24 
+ 1/24;
 

dyf_dp3 = 
(sf*((81*sf*cos((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360))/128 
+ (6561*sf*cos((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680))/8192 
+ (729*sf*cos((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040))/8192 
+ (455625*sf*cos((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912))/262144 
+ (164025*sf*cos((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520))/262144 
+ (194481*sf*cos((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560))/262144 
+ (3969*sf*cos((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560))/262144))/24;

dyf_dp4 = 
(sf*((81*sf*cos((sf*(2*p2*sf^2 + 150*p1*sf + 2589*p0 + 4860*p3 + 243*p4 - 12*p5))/15360))/2560 
+ (72171*sf*cos((3*sf*(- 42*p2*sf^2 + 210*p1*sf + 14231*p0 + 43740*p3 + 24057*p4 - 108*p5))/327680))/163840 
- (1053*sf*cos((sf*(694*p2*sf^2 + 17970*p1*sf + 204903*p0 + 43740*p3 - 3159*p4 + 276*p5))/983040))/163840 
+ (354375*sf*cos((5*sf*(- 1850*p2*sf^2 - 7230*p1*sf + 274239*p0 + 1093500*p3 + 212625*p4 - 7500*p5))/12582912))/1048576 
- (85293*sf*cos((3*sf*(5766*p2*sf^2 + 143490*p1*sf + 1553887*p0 + 1093500*p3 - 28431*p4 + 2484*p5))/20971520))/5242880 
+ (8362683*sf*cos((7*sf*(17738*p2*sf^2 + 302190*p1*sf + 2419881*p0 + 1666980*p3 + 3584007*p4 + 193452*p5))/62914560))/5242880 
- (8181*sf*cos((sf*(11558*p2*sf^2 + 426210*p1*sf + 7648431*p0 + 238140*p3 - 24543*p4 + 2292*p5))/62914560))/5242880 
+ (81*sf*cos((sf*(2*p2*sf^2 + 30*p1*sf + 189*p0 + 243*p4 + 48*p5))/480))/160))/24;
  
 dyf_dp5 = 
(sf*((sf*cos((63*p0*sf)/160 + (81*p4*sf)/160 + (p5*sf)/10 + (p1*sf^2)/16 + (p2*sf^3)/240))/10 
- (sf*cos((863*p0*sf)/5120 + (81*p3*sf)/256 + (81*p4*sf)/5120 - (p5*sf)/1280 + (5*p1*sf^2)/512 + (p2*sf^3)/7680))/640 
- (81*sf*cos((42693*p0*sf)/327680 + (6561*p3*sf)/16384 + (72171*p4*sf)/327680 - (81*p5*sf)/81920 + (63*p1*sf^2)/32768 - (63*p2*sf^3)/163840))/40960 
+ (23*sf*cos((68301*p0*sf)/327680 + (729*p3*sf)/16384 - (1053*p4*sf)/327680 + (23*p5*sf)/81920 + (599*p1*sf^2)/32768 + (347*p2*sf^3)/491520))/40960 
- (3125*sf*cos((457065*p0*sf)/4194304 + (455625*p3*sf)/1048576 + (354375*p4*sf)/4194304 - (3125*p5*sf)/1048576 - (6025*p1*sf^2)/2097152 - (4625*p2*sf^3)/6291456))/262144 
+ (1863*sf*cos((4661661*p0*sf)/20971520 + (164025*p3*sf)/1048576 - (85293*p4*sf)/20971520 + (1863*p5*sf)/5242880 + (43047*p1*sf^2)/2097152 + (8649*p2*sf^3)/10485760))/1310720 
+ (191*sf*cos((2549477*p0*sf)/20971520 + (3969*p3*sf)/1048576 - (8181*p4*sf)/20971520 + (191*p5*sf)/5242880 + (14207*p1*sf^2)/2097152 + (5779*p2*sf^3)/31457280))/1310720 
+ (112847*sf*cos((5646389*p0*sf)/20971520 + (194481*p3*sf)/1048576 + (8362683*p4*sf)/20971520 + (112847*p5*sf)/5242880 + (70511*p1*sf^2)/2097152 + (62083*p2*sf^3)/31457280))/1310720))/24;
 
 dyf_dsf = 
sin((863*p0*sf)/5120 + (81*p3*sf)/256 + (81*p4*sf)/5120 - (p5*sf)/1280 + (5*p1*sf^2)/512 + (p2*sf^3)/7680)/12 
+ sin((42693*p0*sf)/327680 + (6561*p3*sf)/16384 + (72171*p4*sf)/327680 - (81*p5*sf)/81920 + (63*p1*sf^2)/32768 - (63*p2*sf^3)/163840)/12 
+ sin((68301*p0*sf)/327680 + (729*p3*sf)/16384 - (1053*p4*sf)/327680 + (23*p5*sf)/81920 + (599*p1*sf^2)/32768 + (347*p2*sf^3)/491520)/12 
+ sin((457065*p0*sf)/4194304 + (455625*p3*sf)/1048576 + (354375*p4*sf)/4194304 - (3125*p5*sf)/1048576 - (6025*p1*sf^2)/2097152 - (4625*p2*sf^3)/6291456)/6 
+ sin((4661661*p0*sf)/20971520 + (164025*p3*sf)/1048576 - (85293*p4*sf)/20971520 + (1863*p5*sf)/5242880 + (43047*p1*sf^2)/2097152 + (8649*p2*sf^3)/10485760)/6 
+ sin((2549477*p0*sf)/20971520 + (3969*p3*sf)/1048576 - (8181*p4*sf)/20971520 + (191*p5*sf)/5242880 + (14207*p1*sf^2)/2097152 + (5779*p2*sf^3)/31457280)/6 
+ sin((5646389*p0*sf)/20971520 + (194481*p3*sf)/1048576 + (8362683*p4*sf)/20971520 + (112847*p5*sf)/5242880 + (70511*p1*sf^2)/2097152 + (62083*p2*sf^3)/31457280)/6 
+ sin((63*p0*sf)/160 + (81*p4*sf)/160 + (p5*sf)/10 + (p1*sf^2)/16 + (p2*sf^3)/240)/24 
+ (sf*(cos((63*p0*sf)/160 + (81*p4*sf)/160 + (p5*sf)/10 + (p1*sf^2)/16 + (p2*sf^3)/240)*((p2*sf^2)/80 + (p1*sf)/8 + (63*p0)/160 + (81*p4)/160 + p5/10) 
+ 2*cos((863*p0*sf)/5120 + (81*p3*sf)/256 + (81*p4*sf)/5120 - (p5*sf)/1280 + (5*p1*sf^2)/512 + (p2*sf^3)/7680)*((p2*sf^2)/2560 + (5*p1*sf)/256 + (863*p0)/5120 + (81*p3)/256 + (81*p4)/5120 - p5/1280) 
+ 2*cos((42693*p0*sf)/327680 + (6561*p3*sf)/16384 + (72171*p4*sf)/327680 - (81*p5*sf)/81920 + (63*p1*sf^2)/32768 - (63*p2*sf^3)/163840)*((42693*p0)/327680 + (6561*p3)/16384 + (72171*p4)/327680 - (81*p5)/81920 + (63*p1*sf)/16384 - (189*p2*sf^2)/163840) 
+ 2*cos((68301*p0*sf)/327680 + (729*p3*sf)/16384 - (1053*p4*sf)/327680 + (23*p5*sf)/81920 + (599*p1*sf^2)/32768 + (347*p2*sf^3)/491520)*((347*p2*sf^2)/163840 + (599*p1*sf)/16384 + (68301*p0)/327680 + (729*p3)/16384 - (1053*p4)/327680 + (23*p5)/81920) 
+ 4*cos((457065*p0*sf)/4194304 + (455625*p3*sf)/1048576 + (354375*p4*sf)/4194304 - (3125*p5*sf)/1048576 - (6025*p1*sf^2)/2097152 - (4625*p2*sf^3)/6291456)*((457065*p0)/4194304 + (455625*p3)/1048576 + (354375*p4)/4194304 - (3125*p5)/1048576 - (6025*p1*sf)/1048576 - (4625*p2*sf^2)/2097152) 
+ 4*cos((4661661*p0*sf)/20971520 + (164025*p3*sf)/1048576 - (85293*p4*sf)/20971520 + (1863*p5*sf)/5242880 + (43047*p1*sf^2)/2097152 + (8649*p2*sf^3)/10485760)*((25947*p2*sf^2)/10485760 + (43047*p1*sf)/1048576 + (4661661*p0)/20971520 + (164025*p3)/1048576 - (85293*p4)/20971520 + (1863*p5)/5242880) 
+ 4*cos((2549477*p0*sf)/20971520 + (3969*p3*sf)/1048576 - (8181*p4*sf)/20971520 + (191*p5*sf)/5242880 + (14207*p1*sf^2)/2097152 + (5779*p2*sf^3)/31457280)*((5779*p2*sf^2)/10485760 + (14207*p1*sf)/1048576 + (2549477*p0)/20971520 + (3969*p3)/1048576 - (8181*p4)/20971520 + (191*p5)/5242880) 
+ 4*cos((5646389*p0*sf)/20971520 + (194481*p3*sf)/1048576 + (8362683*p4*sf)/20971520 + (112847*p5*sf)/5242880 + (70511*p1*sf^2)/2097152 + (62083*p2*sf^3)/31457280)*((62083*p2*sf^2)/10485760 + (70511*p1*sf)/1048576 + (5646389*p0)/20971520 + (194481*p3)/1048576 + (8362683*p4)/20971520 + (112847*p5)/5242880)))/24;
 

 dbending_dp3 = 
-(sf^2*(1458*p2*sf^2 + 19602*p1*sf + 85131*p0 - 174960*p3 + 67797*p4 + 22032*p5))/73920;

dbending_dp4 = 
(sf^2*(810*p2*sf^2 + 10692*p1*sf + 46008*p0 - 67797*p3 + 83106*p4 + 13527*p5))/73920;

dbending_dp5 = 
(sf^2*(254*p2*sf^2 + 3366*p1*sf + 14601*p0 - 22032*p3 + 13527*p4 + 8688*p5))/73920;

dbending_dsf = 
(sf*(41367*p0^2 + 22176*p0*p1*sf + 2020*p0*p2*sf^2 - 85131*p0*p3 + 46008*p0*p4 + 14601*p0*p5 
+ 3080*p1^2*sf^2 + 550*p1*p2*sf^3 - 29403*p1*p3*sf + 16038*p1*p4*sf + 5049*p1*p5*sf 
+ 24*p2^2*sf^4 - 2916*p2*p3*sf^2 + 1620*p2*p4*sf^2 + 508*p2*p5*sf^2 
+ 87480*p3^2 - 67797*p3*p4 - 22032*p3*p5 
+ 41553*p4^2 + 13527*p4*p5 + 4344*p5^2))/36960;