一、ikine (SerialLink)
SerialLink/ikine Inverse kinematics by optimization without joint limits串连/ikine无关节限制优化逆运动学
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.Q=R.ikine（T）是与机器人相对应的关节坐标（1xN）
作为SE3对象或同质变换的末端效应器姿势T
矩阵（4x4），并且N是机器人关节的数量。
This method can be used for robots with any number of degrees of freedom.这种方法可以用于任何自由度的机器人。
Options::
'ilimit',L maximum number of iterations (default 500)最大迭代次数（默认为500）
'rlimit',L maximum number of consecutive step rejections (default 100)连续步骤拒绝的最大次数（默认为100）
'tol',T final error tolerance (default 1e-10)最终误差容限（默认值1e-10）
'lambda',L initial value of lambda (default 0.1)lambda的初始值（默认值0.1）
'lambdamin',M minimum allowable value of lambda (default 0)lambda的最小允许值（默认值为0）
'quiet' be quiet
'verbose' be verbose
'mask',M mask vector (6x1) that correspond to translation in X, Y and Z,
and rotation about X, Y and Z respectively.
'q0',Q initial joint configuration (default all zeros)初始关节配置（默认为全零）
'search' search over all configurations搜索所有配置
'slimit',L 最大搜索尝试次数（默认为100）
'transpose',A use Jacobian transpose with step size A, rather than Levenberg-Marquadt使用步长为A的雅可比转置，而不是Levenberg-Marquadt
Trajectory operation::轨迹操作：
In all cases if T is a vector of SE3 objects (1xM) or a homogeneous
transform sequence (4x4xM) then returns the joint coordinates
corresponding to each of the transforms in the sequence. Q is MxN where
N is the number of robot joints. The initial estimate of Q for each time
step is taken as the solution from the previous time step.
在所有情况下，如果T是SE3对象的向量（1xM）或齐次
变换序列（4x4xM）然后返回关节坐标
对应于序列中的每个变换。Q是MxN，其中
N是机器人关节的数量。每次Q的初始估计值
步骤作为上一时间步骤的解决方案。
Underactuated robots::欠驱动机器人：
For the case where the manipulator has fewer than 6 DOF the solution
space has more dimensions than can be spanned by the manipulator joint
coordinates.
In this case we specify the 'mask' option where the mask
vector (1x6) specifies the Cartesian DOF (in the wrist coordinate
frame) that will be ignored in reaching a solution. The mask vector
has six elements that correspond to translation in X, Y and Z, and rotation
about X, Y and Z respectively. The value should be 0 (for ignore) or 1.
The number of non-zero elements should equal the number of manipulator DOF.
For example when using a 3 DOF manipulator rotation orientation might be
unimportant in which case use the option: 'mask', [1 1 1 0 0 0].
For robots with 4 or 5 DOF this method is very difficult to use since
orientation is specified by T in world coordinates and the achievable
orientations are a function of the tool position.
对于操纵器的自由度小于6的情况，解决方案
空间的尺寸大于操纵器关节所能跨越的尺寸
坐标。
在这种情况下，我们指定“mask”选项
矢量（1x6）指定笛卡尔自由度（在手腕坐标中
帧），其在达成解决方案时将被忽略。掩码矢量
有六个元素，分别对应于X、Y和Z方向的平移和旋转
分别约为X、Y和Z。该值应为0（表示忽略）或1。
非零元素的数量应等于操纵器DOF的数量。
例如，当使用3自由度操纵器时，旋转方向可能是
不重要，在这种情况下使用选项：“mask”，[1 1 1 0 0 0]。
对于具有4或5自由度的机器人，这种方法很难使用，因为
方向由世界坐标中的T指定
方向是工具位置的函数。
References::
- Robotics, Vision & Control, P. Corke, Springer 2011, Section 8.4.
Notes::
- This has been completely reimplemented in RTB 9.11
- Does NOT require MATLAB Optimization Toolbox.
- Solution is computed iteratively.
- Implements a Levenberg-Marquadt variable step size solver.
- The tolerance is computed on the norm of the error between current
and desired tool pose. This norm is computed from distances
and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess Q0 (defaults to 0).
- The default value of Q0 is zero which is a poor choice for most
manipulators (eg. puma560, twolink) since it corresponds to a kinematic
singularity.
- Such a solution is completely general, though much less efficient
than specific inverse kinematic solutions derived symbolically, like
ikine6s or ikine3.
- This approach allows a solution to be obtained at a singularity, but
the joint angles within the null space are arbitrarily assigned.
- Joint offsets, if defined, are added to the inverse kinematics to
generate Q.
- Joint limits are not considered in this solution.
- The 'search' option peforms a brute-force search with initial conditions
chosen from the entire configuration space.
- If the 'search' option is used any prismatic joint must have joint
limits defined.
参考文献：
-机器人、视觉与控制，P.Corke，Springer 2011，第8.4节。
备注：
-RTB 9.11中已完全重新实施
-不需要MATLAB优化工具箱。
-迭代计算解决方案。
-实现Levenberg-Marquadt可变步长求解器。
-公差是根据电流之间的误差范数计算的
以及所需的工具姿态。这个范数是根据距离计算的
以及没有任何加权的角度。
-运动学逆解通常不是唯一的，并且
取决于初始猜测Q0（默认为0）。
-Q0的默认值为零，这对大多数人来说是一个糟糕的选择
机械手（例如puma560，双连杆），因为它对应于运动学
奇异性。
-这样的解决方案是完全通用的，尽管效率要低得多
而不是象征性地推导出的特定逆运动学解，比如
ikine6s或ikine3。
-这种方法允许在奇点处获得解，但是
空空间内的关节角度是任意分配的。
-关节偏移（如果已定义）将添加到反向运动学中，以
生成Q。
-本解决方案不考虑联合限制。
-“搜索”选项使用初始条件形成强力搜索
从整个配置空间中选择。
-如果使用“搜索”选项，则任何棱柱形接头都必须具有接头
定义的限制。
二、ikcon
%SerialLink.IKCON Inverse kinematics by optimization with joint limits
%
% Q = R.ikcon(T, OPTIONS) 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. OPTIONS is
% an optional list of name/value pairs than can be passed to fmincon.
%
% Q = robot.ikunc(T, Q0, OPTIONS) as above but specify the
% initial joint coordinates Q0 used for the minimisation.
%
% [Q,ERR] = robot.ikcon(T, ...) as above but also returns ERR which is the
% scalar final value of the objective function.
%
% [Q,ERR,EXITFLAG] = robot.ikcon(T, ...) as above but also returns the
% status EXITFLAG from fmincon.
%
% [Q,ERR,EXITFLAG,OUTPUT] = robot.ikcon(T, ...) as above but also returns
% a structure with information such as total number of iterations, and final
% objective function value. See the documentation of fmincon for a complete list.
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous transform
% sequence (4x4xM) then returns the joint coordinates corresponding to
% each of the transforms in the sequence. Q is MxN where N is the number
% of robot joints. The initial estimate of Q for each time step is taken as
% the solution from the previous time step.
%
% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation
% for the corresponding trajectory step.
%
% Notes::
% - Requires fmincon from the MATLAB Optimization Toolbox.
% - Joint limits are considered in this solution.
% - Can be used for robots with arbitrary degrees of freedom.
% - In the case of multiple feasible solutions, the solution returned
% depends on the initial choice of Q0.
% - Works by minimizing the error between the forward kinematics of the
% joint angle solution and the end-effector frame as an optimisation.
% The objective function (error) is described as:
% sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
% Where omega is some gain matrix, currently not modifiable.
%SerialLink.IKCON关节极限优化逆运动学
%
%Q=R.ikcon（T，OPTIONS）是与
%机器人末端执行器姿势T是SE3对象或同质
%变换矩阵（4x4），并且N是机器人关节的数量。OPTIONS
%可传递给fmincon的名称/值对的可选列表。
%
%Q=robot.ikunc（T，Q0，OPTIONS）如上所述，但指定
%用于最小化的初始关节坐标Q0。
%
%[Q，ERR]=robot.ikcon（T，…），但也返回ERR，ERR是
%目标函数的标量最终值。
%
%[Q，ERR，EXITFLAG]=robot.ikcon（T，…），但也返回
%fmincon中的状态EXITFLAG。
%
%[Q，ERR，EXITFLAG，OUTPUT]=robot.ikcon（T，…），如上所述，但也返回
%具有诸如迭代总数和最终次数等信息的结构
%目标函数值。有关完整列表，请参阅fmincon的文档。
%
%轨迹操作：
%
%在所有情况下，如果T是SE3对象的向量（1xM）或齐次变换
%序列（4x4xM）然后返回与
%序列中的每个变换。Q是MxN，其中N是数字
%机器人关节。每个时间步长的Q的初始估计值取为
%上一时间步骤的解决方案。
%
%ERR和EXITFLAG也是Mx1，表示优化结果
%用于对应的轨迹步骤。
%
%备注：
%-需要MATLAB优化工具箱中的fmincon。
%-本解决方案考虑了联合限制。
%-可用于任意自由度的机器人。
%-如果存在多个可行的解决方案，则返回解决方案
%取决于Q0的初始选择。
%-通过最小化
%关节角度解决方案和末端执行器框架作为优化。
%目标函数（误差）描述为：
%sumsqr（（inv（T）*robot.fkine（q）-眼睛（4））*omega）
%其中omega是某个增益矩阵，目前不可修改。
三、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
%SerialLink.ikine6s分析反向运动学
%
%Q=R.ikine（T）是与机器人相对应的关节坐标（1xN）
%作为SE3对象或同质变换的末端效应器姿势T
%矩阵（4x4），并且N是机器人关节的数量。这是一个分析
%具有球形手腕（最常见的形式）的6轴机器人的解决方案
%用于工业机械臂）。
%
%如果T表示轨迹（4x4xM），则反向运动学为
%为所有M个姿势计算，得到Q（MxN），每行表示
%相应姿势下的关节角度。
%
%Q=R.IKINE6S（T，CONFIG）如上所述，但在
%字符串的形式包含一个或多个配置代码：
%
%'l'手臂向左（默认）
%右臂
%“u”肘部向上（默认值）
%肘部朝下
%“n”手腕未翻转（默认设置）
%“f”手腕翻转（旋转180度）
%
%轨迹操作：
%
%在所有情况下，如果T是SE3对象的向量（1xM）或齐次
%变换序列（4x4xM），然后R.ikcon（）返回关节坐标
%对应于序列中的每个变换。
%
%备注：
%-治疗一些特定病例：
%-无肩部偏移的机器人
%-肩部偏移的机器人（具有向左/向右配置）
%-具有肩部偏移和棱柱状第三关节的机器人（如斯坦福手臂）
%-彪马560手臂带有肩部和肘部偏移（4个长度参数）
%-具有许多偏移的Kuka KR5（7个长度参数）
%-使用ikine_sym确定的各种情况下的反向运动学。
%-运动学逆解通常不是唯一的，并且
%取决于配置字符串。
%-关节偏移（如果已定义）将添加到反向运动学中，以
%生成Q。
%-仅适用于标准Denavit-Hartenberg参数
四、 ikunc
%SerialLink.IKUNC Inverse manipulator by optimization without joint limits
%
% Q = R.ikunc(T, OPTIONS) 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. OPTIONS is
% an optional list of name/value pairs than can be passed to fminunc.
%
% Q = robot.ikunc(T, Q0, OPTIONS) as above but specify the
% initial joint coordinates Q0 used for the minimisation.
%
% [Q,ERR] = robot.ikunc(T,...) as above but also returns ERR which is the
% scalar final value of the objective function.
%
% [Q,ERR,EXITFLAG] = robot.ikunc(T,...) as above but also returns the
% status EXITFLAG from fminunc.
%
% [Q,ERR,EXITFLAG,OUTPUT] = robot.ikunc(T,...) as above but also returns the
% structure OUTPUT from fminunc which contains details about the optimization.
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous transform
% sequence (4x4xM) then returns the joint coordinates corresponding to
% each of the transforms in the sequence. Q is MxN where N is the number
% of robot joints. The initial estimate of Q for each time step is taken as
% the solution from the previous time step.
%
% ERR and EXITFLAG are also Mx1 and indicate the results of optimisation
% for the corresponding trajectory step.
%
% Notes::
% - Requires fminunc from the MATLAB Optimization Toolbox.
% - Joint limits are not considered in this solution.
% - Can be used for robots with arbitrary degrees of freedom.
% - In the case of multiple feasible solutions, the solution returned
% depends on the initial choice of Q0
% - Works by minimizing the error between the forward kinematics of the
% joint angle solution and the end-effector frame as an optimisation.
% The objective function (error) is described as:
% sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
% Where omega is some gain matrix, currently not modifiable.
五、ikine3
%SerialLink.ikine3 Inverse kinematics for 3-axis robot with no wrist
%
% Q = R.ikine3(T) is the joint coordinates (1x3) corresponding to the robot
% end-effector pose T represented by the homogenenous transform. This
% is a analytic solution for a 3-axis robot (such as the first three joints
% of a robot like the Puma 560).
%
% Q = R.ikine3(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
%
% Notes::
% - The same as IKINE6S without the wrist.
% - 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.
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous
% transform sequence (4x4xM) then returns the joint coordinates
% corresponding to each of the transforms in the sequence. Q is Mx3.
%
% Reference::
%
% Inverse kinematics for a PUMA 560 based on the equations by Paul and Zhang
% From The International Journal of Robotics Research
% Vol. 5, No. 2, Summer 1986, p. 32-44
六、ikine_sym
%IKINE_SYM Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT. The cells of Q represent the
% solutions for each joint, ie. Q{1} is the solution for joint 1. A
% cell may contain an array of solutions. The solution is expressed in terms
% of other joint angles and elements of the desired end-point pose which is
% represented by the symbolic matrix (3x4) with elements
% nx ox ax tx
% ny oy ay ty
% nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N is the number of joint angles solved for.
%
% Options::
%
% 'file',F Write the solution to an m-file named F
% 'Tpost',T Add a symbolic 4x4 matrix T to the end of the chain
%
% Example::
%
% mdl_planar2
% sol = p2.ikine_sym(2);
% length(sol)
%
% q1 = sol{1} % are the solution for joint 1
% q2 = sol{2} % is the solution for joint 2
% length(q1)
% ans =
% 2 % there are 2 solutions for this joint
% q1(1) % one solution for q1
% q1(2); % the other solution for q1
%
% Notes::
% - ignores tool and base transforms.
%
% References::
% - Robot manipulators: mathematics, programming and control
% Richard Paul, MIT Press, 1981.
% - The kinematics of manipulators under computer control,
% D.L. Pieper, Stanford report AI 72, October 1968.
%
% Notes::
% - Requires the MATLAB Symbolic Math Toolbox.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.
七、ikinem
%SerialLink.IKINEM Numerical inverse kinematics by minimization
%
% Q = R.ikinem(T) is the joint coordinates corresponding to the robot
% end-effector pose T which is a homogenenous transform.
%
% Q = R.ikinem(T, Q0, OPTIONS) specifies the initial estimate of the joint
% coordinates.
%
% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence
% and R.ikinem() returns the joint coordinates corresponding to each of the
% transforms in the sequence. Q is MxN where N is the number of robot joints.
% The initial estimate of Q for each time step is taken as the solution
% from the previous time step.
%
% Options::
% 'pweight',P weighting on position error norm compared to rotation
% error (default 1)
% 'stiffness',S Stiffness used to impose a smoothness contraint on joint
% angles, useful when N is large (default 0)
% 'qlimits' Enforce joint limits
% 'ilimit',L Iteration limit (default 1000)
% 'nolm' Disable Levenberg-Marquadt
%
% Notes::
% - PROTOTYPE CODE UNDER DEVELOPMENT, intended to do numerical inverse kinematics
% with joint limits
% - The inverse kinematic solution is generally not unique, and
% depends on the initial guess Q0 (defaults to 0).
% - The function to be minimized is highly nonlinear and the solution is
% often trapped in a local minimum, adjust Q0 if this happens.
% - The default value of Q0 is zero which is a poor choice for most
% manipulators (eg. puma560, twolink) since it corresponds to a kinematic
% singularity.
% - Such a solution is completely general, though much less efficient
% than specific inverse kinematic solutions derived symbolically, like
% ikine6s or ikine3.% - Uses Levenberg-Marquadt minimizer LMFsolve if it can be found,
% if 'nolm' is not given, and 'qlimits' false
% - The error function to be minimized is computed on the norm of the error
% between current and desired tool pose. This norm is computed from distances
% and angles and 'pweight' can be used to scale the position error norm to
% be congruent with rotation error norm.
% - This approach allows a solution to obtained at a singularity, but
% the joint angles within the null space are arbitrarily assigned.
% - Joint offsets, if defined, are added to the inverse kinematics to
% generate Q.
% - Joint limits become explicit contraints if 'qlimits' is set.
%
% See also fminsearch, fmincon, SerialLink.fkine, SerialLink.ikine, tr2angvec.