Verify plane equation perpendicular to vector

14 次查看(过去 30 天)
When robot gripper is in certain position I have to get a plane equation perpendicular to approach vector.
My code is following:
%% Plot approach vector
approach_vec=[-0.616490990166493, 0.492703172483058, -0.614151807673533]; %Approach vector
t=[-0.234045440283085, -0.520172447189332, -0.042211687615712]; %Approach vector origin
X=t(1);
Y=t(2);
Z=t(3);
U1=approach_vec(1);
V1=approach_vec(2);
W1=approach_vec(3);
quiver3(X,Y,Z,U1,V1,W1,'Color','#0072BD','linewidth',5,'AutoScale','on','AutoScaleFactor',1,'ShowArrowHead','on','Marker','.',MarkerEdgeColor='#D95319');
hold on;
%% Plot PLANE ABCD
A=approach_vec(1);
B=approach_vec(2);
C=approach_vec(3);
x1=t(1);
y1=t(2);
z1=t(3);
D=-(A*x1+B*y1+C*z1);
[x y] = meshgrid(-1:0.05:1); % Generate x and y data
z = -1/C*(A*x + B*y + D); % Solve for z data
surf(x,y,z); %Plot the surface
hold on; axis equal; plane=[A,B,C,D];
Plot and everything looks ok, but elements of this plane equation (A B C D) are input to some other function in which I get errors so I would like to get confirmation that this calculation is correct based on input data, because just maybe I am missing something.

采纳的回答

Chunru
Chunru 2022-8-18
Your code is correct. Here is a slighly simplified and improved(?) version.
%% Plot approach vector
approach_vec=[-0.616490990166493, 0.492703172483058, -0.614151807673533]; %Approach vector
t=[-0.234045440283085, -0.520172447189332, -0.042211687615712]; %Approach vector origin
quiver3(t(1),t(2),t(3),approach_vec(1),approach_vec(2), approach_vec(3), ...
'Color','#0072BD','linewidth',5,'AutoScale','on','AutoScaleFactor',1,'ShowArrowHead','on','Marker','.',MarkerEdgeColor='#D95319');
hold on;
%% Plot PLANE ABCD
A=approach_vec(1);
B=approach_vec(2);
C=approach_vec(3);
% A(x−a)+B(y−b)+C(z−c) = 0 where approach_vec=[A B C] and t=[a b c]
D = - approach_vec(:)'*t(:); % D=-(Aa+Bb+Cc)
[x y] = meshgrid(-1:0.05:1); % Generate x and y data
z = -1/C*(A*x + B*y + D); % Solve for z data
surf(x,y,z, 'EdgeColor', 'none', 'FaceAlpha', 0.7); %Plot the surface
hold on; axis equal;
xlabel('x'); ylabel('y'); zlabel('z');
plane=[A,B,C,D];

更多回答(0 个)

产品


版本

R2021b

Community Treasure Hunt

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

Start Hunting!

Translated by