具有逆运动学的2维轨迹跟踪（翻译--个人学习记录） MATLAB和机器人

1、前记：这几天同样在MATLAB官网上学习了些机器人相关的基础知识。看到官方新推出的机器人系统工具箱（Robotics System Toolbox）的功能在很多方面都强于Robotics Toolbox，而且要更好的利用其学习必须结合到ROS一起学习。之前一直犹豫是因为不熟悉ROS和Ubuntu系统，另外想装虚拟机又嫌麻烦，但从长远看十分有必要学习ROS了，想到自己基础薄弱便担心其路途遥远啊~ ~//
上下求索吧，以下是一个涉及逆运动学的二维轨迹跟踪例子。用机器人系统工具箱函数做的，参看网址在文末。

%% 2-D Path Tracing With Inverse Kinematics %% Introduction % This example shows how to calculate inverse kinematics for a simple 2D % manipulator using the class. % The manipulator robot is a simple 2-degree-of-freedom planar % manipulator with revolute joints which is created by assembling rigid bodies into % a object. A circular trajectory is % created in a 2-D plane and given as points to the inverse kinematics solver. The solver % calculates the required joint positions to achieve this trajectory. % Finally, the robot is animated to show the robot configurations that % achieve the circular trajectory.%本示例演示如何使用机器人来计算简单2D 机械手的逆运动学。机械手机器人是一个简单的2自由度平面机械手, 它是通过将刚体装配成robotics.RigidBodyTree 对象。在2维平面上创建一个圆形轨迹, 并将其作为反向运动学求解器的指向。规划求解计算所需的关节位置以实现此轨迹。最后, 对机器人进行了动画演示, 显示了实现圆形轨迹的机器人配置。 %%% Construct The Robot % Create a |RigidBodyTree| object and rigid bodies with their % associated joints. Specify the geometric properties of each rigid body % and add it to the robot. % Start with a blank rigid body tree model%构造机器人创建一个RigidBodyTree对象和刚性体与他们的关联关节。指定每个刚体的几何性质并将其添加到机器人中。从一个空白的刚体树模型开始 robot = robotics.RigidBodyTree('DataFormat','column','MaxNumBodies',3); %% % Specify arm lengths for the robot arm.//指定机械手臂的长度 L1 = 0.4; L2 = 0.3; %% % Add |'link1'| body with |'joint1'| joint.//添加'link1'身体与'joint1'关节。 body = robotics.RigidBody('link1'); joint = robotics.Joint('joint1', 'revolute'); setFixedTransform(joint,trvec2tform([0 0 0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'base'); %% % Add |'link2'| body with |'joint2'| joint.//添加'link2'身体与'joint2'关节。 body = robotics.RigidBody('link2'); joint = robotics.Joint('joint2','revolute'); setFixedTransform(joint, trvec2tform([L1,0,0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'link1'); %% % Add |'tool'| end effector with |'fix1'| fixed joint.//添加 " 'fix1'固定接头的'tool'端效应器。 body = robotics.RigidBody('tool'); joint = robotics.Joint('fix1','fixed'); setFixedTransform(joint, trvec2tform([L2, 0, 0])); body.Joint = joint; addBody(robot, body, 'link2'); %% % Show details of the robot to validate the input properties. The robot % should have two non-fixed joints for the rigid bodies and a fixed body % for the end-effector.%显示机器人的详细信息以验证输入属性。机器人应该有两个非固定的关节为刚体和一个固定的机构为末端执行器。 % showdetails(robot)%% Define The Trajectory % Define a circle to be traced over the course of 10 seconds. This circle % is in the _xy_ plane with a radius of 0.15.%定义轨迹：定义一个圆, 在10秒的过程中跟踪。这个圆圈在 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))]; %% Inverse Kinematics Solution % Use an |InverseKinematics| object to find a solution of robotic % configurations that achieve the given end-effector positions along the % trajectory. %逆运动学解：使用InverseKinematics对象找到机器人配置的解决方案, 以实现沿轨迹给定的最终末端位置。%% % Pre-allocate configuration solutions as a matrix |qs|.//预分配配置解决方案作为矩阵qs. q0 = homeConfiguration(robot); ndof = length(q0); qs = zeros(count, ndof); %% % Create the inverse kinematics solver. Because the _xy_ Cartesian points are the % only important factors of the end-effector pose for this workflow, % specify a non-zero weight for the fourth and fifth elements of the % |weight| vector. All other elements are set to zero.%创建反向运动学求解器。因为 xy 笛卡尔点是此工作流的末端姿态的唯一重要因素, 所以请为weight向量的第四和第五元素指定一个非零权重。所有其他元素都设置为零。 %ik = robotics.InverseKinematics('RigidBodyTree', robot); weights = [0, 0, 0, 1, 1, 0]; endEffector = 'tool'; %% % Loop through the trajectory of points to trace the circle. Call the |ik| % object for each point to generate the joint configuration that achieves % the end-effector position. Store the configurations to use later.%通过点的轨迹循环来跟踪圆。调用每个点的ik对象以生成实现末端位置的关节配置。存储要稍后使用的配置。 %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%% Animate The Solution % Plot the robot for each frame of the solution using that specific robot % configuration. Also, plot the desired trajectory. %% % Show the robot in the first configuration of the trajectory. Adjust the % plot to show the 2-D plane that circle is drawn on. Plot the desired % trajectory.%对解决方案进行动画处理：使用特定的机器人配置为解决方案的每个帧绘制机器人。同时, 绘制所需的轨迹。在轨迹的第一个配置中显示机器人。调整图形以显示2维平面, 该圆被绘制。绘制所需的轨迹。 %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])%% % Set up a object to display the robot % trajectory at a fixed rate of 15 frames per second. Show the robot in % each configuration from the inverse kinematic solver. Watch as the arm % traces the circular trajectory shown.%建立robotics.Rate对象以每秒15帧的固定速率显示机器人轨迹。从逆运动学求解器中显示每个配置中的机器人。观察手臂跟踪显示的圆形轨迹。 %framesPerSecond = 15; r = robotics.Rate(framesPerSecond); for i = 1:count show(robot,qs(i,:)','PreservePlot',false); drawnow waitfor(r); end

