基于逆运动学的二维路径跟踪

介绍：

这个例子展示了如何使用inverseKinematics类计算简单的2D机械手的逆运动学。该机器人是一种简单的2自由度平面机械臂，具有转动关节，通过将刚体组装成一个rigidBodyTree对象来生成。在二维平面上创建一个圆形轨迹，并将其作为指向逆运动学求解器的点。求解器计算所需的关节位置来实现这一轨迹。最后，对机器人进行动画化处理，以显示实现圆形轨迹的机器人构型。

如何构建机器人：

创建一个rigidBodyTree对象和刚体及其相关关节。指定每个刚体的几何属性，并将其添加到机器人中。

首先：

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);

指定机器人手臂的手臂长度

L1 = 0.3;
L2 = 0.3;

然后描述机器人关节：

body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

然后在描述机器人末端执行器与固定关节

body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

查看机器人的属性：

showdetails(robot)

接下来描述机器人的运动轨迹

定义一个圆形跟踪轨迹

这个圆在xy平面上，半径为0.15

t = (0:0.2:10)'; % Time
count = length(t);
center = [0.3 0.1 0];
radius = 0.15;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

逆运动学求解过程：

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

ik = inverseKinematics('RigidBodyTree', robot);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

跟踪轨迹：

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

绘图可视化：

figure
show(robot,qs(1,:)');
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])