Robotics System Toolbox学习笔记（四）：Inverse Kinematics相关函数

逆运动学（IK）用于确定机器人模型的关节配置，以实现所需的最终效果位置。基于关节之间的转换，在rigidBodyTree机器人模型中指定了机器人运动学约束。您还可以指定外部约束，例如摄像机臂的瞄准约束或特定刚体链接上的笛卡尔边界框。使用机械手约束对象和generalizedInverseKinematics对象。

inverseKinematics

inverseKinematics系统对象创建一个逆运动学（IK）求解器，对于指定的刚体树模型为期望的末端执行器姿势计算各个关节角度。为您的机器人创建一个刚体树模型。该模型定义了求解器强制执行的所有联合约束。如果可能的话，请遵守机器人模型中指定的关节极限。 要指定除末端执行器姿势以外的更多约束（包括目标约束，位置范围或方向目标），请考虑使用generalizedInverseKinematics。通过此类，您可以计算多约束IK解决方案。

对于一个期望的末端执行器姿态，去计算各个关节的角度，这就是逆运动学问题，主要由以下步骤组成：

• 创建inverseKinematics对象，并且设置相关属性；
• 使用参数调用对象，就好像它是一个函数一样。

% 创建对象
% 创建逆运动学求解器。要使用求解器，请在RigidBodyTree属性中指定刚体树模型。
ik = inverseKinematics
    
% Name-Value值对参数，根据例子去理解更深刻
ik = inverseKinematics(Name,Value)

性质如下：

RigidBodyTree	刚体树模型	ridigBodyTree对象
SolverAlgorithm	逆运动学算法	求解逆向运动学的算法，指定为“ BFGSGradientProjection”或“ LevenbergMarquardt”。默认为"BFGSGradientProjection"有关每种算法的详细信息，请参见Inverse Kinematics Algorithms
SolverParameters	算法相关的参数	与指定算法关联的参数，指定为结构。结构中的字段特定于算法

例子：

% 指定rigidbodytree对象，建立该对象的ik逆运动学求解器
ik = inverseKinematics('RigidBodyTree',rigidbodytree)
% 更新刚体树模型
addBody(rigidbodytree,rigidBody('body1'),'base')

% 将刚体树重新分配给IK解算器。如果在修改刚体树模型之前调用了求解器或step函数，请使用release来更改属性
ik.RigidBodyTree = rigidbodytree;

利用创建得到的ik对象进行解算

% 根据预期的末端执行器位姿来逆解得到关节角度
% 为ik指定initialguess，并为姿势的六个组成部分的公差指定所需的权重。与算法执行相关的解决方案信息solInfo与联合配置解决方案configSol一起返回

% Inputs
% endeffector---末端执行器名称，必须在建立ik对象的刚体树模型中
% pose---末端执行器的位姿，指定为4*4齐次变换矩阵，表示想要达到的末端执行器末端位姿
% weights---姿态误差的权重，指定为6元素向量，前三个元素对应于所需姿势的方向误差上的权重。最后三个元素对应于所需姿势的xyz位置误差的权重
% initalguess---机器人各个关节构型的猜测值，作为ik解算器的初始值，要使用矢量形式，请将RigidBodyTree属性中分配的对象的DataFormat属性设置为'row'或'column'。

% OutPuts
% configSol---机器人关节构型的逆解，包含如下两个部分：
% 	- JointName：刚体树模型中的关节名称向量
% 	- JointPosition：各关节对应的角度，这是在公差范围内逆解得到的
% solInfo---解信息，包含下列五个部分：
% 	- Iterations：算法迭代的步数
% 	- NumRandomRestrarts：由于算法陷入了局部最小值而随机重启的次数
% 	- PoseErrorNorm：当前姿态与目标姿态之间的误差大小
% 	- ExitFlag：该代码提供了有关算法执行及其返回原因的更多详细信息。有关每种算法类型的退出标志，请参见退出标志
% 	- Status：描述解决方案是在容差范围内（'success'）还是算法可以找到的最佳解决方案（'best available'）的字符向量
[configSol,solInfo] = ik(endeffector,pose,weights,initialguess)

例子

% 加载机器人模型，puma1是一个六轴机器人刚体树模型
load exampleRobots.mat
showdetails(puma1)

% 获取puma1的一个任意构型
randConfig = puma1.randomConfiguration;

% 在randConfig随机构型下获取从末端执行器（L6）到base的齐次变换
% 使用此变换作为末端执行器的目标姿势
tform = getTransform(puma1,randConfig,'L6','base');
% 展示该目标姿态下的机器人构型
show(puma1,randConfig)

% 创建puma1模型的ik对象
ik = inverseKinematics('RigidBodyTree',puma1);
% 指定姿势不同分量的权重，为啥要设这个权重，目前我也不太清楚，估计和具体算法相关
% 对于方向角分量来说，使用比位置分量小的权重
weights = [0.25 0.25 0.25 1 1 1];
% 将机器人的home构型用作初始猜测的关节角
initialguess = puma1.homeConfiguration;

% 根据预期的末端执行器位姿来逆解得到关节角度
[configSoln,solnInfo] = ik('L6',tform,weights,initialguess);

% 显示逆解得到的关节构型
% 逆解得到的关节构型与目标构型有着细微的差别，因为存在误差
% 多次调用ik对象可以提供相似或非常不同的关节构型
show(puma1,configSoln)

得到的randConfig下的机器人构型

将randConfig下的末端位姿作为目标位姿，以home构型作为初始姿态，进行逆运动学计算，得到的逆解构型为：