How to plot trajectory of angles at varying x, y, and z position

4 次查看（过去 30 天）
Hwajin Choi
Hwajin Choi 2021-4-6
评论： Alan Stevens 2021-4-6
Hello,
I want to plot trajectory of angles theta1, theta2, and theta3 vary depending on xyz position in 2D or 3D.
I got the theta1,2 and 3 functions having x, y, and z variables from the last 3 lines at the bottom of the code.
I would appreciate your help.
function [theta1, theta2, theta3] = trajectory(x,y,z)
syms theta1 theta2 theta3 x y z
L = 0.7;
l = 1.3;
wb = 0.2;
sp = 0.08;
wp = 0.02;
up = 0.05;
a = wb - up;
b = sp/2 - (sqrt(3)/2)*wb;
c = wp - wb/2;
eqn1 = 2*L*(y+a)*cos(theta1) + 2*z*L*sin(theta1) + x^2 + y^2 + z^2 + a^2 + L^2 + 2*y*a - l^2 == 0
eqn2 = -L*(sqrt(3)*(x+b)+y+c)*cos(theta2) + 2*z*L*sin(theta2) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 + 2*x*b + 2*y*c - l^2 == 0
eqn3 = L*(sqrt(3)*(x-b)-y-c)*cos(theta3) + 2*z*L*sin(theta3) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 - 2*x*b + 2*y*c - l^2 == 0
theta1=solve(eqn1,theta1)
theta2=solve(eqn2,theta2)
theta3=solve(eqn3,theta3)
theta1 = theta1(2)
theta2 = theta2(2)
theta3 = theta3(2)

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

采纳的回答

Alan Stevens
Alan Stevens 2021-4-6
Do you mean something like this (where I've obviously used arbitrary data):
x = linspace(0,1,10);
y = linspace(0,2,10);
z = linspace(0,3,10);
THETA = zeros(3,10);
theta = [0,0,0];
for i = 1:10
theta0 = theta;
THETA(:,i) = fminsearch(@(THETA) trajectory(THETA,x(i),y(i),z(i)), theta0);
end
theta1 = THETA(1,:);
theta2 = THETA(2,:);
theta3 = THETA(3,:);
plot3(theta1,theta2,theta3,'-o'),grid
xlabel('x'),ylabel('y'),zlabel('z')
function F = trajectory(THETA, x,y,z)
theta1 = THETA(1);
theta2 = THETA(2);
theta3 = THETA(3);
L = 0.7;
l = 1.3;
wb = 0.2;
sp = 0.08;
wp = 0.02;
up = 0.05;
a = wb - up;
b = sp/2 - (sqrt(3)/2)*wb;
c = wp - wb/2;
F1 = 2*L*(y+a)*cos(theta1) + 2*z*L*sin(theta1) + x^2 + y^2 + z^2 + a^2 + L^2 + 2*y*a - l^2;
F2 = -L*(sqrt(3)*(x+b)+y+c)*cos(theta2) + 2*z*L*sin(theta2) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 + 2*x*b + 2*y*c - l^2;
F3 = L*(sqrt(3)*(x-b)-y-c)*cos(theta3) + 2*z*L*sin(theta3) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 - 2*x*b + 2*y*c - l^2;
F = norm(F1) + norm(F2) + norm(F3);
end
  4 个评论
显示 2更早的评论
Hwajin Choi
Hwajin Choi 2021-4-6
Then should I use x,y,and z functions of theta1, theta2, and theta3 instead of the theta function I originally have? I am not sure how to interchange thetas and x,y, and z. Can I just simply convert the variables?
Alan Stevens
Alan Stevens 2021-4-6
Like the following
theta1 = linspace(0,pi/2,20);
theta2 = linspace(0,3*pi/2,20);
theta3 = linspace(0,pi,20);
xyz = zeros(3,10);
xyz(:,1) = [0;0;0];
for i = 1:10
xyz0 = xyz(:,i);
xyz(:,i) = fminsearch(@(xyz) trajectory(xyz,theta1(i),theta2(i),theta3(i)), xyz0);
end
x =xyz(1,:);
y = xyz(2,:);
z = xyz(3,:);
plot3(x,x,z,'-o'),grid
xlabel('x'),ylabel('y'),zlabel('z')
function F = trajectory(xyz, theta1,theta2,theta3)
x = xyz(1);
y = xyz(2);
z = xyz(3);
L = 0.7;
l = 1.3;
wb = 0.2;
sp = 0.08;
wp = 0.02;
up = 0.05;
a = wb - up;
b = sp/2 - (sqrt(3)/2)*wb;
c = wp - wb/2;
F1 = 2*L*(y+a)*cos(theta1) + 2*z*L*sin(theta1) + x^2 + y^2 + z^2 + a^2 + L^2 + 2*y*a - l^2;
F2 = -L*(sqrt(3)*(x+b)+y+c)*cos(theta2) + 2*z*L*sin(theta2) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 + 2*x*b + 2*y*c - l^2;
F3 = L*(sqrt(3)*(x-b)-y-c)*cos(theta3) + 2*z*L*sin(theta3) + x^2 + y^2 + z^2 + b^2 + c^2 + L^2 - 2*x*b + 2*y*c - l^2;
F = norm(F1) + norm(F2) + norm(F3);
end

请先登录，再进行评论。

更多回答（0 个）

类别

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

标签

Community Treasure Hunt

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

Start Hunting!

Translated by