├── 正运动学求解1.gif ├── 正运动学求解2.gif ├── 逆运动学求解1.gif ├── 逆运动学求解2.gif ├── 逆运动学求解画圆.gif ├── README.md └── puma560.md /正运动学求解1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SEUZTh/Forward-and-inverse-kinematics-of-puma560/HEAD/正运动学求解1.gif -------------------------------------------------------------------------------- /正运动学求解2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SEUZTh/Forward-and-inverse-kinematics-of-puma560/HEAD/正运动学求解2.gif -------------------------------------------------------------------------------- /逆运动学求解1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SEUZTh/Forward-and-inverse-kinematics-of-puma560/HEAD/逆运动学求解1.gif -------------------------------------------------------------------------------- /逆运动学求解2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SEUZTh/Forward-and-inverse-kinematics-of-puma560/HEAD/逆运动学求解2.gif -------------------------------------------------------------------------------- /逆运动学求解画圆.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SEUZTh/Forward-and-inverse-kinematics-of-puma560/HEAD/逆运动学求解画圆.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Forward-and-inverse-kinematics-of-puma560 2 | 东南大学自动化学院2018级机器人技术基础实验2：正逆运动学求解仿真 3 | 4 | 5 | 6 | - [1. 实验目的和要求](#1-实验目的和要求) 7 | - [2. 实验手段](#2-实验手段) 8 | - [2.1 实验原理](#21-实验原理) 9 | - [2.2 函数详解](#22-函数详解) 10 | - [3. 实验主要内容](#3-实验主要内容) 11 | - [4. 实验结果和分析](#4-实验结果和分析) 12 | - [4.1 正运动学求解](#41-正运动学求解) 13 | - [例1](#例1) 14 | - [例2](#例2) 15 | - [4.2 逆运动学求解](#42-逆运动学求解) 16 | - [例1](#例1-1) 17 | - [例2](#例2-1) 18 | 19 | 20 | 21 | # 1. 实验目的和要求 22 | - 掌握常见形式的机器人的D-H坐标系建立方法、运动学建模； 23 | - 正逆运动学分析与求解； 24 | - 使用Robotics Toolbox对直角坐标机器人、圆柱坐标机器人、球坐标机器人、PUMA560其中之一建立运动学模型，正逆求解和仿真。 25 | # 2. 实验手段 26 | ## 2.1 实验原理 27 | - D-H坐标系 28 | - 正运动学求解：已知机器人的所有连杆长度和关节角度，计算机器人手的位姿。 29 | - 逆运动学求解：找到方程的逆，求得所需的关节变量，使机器人放置在期望的位姿。 30 | - Robotics Toolbox & MATLAB 31 | ## 2.2 函数详解 32 | - transl 33 | - Create or unpack an SE3 translational transform 34 | - ```T = transl(x, y, z)``` is an SE(3) homogeneous transform (4x4) representing a pure translation of x, y and z. 35 | ```matlab 36 | >> T=transl(0, 0, 0.62) 37 | 38 | T = 39 | 40 | 1.0000 0 0 0 41 | 0 1.0000 0 0 42 | 0 0 1.0000 0.6200 43 | 0 0 0 1.0000 44 | ``` 45 | --- 46 | - trotz 47 | - Rotation about Z axis 48 | - ```T = trotz(theta)``` is a homogeneous transformation (4x4) representing a rotation of theta radians about the z-axis. 49 | ```matlab 50 | >> T=trotz(pi/2) 51 | 52 | T = 53 | 54 | 0.9996 -0.0274 0 0 55 | 0.0274 0.9996 0 0 56 | 0 0 1.0000 0 57 | 0 0 0 1.0000 58 | ``` 59 | --- 60 | - SerialLink 61 | - Serial-link robot class 62 | - A concrete class that represents a serial-link arm-type robot. The mechanism is described using Denavit-Hartenberg parameters, one set per joint. 63 | ```matlab 64 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 65 | transl(1, 1, 1)* trotz(pi/2)); %连接连杆，机器人取名puma560 66 | ``` 67 | --- 68 | - Link 69 | - Robot manipulator Link class 70 | - A Link object holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. 71 | ```matlab 72 | % theta d a alpha offset 73 | L1=Link([ 0 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 74 | L2=Link([ 0 0 0.4318 0 0 ],'standard'); 75 | L3=Link([ 0 0.1501 0.0203 pi/2 0 ],'standard'); 76 | L4=Link([ 0 0.4318 0 -pi/2 0 ],'standard'); 77 | L5=Link([ 0 0 0 pi/2 0 ],'standard'); 78 | L6=Link([ 0 0 0 0 0 ],'standard'); 79 | ``` 80 | --- 81 | # 3. 实验主要内容 82 | - PUMA机器人的标准D-H参数如下： 83 | 84 | |连杆|$\theta_{i}$|$d$|$a$|$\alpha$| 85 | |:---:|:---:|:---:|:---:|:---:| 86 | |1|$\theta_{1}$|0|0|-90| 87 | |2|$\theta_{2}$|149.09mm|431.8mm|0| 88 | |3|$\theta_{3}$|0|-20.32mm|90| 89 | |4|$\theta_{4}$|433.07mm|0|-90| 90 | |5|$\theta_{5}$|0|0|90| 91 | |6|$\theta_{6}$|56.25mm|0|0| 92 | # 4. 实验结果和分析 93 | ## 4.1 正运动学求解 94 | ### 例1 95 | $\theta=[\frac{\pi}{2},0,\frac{\pi}{2},0,0,0]$ 96 | ```matlab 97 | >> theta=[pi/2 0 pi/2 0 0 0]; 98 | >> T=robot.fkine(theta) 99 | 100 | 101 | T = 102 | 0 -1 0 -0.1501 103 | 0 0 1 0.8636 104 | -1 0 0 0.4797 105 | 0 0 0 1 106 | ``` 107 | ![正运动学求解1](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017211026.png) 108 | 109 | 110 | ### 例2 111 | $\theta=[-\frac{\pi}{2},0,-\frac{\pi}{2},0,0,0]$ 112 | ```matlab 113 | >> theta=[-pi/2 0 -pi/2 0 0 0]; 114 | >> T=robot.fkine(theta) 115 | 116 | 117 | T = 118 | 0 1 0 0.1501 119 | 0 0 1 0 120 | 1 0 0 0.5203 121 | 0 0 0 1 122 | ``` 123 | ![正运动学求解2](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017212415.png) 124 | 125 | ## 4.2 逆运动学求解 126 | ### 例1 127 | ```matlab 128 | bx = -0.06; 129 | by = -0.2; 130 | bz = 0.1; 131 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 132 | targetPos = [bx by bz]; %末端位置向量 133 | TR=transl(targetPos)*tform; %位姿齐次矩阵 134 | q=robot.ikine(TR); % 求逆解 135 | >> q 136 | 137 | q = 138 | 139 | -2.6644 0.1658 -2.6476 -0.7275 1.9528 2.1979 140 | ``` 141 | ![逆运动学求解1](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017214307.png) 142 | 143 | ### 例2 144 | ```matlab 145 | ```matlab 146 | bx = -0.35; 147 | by = -0.4; 148 | bz = 0.2; 149 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 150 | targetPos = [bx by bz]; %末端位置向量 151 | TR=transl(targetPos)*tform; %位姿齐次矩阵 152 | q=robot.ikine(TR); % 求逆解 153 | >> q 154 | 155 | q = 156 | 157 | 1.1383 1.7925 -3.1260 0.3310 2.0308 -1.0988 158 | ``` 159 | ![逆运动学求解2](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017214154.png) 160 | 161 | # 5. 附 Matlab 代码和演示视频 162 | - 正逆运动学求解代码： 163 | ```matlab 164 | %% 标准DH建立puma560机器人模型 165 | clear; 166 | clc; 167 | %建立机器人模型(标准D-H参数) 168 | theta=[-pi/2 0 -pi/2 0 0 0]; 169 | %theta=[0 0 -pi/2 pi/2 pi/2 0]; 170 | % theta d a alpha offset 171 | L1=Link([ theta(1) 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 172 | L2=Link([ theta(2) 0 0.4318 0 0 ],'standard'); 173 | L3=Link([ theta(3) 0.1501 0.0203 pi/2 0 ],'standard'); 174 | L4=Link([ theta(4) 0.4318 0 -pi/2 0 ],'standard'); 175 | L5=Link([ theta(5) 0 0 pi/2 0 ],'standard'); 176 | L6=Link([ theta(6) 0 0 0 0 ],'standard'); 177 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 178 | transl(0, 0, 0.5)* trotz(0)); %连接连杆，机器人取名puma560 179 | % base pose of robot's base (4x4 homog xform) 180 | figure(1) 181 | robot.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。 182 | robot.display(); %Link 类函数，显示建立的机器人DH参数 183 | 184 | T=robot.fkine(theta); %计算机器人正运动学，括号内为theta值 185 | disp(T); 186 | robot.plot(theta) 187 | 188 | %% 使用Peter工具箱逆解 189 | hold on 190 | title("工具箱机器人模型"); 191 | xlabel('x/米','FontSize',12); 192 | ylabel('y/米','FontSize',12); 193 | zlabel('z/米','FontSize',12); 194 | bx = -0.35; 195 | by = -0.4; 196 | bz = 0.2; 197 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 198 | targetPos = [bx by bz]; %末端位置向量 199 | TR=transl(targetPos)*tform; %位姿齐次矩阵 200 | hold on 201 | grid on 202 | plot3(bx,by,bz,'*','LineWidth',1); 203 | q=robot.ikine(TR); 204 | robot.plot(q); 205 | ``` 206 | 207 | - 逆运动学求解——画圆 208 | ```matlab 209 | %% 标准DH建立puma560机器人模型 210 | clear; 211 | clc; 212 | %建立机器人模型(标准D-H参数) 213 | theta=[-pi/2 0 -pi/2 0 0 0]; 214 | %theta=[0 0 -pi/2 pi/2 pi/2 0]; 215 | % theta d a alpha offset 216 | L1=Link([ theta(1) 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 217 | L2=Link([ theta(2) 0 0.4318 0 0 ],'standard'); 218 | L3=Link([ theta(3) 0.1501 0.0203 pi/2 0 ],'standard'); 219 | L4=Link([ theta(4) 0.4318 0 -pi/2 0 ],'standard'); 220 | L5=Link([ theta(5) 0 0 pi/2 0 ],'standard'); 221 | L6=Link([ theta(6) 0 0 0 0 ],'standard'); 222 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 223 | transl(0, 0, 0.5)* trotz(0)); %连接连杆，机器人取名puma560 224 | % base pose of robot's base (4x4 homog xform) 225 | figure(1) 226 | robot.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。 227 | robot.display(); %Link 类函数，显示建立的机器人DH参数 228 | 229 | T=robot.fkine(theta); %计算机器人正运动学，括号内为theta值 230 | disp(T); 231 | robot.plot(theta) 232 | 233 | %% 定义圆路径 234 | t = (0:0.2:20)'; count = length(t);center = [-0.25 0.6 0.1];radius = 0.1;%圆心和半径 235 | theta = t*(2*pi/t(end)); 236 | points =(center + radius*[cos(theta) sin(theta) zeros(size(theta))])'; 237 | figure(1); 238 | plot3(points(1,:),points(2,:),points(3,:),'r') 239 | 240 | %% 使用Peter工具箱逆解进行轨迹重现 241 | hold on 242 | % axis([-1 1.2 -1 1.2 -1 1.2]) 243 | title("工具箱机器人模型末端运动轨迹"); 244 | xlabel('x/米','FontSize',12); 245 | ylabel('y/米','FontSize',12); 246 | zlabel('z/米','FontSize',12); 247 | for i = 1:size(points,2) 248 | pause(0.01) 249 | bx = points(1,i); 250 | by = points(2,i); 251 | bz =points(3,i); 252 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 253 | targetPos = [bx by bz]; %末端位置向量 254 | % targetOr = [0.02 -0.14 0.99 -0.01] %四元数 255 | % tform = quat2tform(targetOr) %四元数转姿态齐次矩阵 256 | TR=transl(targetPos)*tform; %位姿齐次矩阵 257 | hold on 258 | grid on 259 | plot3(bx,by,bz,'*','LineWidth',1); 260 | q=robot.ikine(TR); 261 | pause(0.01) 262 | robot.plot(q);%动画演示 263 | end 264 | ``` 265 | -------------------------------------------------------------------------------- /puma560.md: -------------------------------------------------------------------------------- 1 |
机器人技术基础实验2报告
2 |
张天浩
3 |
2020.10.19
4 | 5 | 6 | 7 | - [1. 实验目的和要求](#1-实验目的和要求) 8 | - [2. 实验手段](#2-实验手段) 9 | - [2.1 实验原理](#21-实验原理) 10 | - [2.2 函数详解](#22-函数详解) 11 | - [3. 实验主要内容](#3-实验主要内容) 12 | - [4. 实验结果和分析](#4-实验结果和分析) 13 | - [4.1 正运动学求解](#41-正运动学求解) 14 | - [例1](#例1) 15 | - [例2](#例2) 16 | - [4.2 逆运动学求解](#42-逆运动学求解) 17 | - [例1](#例1-1) 18 | - [例2](#例2-1) 19 | 20 | 21 | 22 | # 1. 实验目的和要求 23 | - 掌握常见形式的机器人的D-H坐标系建立方法、运动学建模； 24 | - 正逆运动学分析与求解； 25 | - 使用Robotics Toolbox对直角坐标机器人、圆柱坐标机器人、球坐标机器人、PUMA560其中之一建立运动学模型，正逆求解和仿真。 26 | # 2. 实验手段 27 | ## 2.1 实验原理 28 | - D-H坐标系 29 | - 正运动学求解：已知机器人的所有连杆长度和关节角度，计算机器人手的位姿。 30 | - 逆运动学求解：找到方程的逆，求得所需的关节变量，使机器人放置在期望的位姿。 31 | - Robotics Toolbox & MATLAB 32 | ## 2.2 函数详解 33 | - transl 34 | - Create or unpack an SE3 translational transform 35 | - ```T = transl(x, y, z)``` is an SE(3) homogeneous transform (4x4) representing a pure translation of x, y and z. 36 | ```matlab 37 | >> T=transl(0, 0, 0.62) 38 | 39 | T = 40 | 41 | 1.0000 0 0 0 42 | 0 1.0000 0 0 43 | 0 0 1.0000 0.6200 44 | 0 0 0 1.0000 45 | ``` 46 | --- 47 | - trotz 48 | - Rotation about Z axis 49 | - ```T = trotz(theta)``` is a homogeneous transformation (4x4) representing a rotation of theta radians about the z-axis. 50 | ```matlab 51 | >> T=trotz(pi/2) 52 | 53 | T = 54 | 55 | 0.9996 -0.0274 0 0 56 | 0.0274 0.9996 0 0 57 | 0 0 1.0000 0 58 | 0 0 0 1.0000 59 | ``` 60 | --- 61 | - SerialLink 62 | - Serial-link robot class 63 | - A concrete class that represents a serial-link arm-type robot. The mechanism is described using Denavit-Hartenberg parameters, one set per joint. 64 | ```matlab 65 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 66 | transl(1, 1, 1)* trotz(pi/2)); %连接连杆，机器人取名puma560 67 | ``` 68 | --- 69 | - Link 70 | - Robot manipulator Link class 71 | - A Link object holds all information related to a robot link such as kinematics parameters, rigid-body inertial parameters, motor and transmission parameters. 72 | ```matlab 73 | % theta d a alpha offset 74 | L1=Link([ 0 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 75 | L2=Link([ 0 0 0.4318 0 0 ],'standard'); 76 | L3=Link([ 0 0.1501 0.0203 pi/2 0 ],'standard'); 77 | L4=Link([ 0 0.4318 0 -pi/2 0 ],'standard'); 78 | L5=Link([ 0 0 0 pi/2 0 ],'standard'); 79 | L6=Link([ 0 0 0 0 0 ],'standard'); 80 | ``` 81 | --- 82 | # 3. 实验主要内容 83 | - PUMA机器人的标准D-H参数如下： 84 | 85 | |连杆|$\theta_{i}$|$d$|$a$|$\alpha$| 86 | |:---:|:---:|:---:|:---:|:---:| 87 | |1|$\theta_{1}$|0|0|-90| 88 | |2|$\theta_{2}$|149.09mm|431.8mm|0| 89 | |3|$\theta_{3}$|0|-20.32mm|90| 90 | |4|$\theta_{4}$|433.07mm|0|-90| 91 | |5|$\theta_{5}$|0|0|90| 92 | |6|$\theta_{6}$|56.25mm|0|0| 93 | # 4. 实验结果和分析 94 | ## 4.1 正运动学求解 95 | ### 例1 96 | $\theta=[\frac{\pi}{2},0,\frac{\pi}{2},0,0,0]$ 97 | ```matlab 98 | >> theta=[pi/2 0 pi/2 0 0 0]; 99 | >> T=robot.fkine(theta) 100 | 101 | 102 | T = 103 | 0 -1 0 -0.1501 104 | 0 0 1 0.8636 105 | -1 0 0 0.4797 106 | 0 0 0 1 107 | ``` 108 | ![正运动学求解1](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017211026.png) 109 | 110 | 111 | ### 例2 112 | $\theta=[-\frac{\pi}{2},0,-\frac{\pi}{2},0,0,0]$ 113 | ```matlab 114 | >> theta=[-pi/2 0 -pi/2 0 0 0]; 115 | >> T=robot.fkine(theta) 116 | 117 | 118 | T = 119 | 0 1 0 0.1501 120 | 0 0 1 0 121 | 1 0 0 0.5203 122 | 0 0 0 1 123 | ``` 124 | ![正运动学求解2](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017212415.png) 125 | 126 | ## 4.2 逆运动学求解 127 | ### 例1 128 | ```matlab 129 | bx = -0.06; 130 | by = -0.2; 131 | bz = 0.1; 132 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 133 | targetPos = [bx by bz]; %末端位置向量 134 | TR=transl(targetPos)*tform; %位姿齐次矩阵 135 | q=robot.ikine(TR); % 求逆解 136 | >> q 137 | 138 | q = 139 | 140 | -2.6644 0.1658 -2.6476 -0.7275 1.9528 2.1979 141 | ``` 142 | ![逆运动学求解1](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017214307.png) 143 | 144 | ### 例2 145 | ```matlab 146 | ```matlab 147 | bx = -0.35; 148 | by = -0.4; 149 | bz = 0.2; 150 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 151 | targetPos = [bx by bz]; %末端位置向量 152 | TR=transl(targetPos)*tform; %位姿齐次矩阵 153 | q=robot.ikine(TR); % 求逆解 154 | >> q 155 | 156 | q = 157 | 158 | 1.1383 1.7925 -3.1260 0.3310 2.0308 -1.0988 159 | ``` 160 | ![逆运动学求解2](https://gitee.com/zhang_ma_nong/image-bed/raw/master/img/20201017214154.png) 161 | 162 | # 5. 附 Matlab 代码和演示视频 163 | - 正逆运动学求解代码： 164 | ```matlab 165 | %% 标准DH建立puma560机器人模型 166 | clear; 167 | clc; 168 | %建立机器人模型(标准D-H参数) 169 | theta=[-pi/2 0 -pi/2 0 0 0]; 170 | %theta=[0 0 -pi/2 pi/2 pi/2 0]; 171 | % theta d a alpha offset 172 | L1=Link([ theta(1) 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 173 | L2=Link([ theta(2) 0 0.4318 0 0 ],'standard'); 174 | L3=Link([ theta(3) 0.1501 0.0203 pi/2 0 ],'standard'); 175 | L4=Link([ theta(4) 0.4318 0 -pi/2 0 ],'standard'); 176 | L5=Link([ theta(5) 0 0 pi/2 0 ],'standard'); 177 | L6=Link([ theta(6) 0 0 0 0 ],'standard'); 178 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 179 | transl(0, 0, 0.5)* trotz(0)); %连接连杆，机器人取名puma560 180 | % base pose of robot's base (4x4 homog xform) 181 | figure(1) 182 | robot.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。 183 | robot.display(); %Link 类函数，显示建立的机器人DH参数 184 | 185 | T=robot.fkine(theta); %计算机器人正运动学，括号内为theta值 186 | disp(T); 187 | robot.plot(theta) 188 | 189 | %% 使用Peter工具箱逆解 190 | hold on 191 | title("工具箱机器人模型"); 192 | xlabel('x/米','FontSize',12); 193 | ylabel('y/米','FontSize',12); 194 | zlabel('z/米','FontSize',12); 195 | bx = -0.35; 196 | by = -0.4; 197 | bz = 0.2; 198 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 199 | targetPos = [bx by bz]; %末端位置向量 200 | TR=transl(targetPos)*tform; %位姿齐次矩阵 201 | hold on 202 | grid on 203 | plot3(bx,by,bz,'*','LineWidth',1); 204 | q=robot.ikine(TR); 205 | robot.plot(q); 206 | ``` 207 | 208 | - 逆运动学求解——画圆 209 | ```matlab 210 | %% 标准DH建立puma560机器人模型 211 | clear; 212 | clc; 213 | %建立机器人模型(标准D-H参数) 214 | theta=[-pi/2 0 -pi/2 0 0 0]; 215 | %theta=[0 0 -pi/2 pi/2 pi/2 0]; 216 | % theta d a alpha offset 217 | L1=Link([ theta(1) 0 0 -pi/2 0 ],'standard'); %连杆的D-H参数 218 | L2=Link([ theta(2) 0 0.4318 0 0 ],'standard'); 219 | L3=Link([ theta(3) 0.1501 0.0203 pi/2 0 ],'standard'); 220 | L4=Link([ theta(4) 0.4318 0 -pi/2 0 ],'standard'); 221 | L5=Link([ theta(5) 0 0 pi/2 0 ],'standard'); 222 | L6=Link([ theta(6) 0 0 0 0 ],'standard'); 223 | robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ... 224 | transl(0, 0, 0.5)* trotz(0)); %连接连杆，机器人取名puma560 225 | % base pose of robot's base (4x4 homog xform) 226 | figure(1) 227 | robot.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。 228 | robot.display(); %Link 类函数，显示建立的机器人DH参数 229 | 230 | T=robot.fkine(theta); %计算机器人正运动学，括号内为theta值 231 | disp(T); 232 | robot.plot(theta) 233 | 234 | %% 定义圆路径 235 | t = (0:0.2:20)'; count = length(t);center = [-0.25 0.6 0.1];radius = 0.1;%圆心和半径 236 | theta = t*(2*pi/t(end)); 237 | points =(center + radius*[cos(theta) sin(theta) zeros(size(theta))])'; 238 | figure(1); 239 | plot3(points(1,:),points(2,:),points(3,:),'r') 240 | 241 | %% 使用Peter工具箱逆解进行轨迹重现 242 | hold on 243 | % axis([-1 1.2 -1 1.2 -1 1.2]) 244 | title("工具箱机器人模型末端运动轨迹"); 245 | xlabel('x/米','FontSize',12); 246 | ylabel('y/米','FontSize',12); 247 | zlabel('z/米','FontSize',12); 248 | for i = 1:size(points,2) 249 | pause(0.01) 250 | bx = points(1,i); 251 | by = points(2,i); 252 | bz =points(3,i); 253 | tform = rpy2tr(136,-180,-180); %欧拉角转姿态齐次矩阵 254 | targetPos = [bx by bz]; %末端位置向量 255 | % targetOr = [0.02 -0.14 0.99 -0.01] %四元数 256 | % tform = quat2tform(targetOr) %四元数转姿态齐次矩阵 257 | TR=transl(targetPos)*tform; %位姿齐次矩阵 258 | hold on 259 | grid on 260 | plot3(bx,by,bz,'*','LineWidth',1); 261 | q=robot.ikine(TR); 262 | pause(0.01) 263 | robot.plot(q);%动画演示 264 | end 265 | ``` 266 | 267 | --------------------------------------------------------------------------------