How to use a 'for loop' with inputs from the output of a previous equation.
1 回表示 (過去 30 日間)
古いコメントを表示
Hello, Below is the code for inverse kinematics of a robot.
I wish to use the output of Theta [2x1] in for loop where value in first row and first colums of Theta is used in first iteration and value in 1st row and second column is used in second iteration.
I am new to Matlab.
gd=[-0.0256,-0.4496,-0.8929,-4.197;0.9758,0.1829,-0.1200,15.369;0.2173,-0.8743,0.4340,13.931;0,0,0,1];
gst0=[1 0 0 33; 0 1 0 -13; 0 0 1 14.7; 0 0 0 1];
g1=gd*inv(gst0);
p1=[24;-13;14.7];
p2=[0;-13;14.7];
r=[12;-13;0];
Delta=norm((g1*[p1;1])-[p2;1])
w4=[0;0;-1];
w=w4;
p=p1;
q=p2;
Theta=Thirdsubproblem (w,Delta,p,q,r)
for i=Theta(1,1):Theta(1:2)
w5=[0;0;-1];
w6=[1;0;0];
I=[1,0,0;0,1,0;0,0,1];
q4=[12;-13;14.7];
expOmega4hatTheta4=[cos(i),-sin(i),0;sin(i),cos(i),0;0,0,1];
expPsihat4Theta4=[expOmega4hatTheta4,((I-expOmega4hatTheta4)*q4);0,0,0,1];
p2=[0;-13;14.7];
p3=expPsihat4Theta4*[p2;1];
gd=[-0.0256,-0.4496,-0.8929,-4.197;0.9758,0.1829,-0.1200,15.369;0.2173,-0.8743,0.4340,13.931;0,0,0,1];
gst0=[1 0 0 33; 0 1 0 -13; 0 0 1 14.7; 0 0 0 1];
g2p2=inv(gd)*gst0*[p2;1];
q=p3([1,2,3],:);
p=g2p2([1,2,3],:);
w1=w5;
w2=w6;
r=[24;-13;14.7];
[Gamma]=Secondsubproblem (w1,w2,p,q,r)
for X=Gamma(1,1):Gamma(1:2)
u=p-r;
v=q-r;
alpha=((w1'*w2)*w2'*u-w1'*v)/(((w1'*w2)^2)-1);
Beta=((w1'*w2)*w1'*v-w2'*u)/(((w1'*w2)^2)-1);
z=(alpha*w1)+(Beta*w2)+(X)*(cross(w1,w2));
c=z+r;
Theta2SP2=Firstsubproblemsecond1 (w2,p,z,r)
Theta1SP1=Firstsubproblemsecond2 (w1,q,z,r)
clear
end
clear
end
0 件のコメント
採用された回答
Ghazwan
2022 年 10 月 15 日
gd=[-0.0256,-0.4496,-0.8929,-4.197;0.9758,0.1829,-0.1200,15.369;0.2173,-0.8743,0.4340,13.931;0,0,0,1];
gst0=[1 0 0 33; 0 1 0 -13; 0 0 1 14.7; 0 0 0 1];
g1=gd*inv(gst0);
p1=[24;-13;14.7];
p2=[0;-13;14.7];
r=[12;-13;0];
Delta=norm((g1*[p1;1])-[p2;1])
w4=[0;0;-1];
w=w4;
p=p1;
q=p2;
Theta=Thirdsubproblem(w,Delta,p,q,r)
for ii=1:2
i=Theta(1,ii);
w5=[0;0;-1];
w6=[1;0;0];
I=[1,0,0;0,1,0;0,0,1];
q4=[12;-13;14.7];
expOmega4hatTheta4=[cos(i),-sin(i),0;sin(i),cos(i),0;0,0,1];
expPsihat4Theta4=[expOmega4hatTheta4,((I-expOmega4hatTheta4)*q4);0,0,0,1];
p2=[0;-13;14.7];
p3=expPsihat4Theta4*[p2;1];
gd=[-0.0256,-0.4496,-0.8929,-4.197;0.9758,0.1829,-0.1200,15.369;0.2173,-0.8743,0.4340,13.931;0,0,0,1];
gst0=[1 0 0 33; 0 1 0 -13; 0 0 1 14.7; 0 0 0 1];
g2p2=inv(gd)*gst0*[p2;1];
q=p3([1,2,3],:);
p=g2p2([1,2,3],:);
w1=w5;
w2=w6;
r=[24;-13;14.7];
[Gamma]=Secondsubproblem (w1,w2,p,q,r)
for jj=1:2
X=Gamma(1,jj);
u=p-r;
v=q-r;
alpha=((w1'*w2)*w2'*u-w1'*v)/(((w1'*w2)^2)-1);
Beta=((w1'*w2)*w1'*v-w2'*u)/(((w1'*w2)^2)-1);
z=(alpha*w1)+(Beta*w2)+(X)*(cross(w1,w2));
c=z+r;
Theta2SP2=Firstsubproblemsecond1 (w2,p,z,r)
Theta1SP1=Firstsubproblemsecond2 (w1,q,z,r)
clear
end
clear
end
2 件のコメント
Ghazwan
2022 年 10 月 15 日
My pleasure. If my solution is correct, please mark "Accept this answer." Thank you.
その他の回答 (0 件)
参考
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!