2、删掉汉字，运行结果

文章图片

文章图片

3、从以上格式继续添加一个link.
代码：

%%机器人创建 robot = robotics.RigidBodyTree('DataFormat','column','MaxNumBodies',4); L1 = 0.3; L2 = 0.3; L3=0.2; body = robotics.RigidBody('link1'); joint = robotics.Joint('joint1', 'revolute'); setFixedTransform(joint,trvec2tform([0 0 0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'base'); body = robotics.RigidBody('link2'); joint = robotics.Joint('joint2','revolute'); setFixedTransform(joint, trvec2tform([L1,0,0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'link1'); body = robotics.RigidBody('link3'); joint = robotics.Joint('joint3','revolute'); setFixedTransform(joint, trvec2tform([L2,0,0])); joint.JointAxis = [0 0 1]; body.Joint = joint; addBody(robot, body, 'link2'); body = robotics.RigidBody('tool'); joint = robotics.Joint('fix1','fixed'); setFixedTransform(joint, trvec2tform([L3, 0, 0])); body.Joint = joint; addBody(robot, body, 'link3'); %%定义圆的轨迹 t = (0:0.2:10)'; % Time count = length(t); center = [0.5 0.1 0]; radius = 0.2; 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 = robotics.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])framesPerSecond = 15; r = robotics.Rate(framesPerSecond); for i = 1:count show(robot,qs(i,:)','PreservePlot',false); drawnow waitfor(r); end

【具有逆运动学的2维轨迹跟踪（翻译--个人学习记录）】结果：