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

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)

Answer 1

[Alan Stevens]

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

[Hwajin Choi]

Sorry, what I want is to show kinda range of movement in x,y, and z workspace depending on theta1, theta2, and theta3. But I really appreciate you for trying to help me : )

[Alan Stevens]

So, you want the inverse of what I did. You should be able to make use of what I supplied, by interchanging the thetas with x y and z.

[Hwajin Choi]

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]

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