Supplied objective function must return a scalar value. ERROR in fmincon

2 次查看（过去 30 天）
john white
john white 2021-1-19
编辑： john white 2021-1-19
hi I write this code and i'm using fmincon to minimize the K function, this is the code:
clc;
clear;
close all;
Ts = 0.01;
Tf = 10;
t = 0:Ts:Tf;
Np = 5;
N = numel(t);
Yref = ones(3 , N);
y = zeros(3,N);
u = zeros(3,N);
U = zeros(3,N);
theta1=zeros(N,1);
theta2=zeros(N,1);
theta3=zeros(N,1);
theta_dot1=zeros(N,1);
theta_dot2=zeros(N,1);
theta_dot3=zeros(N,1);
theta1(1) = 0;
theta2(1) = 0;
theta3(1) = 0;
theta_dot1(1) = 0.01;
theta_dot2(1) = 0.01;
theta_dot3(1) = 0.01;
theta_vec(:,1)=[theta1(1);theta2(1);theta3(1);theta_dot1(1);theta_dot2(1);theta_dot3(1)];
for i = 1:N
C=[(1.0*cos(theta2(i))*(0.36*cos(conj(theta2(i))) + cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) + 3.8e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.8e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-34*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) + sin(theta2(i))*(0.04*sin(conj(theta2(i))) + 1.0*cos(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))))) + 0.066) (1.0*cos(theta2(i))*(9.3e-19*sin(conj(theta3(i)))*sin(theta3(i)) + 2.6e-17*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) - cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) - 5.7e-19) - sin(theta2(i))*(6.2e-21*sin(conj(theta3(i)))*cos(theta3(i)) - sin(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19))) (-1.0*cos(theta2(i))*(4.6e-19*cos(theta3(i)) + 3.7e-19));(0.21*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) - 1.2e-16*cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.2e-16*sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 5.7e-19*cos(conj(theta2(i))) + 4.6e-19*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 9.4e-19*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) (3.8e-3*cos(conj(theta3(i))) + 7.7e-3*sin(conj(theta3(i)))*sin(theta3(i)) + 0.21*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) + 1.2e-16*cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) + 0.36) (- 3.8e-3*cos(theta3(i)) - 3.0e-3);(- 3.7e-19*cos(conj(theta2(i))) - 4.6e-19*cos(conj(theta2(i)))*cos(conj(theta3(i)))) (- 3.8e-3*cos(conj(theta3(i))) - 3.0e-3) 3.0e-3];
E1=- 1.0*cos(theta2(i))*(4.3e-17*cos(conj(theta2(i))) - 0.036*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.2e-18*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-34*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 1.2e-16*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-17*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.1e-17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 2.2e-18*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.2e-16*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.1e-17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 0.67*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 9.4e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.9e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 4.6e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta2(i))*(1.0*cos(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - sin(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.045*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i))));
E2=0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 4.4e-18*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.28*cos(conj(theta2(i))) - 0.21*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.0*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 1.2e-16*cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.21*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-16*sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 8.0e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.31*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.1e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 5.7e-35*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E3= (5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.0*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) - 1.4e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) + 2.3e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E=[E1;E2;E3];
digits(2);
c_inv=inv(C);
V=c_inv*(-E);
A=[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;0 0 0 V(1,1)/theta_dot1(i) 0 0;0 0 0 0 V(2,1)/theta_dot2(i) 0;0 0 0 0 0 V(3,1)/theta_dot3(i)];
B=[0 0 0;0 0 0;0 0 0;c_inv(1,1) 0 0;0 c_inv(2,2) c_inv(2,3);0 c_inv(3,2) c_inv(3,3)];
fc = A*theta_vec(:,i)+B*u(:,i) ;
f = theta_vec(:,i) + Ts*fc ;
K = @(u) tee(theta_vec(:,i),Yref(:,i),Np,u(:,i),Ts);
U = fmincon(K,U,[],[]);
u(:,i+1) = U(:,i);
theta1(i+1) = f(1,1);
theta2(i+1) = f(2,1);
theta3(i+1) = f(3,1);
theta_dot1(i+1) = f(4,1);
theta_dot2(i+1) = f(5,1);
theta_dot3(i+1) = f(6,1);
y(:,i+1) = [theta1(i+1);theta2(i+1);theta3(i+1)];
theta_vec(:,i+1)=[theta1(i+1);theta2(i+1);theta3(i+1);theta_dot1(i+1);theta_dot2(i+1);theta_dot3(i+1)];
end
and tee is:
function J = tee(theta_veci,Yref,Np,ui,Ts)
u = zeros(3,Np);
u = [ui u];
theta_vec = zeros(6,Np);
theta_vec = [theta_veci theta_vec];
theta1(1) = theta_vec(1,1);
theta2(1) = theta_vec(2,1);
theta3(1) = theta_vec(3,1);
theta_dot1(1) = theta_vec(4,1);
theta_dot2(1) = theta_vec(5,1);
theta_dot3(1) = theta_vec(6,1);
for i =1:Np
C=[(1.0*cos(theta2(i))*(0.36*cos(conj(theta2(i))) + cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) + 3.8e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.8e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-34*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) + sin(theta2(i))*(0.04*sin(conj(theta2(i))) + 1.0*cos(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))))) + 0.066) (1.0*cos(theta2(i))*(9.3e-19*sin(conj(theta3(i)))*sin(theta3(i)) + 2.6e-17*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) - cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) - 5.7e-19) - sin(theta2(i))*(6.2e-21*sin(conj(theta3(i)))*cos(theta3(i)) - sin(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19))) (-1.0*cos(theta2(i))*(4.6e-19*cos(theta3(i)) + 3.7e-19));(0.21*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) - 1.2e-16*cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.2e-16*sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 5.7e-19*cos(conj(theta2(i))) + 4.6e-19*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 9.4e-19*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) (3.8e-3*cos(conj(theta3(i))) + 7.7e-3*sin(conj(theta3(i)))*sin(theta3(i)) + 0.21*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) + 1.2e-16*cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) + 0.36) (- 3.8e-3*cos(theta3(i)) - 3.0e-3);(- 3.7e-19*cos(conj(theta2(i))) - 4.6e-19*cos(conj(theta2(i)))*cos(conj(theta3(i)))) (- 3.8e-3*cos(conj(theta3(i))) - 3.0e-3) 3.0e-3];
E1=- 1.0*cos(theta2(i))*(4.3e-17*cos(conj(theta2(i))) - 0.036*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.2e-18*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-34*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 1.2e-16*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-17*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.1e-17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 2.2e-18*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.2e-16*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.1e-17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 0.67*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 9.4e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.9e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 4.6e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta2(i))*(1.0*cos(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - sin(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.045*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i))));
E2=0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 4.4e-18*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.28*cos(conj(theta2(i))) - 0.21*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.0*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 1.2e-16*cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.21*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-16*sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 8.0e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.31*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.1e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 5.7e-35*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E3= (5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.0*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) - 1.4e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) + 2.3e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E=[E1;E2;E3];
digits(2);
c_inv=inv(C);
V=c_inv*(-E);
A=[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;0 0 0 V(1,1)/theta_dot1(i) 0 0;0 0 0 0 V(2,1)/theta_dot2(i) 0;0 0 0 0 0 V(3,1)/theta_dot3(i)];
B=[0 0 0;0 0 0;0 0 0;c_inv(1,1) 0 0;0 c_inv(2,2) c_inv(2,3);0 c_inv(3,2) c_inv(3,3)];
fc = A*theta_vec(:,i)+B*u(:,i) ;
f = theta_vec(:,i) + Ts*fc ;
theta1(i+1) = f(1,1);
theta2(i+1) = f(2,1);
theta3(i+1) = f(3,1);
theta_dot1(i+1) = f(4,1);
theta_dot2(i+1) = f(5,1);
theta_dot3(i+1) = f(6,1);
y(:,i+1) = [theta1(i+1);theta2(i+1);theta3(i+1)];
theta_vec(:,i+1)=[theta1(i+1);theta2(i+1);theta3(i+1);theta_dot1(i+1);theta_dot2(i+1);theta_dot3(i+1)];
end
J = sum((Yref-y).^2) + sum(u.^2);
end
and the error is:
Error using fmincon (line 609)
Supplied objective function must return a scalar value.
Error in MPC1 (line 42)
U = fmincon(K,U,[],[]);

请先登录，再回答此问题。

回答（1 个）

Stephan
Stephan 2021-1-19
编辑：Stephan 2021-1-19
J is a 1x6 vector, but should be a scalar. You have to find out why and fix this.
  1 个评论
john white
john white 2021-1-19
编辑：john white 2021-1-19
thanks for your answer
I changed the code as:
J = sum((Yref(:,i)-y(:,i)).^2) + sum(u(:,i).^2);
and the J be the scalar but I got another error:
Index exceeds matrix dimensions.
Error in tee (line 39)
J = sum((Yref(:,i)-y(:,i)).^2) + sum(u(:,i).^2);
Error in MPC1>@(u)tee(theta_vec(:,i),Yref(:,i),Np,u(:,i),Ts)
Error in fmincon (line 536)
initVals.f = feval(funfcn{3},X,varargin{:});
Error in MPC1 (line 42)
U = fmincon(K,U,[],[]);
Caused by:
Failure in initial objective function evaluation. FMINCON cannot continue.

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 Specialized Power Systems 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by