@机器人逆向运动控制close loop matlab函数记录
本次经验 1.编写函数一定要保存，一定要保存！！！！ 2. 编写函数的形式：function [a,b,c]=close_loop(m,n,b,v,c)，注意文件名要与函数名一致。Q矩阵形式如何表示。 3.不等于号等于号和==的区别加深印象，绝对值abs，dot 和cross函数使用 4.数乘一个向量，选取单个向量a（2，1）二行一列的数
5.右手定则的相关手势表示 错误：使用变量前还没有定义，原因是"cos" 以前似乎用作函数或命令，与其在此处作为变量名称的用法冲突。cos(al71)必须要定义al71才行。而且cosal71也不行，会差生矛盾。
6.如何求矩阵各个元素绝对值相加之和，目前学到的只能老笨方法进行求和。注意不能使用abs（矩阵）的形式。
7.P6orig_f=P_tool_f-P_tool_6;计算P6orig时对于坐标系的体会不够深刻。不同坐标系下坐标转换要将坐标转移到同一个坐标系下才可以相加减。P6orig_f=P_tool_f-P_tool_6(1,1)*a67_f-P_tool_6(2,1)*cross(S6_f,a67_f)-P_tool_6(3,1)*S6_f;
8.matlab中的函数使用是先定义后调用，即将function后面的语句全部挪到开头。否则会出现 错误: 脚本中的函数定义必须出现在文件的结尾。 请将 “close_loop” 函数定义后面的所有语句都移到第一个局部函数定义前面。
具体函数
function
[a71
,s7
,s1
,al71
,th7
,gam1
]=close_loop(P_tool_6
,P_tool_f
,S6_f
,a67_f
)
%1.caculate S7_f unit vectoe
b
=cross(a67_f
,S6_f
);
b1
=sqrt(b(1,1)^2+b(2,1)^2+b(3,1)^2);
S7_f
=b
/b1
;
S1_f
=[0;0;1];
Xf_f
=[1;0;0];
%2.caculate al71
a
=cross(S7_f
,S1_f
); % cos(al71
)=dot(S7_f
,S1_f
);% sin(al71
)=dot(cross(S7_f
,S1_f
),a71_f
);% al71
=atan(sin(al71
)/cos(al71
));
absolute
=sqrt(a(1,1)^2+a(2,1)^2+a(3,1)^2);
a71_f
=cross(S7_f
,S1_f
)/absolute
;
cal71
=dot(S7_f
,S1_f
);
sal71
=dot(a
,a71_f
);
al71
=atan(sal71
/cal71
);
al71
=al71
*180/pi
;
if al71
<0
al71
=180+al71
;
end
if cal71~
=1 && cal71~
=-1
cth7
=dot(a67_f
,a71_f
);
sth7
=dot(cross(a67_f
,a71_f
),S7_f
);
th7
=atan(sth7
/cth7
);
th7
=th7
*180/pi
;
cgam1
=dot(a71_f
,Xf_f
);
sgam1
=dot(cross(a71_f
,Xf_f
),S1_f
);
gam1
=atan(sgam1
/cgam1
);
gam1
=gam1
*180/pi
;
P6orig_f
=P_tool_f
-P_tool_6(1,1)*a67_f
-P_tool_6(2,1)*cross(S6_f
,a67_f
)-P_tool_6(3,1)*S6_f
;
s7
=dot(cross(S1_f
,P6orig_f
),a71_f
)/sal71
;
a71
=dot(cross(P6orig_f
,S1_f
),S7_f
)/sal71
;
s1
=dot(cross(P6orig_f
,S7_f
),a71_f
)/sal71
;
end
if cal71
==1 || cal71
==-1
s7
=0;
s1
=-dot(P6orig_f
,S1_f
);
aa71
=-(P6orig_f
+s1
*S1_f
);
a71
=sqrt(aa71(1,1)^2+aa71(2,1)^2+aa71(3,1)^2);%a71
=abs(-(P6orig_f
+s1
*S1_f
));%ssssssssss
if a71
==0
th7
=0;
cgam1
=dot(a71_f
,Xf_f
);
sgam1
=dot(cross(a71_f
,Xf_f
),S1_f
);
gam1
=atan(sgam1
/cgam1
);
end
if a71~
=0
P6orig_f
=P_tool_f
-P_tool_6(1,1)*a67_f
-P_tool_6(2,1)*cross(S6_f
,a67_f
)-P_tool_6(3,1)*S6_f
;
a71_f
=-(P6orig
+s1
*S1_f
)/a71
;
cth7
=dot(a67_f
,a71_f
);
sth7
=dot(cross(a67_f
,a71_f
),S7_f
);
th7
=atan(sth7
/cth7
);
cgam1
=dot(a71_f
,Xf_f
);
sgam1
=dot(cross(a71_f
,Xf_f
),S1_f
);
gam1
=atan(sgam1
/cgam1
);
end
end
具体例子验证结果正确
