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!