Matlab机器人工具箱（3-1）：五自由度机械臂（正逆运动学）
01 正运动学：DH表示法
1955年, Denavit和Hartenberg在“ASME Journal of Applied Mechanic”发表了一篇论文,这篇论文介绍了一种机器人表示和建模的方法,并导出了它们的运动方程,目前已成为机器人表示表示和机器人运动建模的标准方法
Denavit-Hartenberg（D-H）模型表示了对机器人连杆和关节进行建模的一种非常简单的方法,可用于任何机器人构型,而不管机器人的结构顺序和复杂程度。
-
Link()
参数可从DH参数表获得
sigma 为0，表示转动关节
offset 表示初始角度
modified 表示采用改进D-H法
qlim() 设定关节运动范围 -
SerialLink()
连接机械臂各杆件，并且返回机械臂名字，供后续使用 -
plot()
显示出机械臂位置。姿态 -
teach()
示教 -
tool()指定工具端长度 -
base()指定基座位置
说明：
- 位姿矩阵保存为se3格式，与（4×4）矩阵的替换方法为：
（若A1为前者，A2为后者）
A2=A1.T;
A1=SE3(A2); - 工具端和基座都可以通过 ``transl```确定，
工具端是 右乘 ， 基座是 左乘 L(2).A(pi/3)
获得pi/3时，连杆2的转换矩阵
1.1 建立机械臂模型
% mdl_5dof.m
% 5自由度机械臂-单臂结构参数
d=[ 0, 0, 0, 0, 0];
a=[ 0, 13, 233.24, 175.64, 0];
alpha=[ 0, pi/2, 0, 0, pi/2];
%使用offset
L(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1),'modified');
L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2),'offset',pi/2,'modified');
L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3),'modified');
L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4),'offset',pi/2,'modified');
L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5),'modified');
du=pi/180;
ra=180/pi;
%定义关节范围
L(1).qlim =[-170, 170]*du;
L(2).qlim =[60-70, 60+70]*du;%-10,130
L(3).qlim =[-70-70,-70+70]*du;%-140,0
L(4).qlim =[-70,70]*du;
L(5).qlim =[-170, 170]*du;
bot=SerialLink(L,'name','五自由度机械臂');
%bot.tool= transl(0, 0, tool)
% bot.teach()
% bot.plot([0 0 0 0 0])
1.2 计算位姿矩阵
simplify()
化简
% 总转换矩阵计算
% v10
clc;
clear;
syms th1 th2 th3 th4 th5;
syms a1 a2 a3 a4 a5;
syms d1 d2 d3 d4 d5;
syms alp1 alp2 alp3 alp4 alp5;
d=[ 0, 0, 0, 0, 0];
a=[ 0, 13, 233.24, 175.64, 0];
alpha=[ 0, pi/2, 0, 0, pi/2];
% theta d a alpha
% L(1) = Link([0 d1 a1 alp1 th1],'modified');
% L(2) = Link([0 d2 a2 alp2 th2],'modified');
% L(3) = Link([0 d3 a3 alp3 th3],'modified');
% L(4) = Link([0 d4 a4 alp4 th4],'modified');
% L(5) = Link([0 d5 a5 alp5 th5],'modified');
[d1,d2,d3,d4,d5]=deal(0);
[alp1,alp2,alp3,alp4,alp5]=deal(alpha(1),alpha(2),alpha(3),alpha(4),alpha(5));
a1=0;
L(1) = Link([th1 d1 a1 alp1 0],'modified');
L(2) = Link([th2 d2 a2 alp2 0],'modified');
L(3) = Link([th3 d3 a3 alp3 0],'modified');
L(4) = Link([th4 d4 a4 alp4 0],'modified');
L(5) = Link([th5 d5 a5 alp5 0],'modified');
bot=SerialLink(L,'name','my robot');
%计算总转换矩阵
T=L(1).A(th1)*L(2).A(th2)*L(3).A(th3)*L(4).A(th4)*L(5).A(th5)
%简化
simplify(T)
02 逆运动学
逆运动学可以分为 迭代法 和 解析法（封闭解）
求封闭解有两个充分条件：
- 三个相邻关节轴相交与一点
- 三个相邻关节轴平行
MATLAB机器人工具箱提供了两种逆运动学函数：
ikine6s()
6轴机械臂的封闭解法ikine()
机械臂的迭代解法
默认是六轴
可以用于非六轴的情况
eg:
对于这个五轴机械臂
q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1);
其中
mask的“1”标记有那些关节
q0表示迭代的初始位置是那里（影响迭代速度和正确性）
迭代法往往计算速度比较慢，所以需要计算五轴机械臂封闭解
封闭解的计算方法，关键就是比较矩阵中每个位置，具体可以参考一个硕士论文《协作机器人零力控制与碰撞检测技术研究》
2.1 五轴机械臂封闭解
对比矩阵1、2
按照
theta1 → theta5→ theta2→ (theta2+theta3)→ theta3→ (theta2+theta3+theta4) → theta4
的顺序计算
theta1 和 theta5 的计算方法如下图
注意求解过程一定要使用atan2()
不要用arctan()，arcsin()之类的
以此结果得到逆运动学函数（共四组解）：
% 逆运动学求解函数
% 输入 机器人名称，转换矩阵（SE3）
% 输出关节角度
% 说明：函数执行过程中打印4组解（deg），目前函数返回第三组解（rad）
function [ th ] = MDH_IK(robot,T)
TT=SE3(T);%矩阵转换为SE3格式
T = inv(robot.base) * TT * inv(robot.tool);
%对于多个连续的问题，可设置k
% T = inv(robot.base) * TT(k) * inv(robot.tool);
%将函数变量导入
% T是SE3类型函数，所以不能用下列写法(也可以转换为T=T.T）
% [nx ny nz]=deal(T(1,1),T(2,1),T(3,1));
% [ox oy oz]=deal(T(1,2),T(2,2),T(3,2));
% [ax ay az]=deal(T(1,3),T(2,3),T(3,3));
% [px py pz]=deal(T(1,4),T(2,4),T(3,4));
n=T.n;
o=T.o;
a=T.a;
p=T.t;
[nx ny nz]=deal(n(1),n(2),n(3));
[ox oy oz]=deal(o(1),o(2),o(3));
[ax ay az]=deal(a(1),a(2),a(3));
[px py pz]=deal(p(1),p(2),p(3));
% 结构参数
syms theta1 theta2 theta3 theta4 theta5 ;
syms a1 a2 a3 d5 ;
%alpha,a,d,theta
DH_Tab =[ 0 0 0 0;
90 13 0 0;
0 233.24 0 0;
0 175.64 0 0;
90 0 0 0];
%参数赋值
a0=robot.a(1);a1=robot.a(2);a2=robot.a(3);a3=robot.a(4);a4=robot.a(5);
%角度转弧度
% DH_Tab(1:5,1)=DH_Tab(1:5,1)/180*pi;
% alp0=DH_Tab(1,1); alp1=DH_Tab(2,1);alp2=DH_Tab(3,1);alp3=DH_Tab(4,1);alp4=DH_Tab(5,1);
%% 求解过程1
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_1;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2=theta2_1;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的
if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th1=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th1(j) = th1(j) - robot.offset(j);
end
%输出
th1=th1*180/pi
%% 求解过程2
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_2;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2=theta2_1;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的
if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th2=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th2(j) = th2(j) - robot.offset(j);
end
%输出
th2=th2*180/pi
%% 求解过程3
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_1;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2=theta2_2;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的
if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th3=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th3(j) = th3(j) - robot.offset(j);
end
%输出
th3=th3*180/pi
%% 求解过程4
theta1_1=atan2(-py,-px);
theta1_2=atan2(py,px);
theta1=theta1_2;
theta5=atan2(nx*sin(theta1)-ny*cos(theta1),ox*sin(theta1)-oy*cos(theta1));
theta234=atan2(nz,nx*cos(theta1)+ny*sin(theta1));
A=px*cos(theta1)+py*sin(theta1)-a1;
B=pz;
theta2_1=atan2(A^2+B^2+a2^2-a3^2,sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2_2=atan2(A^2+B^2+a2^2-a3^2,-sqrt(4*a2^2*(A^2+B^2)-(A^2+B^2+a2^2-a3^2)^2))-atan2(A,B);
theta2=theta2_2;
theta23=atan2(pz-a2*sin(theta2),px*cos(theta1)+py*sin(theta1)-a1-a2*cos(theta2));
theta3=theta23-theta2;
theta4=theta234-theta23;%theta4不一样只是一种表示而已，差别360°，就是转一圈，位置是一样的
if(theta4>pi)
theta4=theta4-2*pi;
end
if(theta4<-pi)
theta4=theta4+2*pi;
end
th4=[theta1,theta2,theta3,theta4,theta5];
for j=1:robot.n
th4(j) = th4(j) - robot.offset(j);
end
%输出
th4=th4*180/pi
%% 函数输出，转换为rad形式
th=th3*pi/180;
end
2.2 ikine6s源码
%SerialLink.ikine6s Analytical inverse kinematics
%
% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot
% end-effector pose T which is an SE3 object or homogenenous transform
% matrix (4x4), and N is the number of robot joints. This is a analytic
% solution for a 6-axis robot with a spherical wrist (the most common form
% for industrial robot arms).
%
% If T represents a trajectory (4x4xM) then the inverse kinematics is
% computed for all M poses resulting in Q (MxN) with each row representing
% the joint angles at the corresponding pose.
%
% Q = R.IKINE6S(T, CONFIG) as above but specifies the configuration of the arm in
% the form of a string containing one or more of the configuration codes:
%
% 'l' arm to the left (default)
% 'r' arm to the right
% 'u' elbow up (default)
% 'd' elbow down
% 'n' wrist not flipped (default)
% 'f' wrist flipped (rotated by 180 deg)
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous
% transform sequence (4x4xM) then R.ikcon() returns the joint coordinates
% corresponding to each of the transforms in the sequence.
%
% Notes::
% - Treats a number of specific cases:
% - Robot with no shoulder offset
% - Robot with a shoulder offset (has lefty/righty configuration)
% - Robot with a shoulder offset and a prismatic third joint (like Stanford arm)
% - The Puma 560 arms with shoulder and elbow offsets (4 lengths parameters)
% - The Kuka KR5 with many offsets (7 length parameters)
% - The inverse kinematics for the various cases determined using ikine_sym.
% - The inverse kinematic solution is generally not unique, and
% depends on the configuration string.
% - Joint offsets, if defined, are added to the inverse kinematics to
% generate Q.
% - Only applicable for standard Denavit-Hartenberg parameters
%
% Reference::
% - Inverse kinematics for a PUMA 560,
% Paul and Zhang,
% The International Journal of Robotics Research,
% Vol. 5, No. 2, Summer 1986, p. 32-44
%
% Author::
% - The Puma560 case: Robert Biro with Gary Von McMurray,
% GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
% - Kuka KR5 case: Gautam Sinha,
% Autobirdz Systems Pvt. Ltd., SIDBI Office,
% Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh.
%
% See also SerialLink.fkine, SerialLink.ikine, SerialLink.ikine_sym.
function thetavec = ikine6s(robot, TT, varargin)
if robot.mdh ~= 0
error('RTB:ikine:notsupported','Solution only applicable for standard DH conventions');
end
if robot.n ~= 6
error('RTB:ikine:notsupported','Solution only applicable for 6-axis robot');
end
%
% % recurse over all poses in a trajectory
% if ndims(T) == 3
% theta = zeros(size(T,3),robot.n);
% for k=1:size(T,3)
% theta(k,:) = ikine6s(robot, T(:,:,k), varargin{:});
% end
% return;
% end
%
%
% if ~ishomog(T)
% error('RTB:ikine:badarg', 'T is not a homog xform');
% end
TT = SE3(TT);
L = robot.links;
if ~robot.isspherical()
error('RTB:ikine:notsupported', 'wrist is not spherical');
end
% The configuration parameter determines what n1,n2,n4 values are used
% and how many solutions are determined which have values of -1 or +1.
if nargin < 3
configuration = '';
else
configuration = lower(varargin{1});
end
% default configuration
sol = [1 1 1]; % left, up, noflip
for c=configuration
switch c
case 'l'
sol(1) = 1;
case 'r'
sol(1) = 2;
case 'u'
sol(2) = 1;
case 'd'
sol(2) = 2;
case 'n'
sol(3) = 1;
case 'f'
sol(3) = 2;
end
end
% determine the arm structure and the relevant solution to use
if isempty(robot.ikineType)
if is_simple(L)
robot.ikineType = 'nooffset';
elseif is_puma(L)
robot.ikineType = 'puma';
elseif is_offset(L)
robot.ikineType = 'offset';
elseif is_rrp(L)
robot.ikineType = 'rrp';
else
error('RTB:ikine6s:badarg', 'This kinematic structure not supported');
end
end
for k=1:length(TT)
% undo base and tool transformations
T = inv(robot.base) * TT(k) * inv(robot.tool);
% drop back to matrix form
T = T.T;
%% now solve for the first 3 joints, based on position of the spherical wrist centre
switch robot.ikineType
case 'puma'
% Puma model with shoulder and elbow offsets
%
% - Inverse kinematics for a PUMA 560,
% Paul and Zhang,
% The International Journal of Robotics Research,
% Vol. 5, No. 2, Summer 1986, p. 32-44
%
% Author::
% Robert Biro with Gary Von McMurray,
% GTRI/ATRP/IIMB,
% Georgia Institute of Technology
% 2/13/95
a2 = L(2).a;
a3 = L(3).a;
d1 = L(1).d;
d3 = L(3).d;
d4 = L(4).d;
% The following parameters are extracted from the Homogeneous
% Transformation as defined in equation 1, p. 34
Ox = T(1,2);
Oy = T(2,2);
Oz = T(3,2);
Ax = T(1,3);
Ay = T(2,3);
Az = T(3,3);
Px = T(1,4);
Py = T(2,4);
Pz = T(3,4) - d1;
%
% Solve for theta(1)
%
% r is defined in equation 38, p. 39.
% theta(1) uses equations 40 and 41, p.39,
% based on the configuration parameter n1
%
r = sqrt(Px^2 + Py^2);
if sol(1) == 1
theta(1) = atan2(Py,Px) + pi - asin(d3/r);
else
theta(1) = atan2(Py,Px) + asin(d3/r);
end
%
% Solve for theta(2)
%
% V114 is defined in equation 43, p.39.
% r is defined in equation 47, p.39.
% Psi is defined in equation 49, p.40.
% theta(2) uses equations 50 and 51, p.40, based on the configuration
% parameter n2
%
if sol(2) == 1
n2 = -1;
else
n2 = 1;
end
if sol(1) == 2
n2 = -n2;
end
V114 = Px*cos(theta(1)) + Py*sin(theta(1));
r = sqrt(V114^2 + Pz^2);
Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
if ~isreal(Psi)
theta = [];
else
theta(2) = atan2(Pz,V114) + n2*Psi;
%
% Solve for theta(3)
%
% theta(3) uses equation 57, p. 40.
%
num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
den = cos(theta(2))*Pz - sin(theta(2))*V114;
theta(3) = atan2(a3,d4) - atan2(num, den);
end
case 'nooffset'
a2 = L(2).a;
a3 = L(3).a;
d1 = L(1).d;
px = T(1,4); py = T(2,4); pz = T(3,4);
%%% autogenerated code
if L(1).alpha < 0
if sol(1) == 1
q(1) = angle(-px-py*1i);
else
q(1) = angle(px+py*1i);
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
else
q(2) = -angle(a2*d1*-2.0+a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
end
S2 = sin(q(2));
C2 = cos(q(2));
if sol(3) == 1
q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
else
q(3) = -angle(a2*-1i+C2*d1-C2*pz+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2+S2*d1-S2*pz+C1*C2*px+C2*S1*py)^2+(-C2*d1+C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
end
else
if sol(1) == 1
q(1) = angle(px+py*1i);
else
q(1) = angle(-px-py*1i);
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i-sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
else
q(2) = -angle(a2*d1*2.0-a2*pz*2.0-C1*a2*px*2.0i-S1*a2*py*2.0i)+angle(d1*pz*2.0i-a2^2*1i+a3^2*1i-d1^2*1i-pz^2*1i-C1^2*px^2*1i+sqrt((a2*d1*2.0-a2*pz*2.0)^2+(C1*a2*px*2.0+S1*a2*py*2.0)^2-(d1*pz*-2.0+a2^2-a3^2+d1^2+pz^2+C1^2*px^2+S1^2*py^2+C1*S1*px*py*2.0)^2)-S1^2*py^2*1i-C1*S1*px*py*2.0i);
end
S2 = sin(q(2));
C2 = cos(q(2));
if sol(3) == 1
q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i-sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
else
q(3) = -angle(a2*-1i-C2*d1+C2*pz-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt((-a2-S2*d1+S2*pz+C1*C2*px+C2*S1*py)^2+(C2*d1-C2*pz+C1*S2*px+S1*S2*py)^2-a3^2));
end
end
theta(1:3) = q;
case'offset'
% general case with 6 length parameters
a1 = L(1).a;
a2 = L(2).a;
a3 = L(3).a;
d1 = L(1).d;
d2 = L(2).d;
d3 = L(3).d;
px = T(1,4); py = T(2,4); pz = T(3,4);
%%% autogenerated code
if L(1).alpha < 0
if sol(1) == 1
q(1) = -angle(-px+py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
else
q(1) = angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2))-angle(-px+py*1i);
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
else
q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i+d1-pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
end
S2 = sin(q(2));
C2 = cos(q(2));
if sol(3) == 1
q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
else
q(3) = -angle(a2*-1i-C2*a1*1i+C2*d1-C2*pz+S2*a1+S2*d1*1i-S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0-S2*a2*d1*2.0-S1*a1*py*2.0+S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
end
else
if sol(1) == 1
q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i-sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
else
q(1) = -angle(px-py*1i)+angle(d2*1i+d3*1i+sqrt(d2*d3*-2.0-d2^2-d3^2+px^2+py^2));
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = angle(d1*pz*2.0i-sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
else
q(2) = angle(d1*pz*2.0i+sqrt(d1*pz^3*4.0+d1^3*pz*4.0-a1^4-a2^4-a3^4-d1^4-py^4-pz^4-C1^4*px^4+C1^2*py^4*2.0-C1^4*py^4+a1^2*a2^2*2.0+a1^2*a3^2*2.0+a2^2*a3^2*2.0-a1^2*d1^2*2.0+a2^2*d1^2*2.0+a3^2*d1^2*2.0-a1^2*py^2*6.0-a2^2*py^2*2.0+a3^2*py^2*2.0-a1^2*pz^2*2.0+a2^2*pz^2*2.0+a3^2*pz^2*2.0-d1^2*py^2*2.0-d1^2*pz^2*6.0-py^2*pz^2*2.0+d1*py^2*pz*4.0+C1^3*a1*px^3*4.0+S1^3*a1*py^3*4.0-C1^2*a1^2*px^2*6.0+C1^2*a2^2*px^2*2.0+C1^2*a3^2*px^2*2.0+C1^2*a1^2*py^2*6.0+C1^2*a2^2*py^2*1.0e1-C1^2*a3^2*py^2*2.0-C1^4*a2^2*py^2*1.2e1+C1^6*a2^2*py^2*4.0-C1^2*d1^2*px^2*2.0+C1^2*d1^2*py^2*2.0-C1^2*px^2*py^2*6.0+C1^4*px^2*py^2*6.0-C1^2*px^2*pz^2*2.0+C1^2*py^2*pz^2*2.0+S1^6*a2^2*py^2*4.0+C1*a1^3*px*4.0+S1*a1^3*py*4.0+a1^2*d1*pz*4.0-a2^2*d1*pz*4.0-a3^2*d1*pz*4.0-C1*a1*a2^2*px*4.0-C1*a1*a3^2*px*4.0-C1*S1*px^3*py*4.0+C1*a1*d1^2*px*4.0+C1*a1*px*py^2*1.2e1+C1*a1*px*pz^2*4.0+S1*a1*a2^2*py*4.0-S1*a1*a3^2*py*4.0+S1*a1*d1^2*py*4.0+S1*a1*px^2*py*4.0+S1*a1*py*pz^2*4.0-C1*S1^3*px*py^3*4.0+C1*S1^3*px^3*py*4.0-C1^3*a1*px*py^2*1.2e1-S1^3*a1*a2^2*py*8.0+C1^2*d1*px^2*pz*4.0-C1^2*d1*py^2*pz*4.0-S1^3*a1*px^2*py*4.0-C1*a1*d1*px*pz*8.0-S1*a1*d1*py*pz*8.0-C1*S1*a1^2*px*py*1.2e1-C1*S1*a2^2*px*py*4.0+C1*S1*a3^2*px*py*4.0-C1*S1*d1^2*px*py*4.0-C1*S1*px*py*pz^2*4.0-C1^2*S1*a1*a2^2*py*8.0+C1^2*S1*a1*px^2*py*8.0+C1*S1^3*a2^2*px*py*8.0+C1^3*S1*a2^2*px*py*8.0+C1*S1*d1*px*py*pz*8.0)-a1^2*1i-a2^2*1i+a3^2*1i-d1^2*1i-py^2*1i-pz^2*1i-C1^2*px^2*1i+C1^2*py^2*1i+C1*a1*px*2.0i+S1*a1*py*2.0i-C1*S1*px*py*2.0i)-angle(-a2*(a1*-1i-d1+pz+C1*px*1i+S1^3*py*1i+C1^2*S1*py*1i));
end
S2 = sin(q(2));
C2 = cos(q(2));
if sol(3) == 1
q(3) = angle(a3*1i-sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0))-angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py);
else
q(3) = -angle(a2*-1i-C2*a1*1i-C2*d1+C2*pz+S2*a1-S2*d1*1i+S2*pz*1i+C1*C2*px*1i-C1*S2*px+C2*S1*py*1i-S1*S2*py)+angle(a3*1i+sqrt(d1*pz*-2.0+a1^2+a2^2-a3^2+d1^2+py^2+pz^2+C1^2*px^2-C1^2*py^2+C2*a1*a2*2.0-C1*a1*px*2.0+S2*a2*d1*2.0-S1*a1*py*2.0-S2*a2*pz*2.0-C1*C2*a2*px*2.0-C2*S1*a2*py*2.0+C1*S1*px*py*2.0));
end
end
theta(1:3) = q;
case 'rrp'
% RRP (Stanford arm like)
px = T(1,4); py = T(2,4); pz = T(3,4);
d1 = L(1).d;
d2 = L(2).d;
%%% autogenerated code
if L(1).alpha < 0
if sol(1) == 1
q(1) = -angle(-px+py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
else
q(1) = angle(d2*1i+sqrt(-d2^2+px^2+py^2))-angle(-px+py*1i);
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = angle(d1-pz-C1*px*1i-S1*py*1i);
else
q(2) = angle(-d1+pz+C1*px*1i+S1*py*1i);
end
S2 = sin(q(2));
C2 = cos(q(2));
q(3) = -C2*d1+C2*pz+C1*S2*px+S1*S2*py;
else
if sol(1) == 1
q(1) = -angle(px-py*1i)+angle(d2*1i-sqrt(-d2^2+px^2+py^2));
else
q(1) = -angle(px-py*1i)+angle(d2*1i+sqrt(-d2^2+px^2+py^2));
end
S1 = sin(q(1));
C1 = cos(q(1));
if sol(2) == 1
q(2) = angle(-d1+pz-C1*px*1i-S1*py*1i);
else
q(2) = angle(d1-pz+C1*px*1i+S1*py*1i);
end
S2 = sin(q(2));
C2 = cos(q(2));
q(3) = -C2*d1+C2*pz-C1*S2*px-S1*S2*py;
end
theta(1:3) = q;
case 'kr5'
%Given function will calculate inverse kinematics for KUKA KR5 robot
% Equations are calculated and implemented by
% Gautam Sinha
% Autobirdz Systems Pvt. Ltd.
% SIDBI Office,
% Indian Institute of Technology Kanpur, Kanpur, Uttar Pradesh
% 208016
% India
%email- gautam.sinha705@gmail.com
% get the a1, a2 and a3-- link lenghts for link no 1,2,3
L = robot.links;
a1 = L(1).a;
a2 = L(2).a;
a3 = L(3).a;
% Check wether wrist is spherical or not
if ~robot.isspherical()
error('wrist is not spherical')
end
% get d1,d2,d3,d4---- Link offsets for link no 1,2,3,4
d1 = L(1).d;
d2 = L(2).d;
d3 = L(3).d;
d4 = L(4).d;
% Get the parameters from transformation matrix
Ox = T(1,2);
Oy = T(2,2);
Oz = T(3,2);
Ax = T(1,3);
Ay = T(2,3);
Az = T(3,3);
Px = T(1,4);
Py = T(2,4);
Pz = T(3,4);
% Set the parameters n1, n2 and n3 to get required configuration from
% solution
n1 = -1; % 'l'
n2 = -1; % 'u'
n4 = -1; % 'n'
if ~isempty(strfind(configuration, 'l'))
n1 = -1;
end
if ~isempty(strfind(configuration, 'r'))
n1 = 1;
end
if ~isempty(strfind(configuration, 'u'))
if n1 == 1
n2 = 1;
else
n2 = -1;
end
end
if ~isempty(strfind(configuration, 'd'))
if n1 == 1
n2 = -1;
else
n2 = 1;
end
end
if ~isempty(strfind(configuration, 'n'))
n4 = 1;
end
if ~isempty(strfind(configuration, 'f'))
n4 = -1;
end
% Calculation for theta(1)
r=sqrt(Px^2+Py^2);
if (n1 == 1)
theta(1)= atan2(Py,Px) + asin((d2-d3)/r);
else
theta(1)= atan2(Py,Px)+ pi - asin((d2-d3)/r);
end
% Calculation for theta(2)
X= Px*cos(theta(1)) + Py*sin(theta(1)) - a1;
r=sqrt(X^2 + (Pz-d1)^2);
Psi = acos((a2^2-d4^2-a3^2+X^2+(Pz-d1)^2)/(2.0*a2*r));
if ~isreal(Psi)
warning('RTB:ikine6s:notreachable', 'point not reachable');
theta = [NaN NaN NaN NaN NaN NaN];
return
end
theta(2) = atan2((Pz-d1),X) + n2*Psi;
% Calculation for theta(3)
Nu = cos(theta(2))*X + sin(theta(2))*(Pz-d1) - a2;
Du = sin(theta(2))*X - cos(theta(2))*(Pz-d1);
theta(3) = atan2(a3,d4) - atan2(Nu, Du);
% Calculation for theta(4)
Y = cos(theta(1))*Ax + sin(theta(1))*Ay;
M2 = sin(theta(1))*Ax - cos(theta(1))*Ay ;
M1 = ( cos(theta(2)-theta(3)) )*Y + ( sin(theta(2)-theta(3)) )*Az;
theta(4) = atan2(n4*M2,n4*M1);
% Calculation for theta(5)
Nu = -cos(theta(4))*M1 - M2*sin(theta(4));
M3 = -Az*( cos(theta(2)-theta(3)) ) + Y*( sin(theta(2)-theta(3)) );
theta(5) = atan2(Nu,M3);
% Calculation for theta(6)
Z = cos(theta(1))*Ox + sin(theta(1))*Oy;
L2 = sin(theta(1))*Ox - cos(theta(1))*Oy;
L1 = Z*( cos(theta(2)-theta(3) )) + Oz*( sin(theta(2)-theta(3)));
L3 = Z*( sin(theta(2)-theta(3) )) - Oz*( cos(theta(2)-theta(3)));
A1 = L1*cos(theta(4)) + L2*sin(theta(4));
A3 = L1*sin(theta(4)) - L2*cos(theta(4));
Nu = -A1*cos(theta(5)) - L3*sin(theta(5));
Du = -A3;
theta(6) = atan2(Nu,Du);
otherwise
error('RTB:ikine6s:badarg', 'Unknown solution type [%s]', robot.ikineType);
end
if ~isempty(theta)
% Solve for the wrist rotation
% we need to account for some random translations between the first and last 3
% joints (d4) and also d6,a6,alpha6 in the final frame.
T13 = robot.A(1:3, theta(1:3)); % transform of first 3 joints
% T = T13 * Tz(d4) * R * Tz(d6) Tx(a5)
Td4 = SE3(0, 0, L(4).d); % Tz(d4)
Tt = SE3(L(6).a, 0, L(6).d) * SE3.Rx(L(6).alpha); % Tz(d6) Tx(a5) Rx(alpha6)
R = inv(Td4) * inv(T13) * SE3(T) * inv(Tt);
% the spherical wrist implements Euler angles
if sol(3) == 1
theta(4:6) = tr2eul(R, 'flip');
else
theta(4:6) = tr2eul(R);
end
if L(4).alpha > 0
theta(5) = -theta(5);
end
% remove the link offset angles
for j=1:robot.n %#ok<*AGROW>
theta(j) = theta(j) - L(j).offset;
end
% stack the rows
thetavec(k,:) = theta;
else
warning('RTB:ikine6s:notreachable', 'point not reachable');
thetavec(k,:) = [NaN NaN NaN NaN NaN NaN];
end
end
end
% predicates to determine which kinematic solution to use
function s = is_simple(L)
alpha = [-pi/2 0 pi/2];
s = all([L(2:3).d] == 0) && ...
(all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
all([L(1:3).isrevolute] == 1) && ...
(L(1).a == 0);
end
function s = is_offset(L)
alpha = [-pi/2 0 pi/2];
s = (all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
all([L(1:3).isrevolute] == 1);
end
function s = is_rrp(L)
alpha = [-pi/2 pi/2 0];
s = all([L(2:3).a] == 0) && ...
(all([L(1:3).alpha] == alpha) || all([L(1:3).alpha] == -alpha)) && ...
all([L(1:3).isrevolute] == [1 1 0]);
end
function s = is_puma(L)
alpha = [pi/2 0 -pi/2];
s = (L(2).d == 0) && (L(1).a == 0) && ...
(L(3).d ~= 0) && (L(3).a ~= 0) && ...
all([L(1:3).alpha] == alpha) && ...
all([L(1:3).isrevolute] == 1);
end
2.3 其他
- 在工作空间内实现轨迹规划