|
根据足端位置,本体和肩关节位置,反求三个关节角。单片机运行速度可以满足实时性
function angle=anglecal(zu1,rc,b) %%
%% zu1 为足端位置, rc为本体中心坐标,b为 髋关节坐标。求解
%% th1 th2 th3 为三个关节角。
%b=[-0.1 0.2 0];
% rc=[0 0.25 0.25];
r=[1,0,0;0,1,0;0,0,1;];
% zu1=[-0.3,0.45,0.45];
zub1=r*(zu1.'-rc.')-b.';
x=zub1(1,1);
y=zub1(2,1);
z=zub1(3,1);
L=sqrt(x*x+y*y);
if y<0&&y/x<-sqrt(3)/3
L=-sqrt(x*x+y*y);
end;
if y<0&&y/x>0
L=-sqrt(x*x+y*y);
end;
if y>0&&y/x>-sqrt(3)/3&&y/x<0
L=-sqrt(x*x+y*y);
end;
% if y<0
% L=-sqrt(x*x+y*y);
% end;
% if y>0
% L=sqrt(x*x+y*y);
% end;
l1=50;
l2=120;
l3=200;
th1=atan2(y,x);
a1=-2*l2*z;
a2=2*l2*(L-l1);
a3=z*z+(L-l1)*(L-l1)+l2*l2-l3*l3;
b1=-2*l3*z;
b2=2*l3*(L-l1);
b3=z*z+(L-l1)*(L-l1)+l3*l3-l2*l2;
t1=(a1-sqrt(abs(a1*a1+a2*a2-a3*a3)))/(a2+a3);
t2=(b1+sqrt(abs(b1*b1+b2*b2-b3*b3)))/(b2+b3);
t11=(a1+sqrt(abs(a1*a1+a2*a2-a3*a3)))/(a2+a3);
t21=(b1-sqrt(abs(b1*b1+b2*b2-b3*b3)))/(b2+b3);
th2=2*atan(t1);
th3=2*atan(t2)-2*atan(t1);
th21=2*atan(t11);
th31=2*atan(t21)-2*atan(t11);
% th1=th1*180/3.14;
% th2=th2*180/3.14;
% th3=th3*180/3.14;
% th21=th21*180/3.14;
% th31=th31*180/3.14;
if th1<-pi/6
th1=th1+pi;
end;
if th1>5*pi/6
th1=th1-pi;
% if th1<0
% th1=th1+pi;
end;
angle=[th1 th2 th3 ];
|
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|