Robotics Toolbox ：（2）正逆运动学

转载自这里

matlab机器人工具箱 robotic toolbox 做运动学分析非常方便，SerialLink 类中有现成的函数：SerialLink.fkine(theta)，可以直接对已经建立的机器人模型做运动学分析，同时可以使用SerialLink.ikine（T) 求逆运动学参数。

clear;
clc;
L1 = Link('d', 0, 'a', 0, 'alpha', pi/2);    %Link 类函数
L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);
L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);
L4 = Link('d', 1, 'a', 0, 'alpha', -pi/2);
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L6 = Link('d', 1, 'a', 0, 'alpha', 0);
b=isrevolute(L1);  %Link 类函数
robot=SerialLink([L1,L2,L3,L4,L5,L6]);   %SerialLink 类函数
robot.name='带球形腕的拟人臂';     %SerialLink 属性值
robot.manuf='飘零过客';     %SerialLink 属性值
robot.display();  %Link 类函数
theta=[0 0 0 0 0 0];
robot.plot(theta);   %SerialLink 类函数

theta1=[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6];
p0=robot.fkine(theta);
p1=robot.fkine(theta1);
s=robot.A([4 5 6],theta);
cchain=robot.trchain;
q=robot.getpos();

q2=robot.ikine(p1);  %逆运动学
j0=robot.jacob0(q2);    %雅可比矩阵

theta1 =

    0.7854   -1.0472    0.5236    0.7854   -1.0472    0.5236



p0 = 
   -0.7071         0    0.7071     1.414
         0        -1         0         0
    0.7071         0    0.7071     1.914
         0         0         0         1


p1 = 
    0.9874    0.1567    0.0206      1.01
    0.0544   -0.4593    0.8866     1.876
    0.1484   -0.8743   -0.4621   0.04672
         0         0         0         1


s = 
         1         0         0         0
         0         1         0         0
         0         0         1         2
         0         0         0         1

cchain =

    'Rz(q1)Rx(90)Rz(q2)Tx(0.5)Rz(q3)Rx(90)Rz(q4)Tz(1)Rx(-90)Rz(q5)Rx(90)Rz(q6)Tz(1)'


q =

     0     0     0     0     0     0


q2 =

    0.7854   -1.0472    0.5236    0.7854   -1.0472    0.5236


j0 =

   -1.8758   -0.0330    0.1437   -0.5451    0.7768         0
    1.0098   -0.0330    0.1437    0.3209    0.2768         0
    0.0000    2.0404    1.6074    0.5915    0.5657         0
    0.0000    0.7071    0.7071    0.6830    0.6294    0.0206
   -0.0000   -0.7071   -0.7071    0.6830   -0.3706    0.8866
    1.0000   -0.0000   -0.0000    0.2588   -0.6830   -0.4621

可以看出，逆解和原始的角度相同（原微博说不同？）。