matlab机器人工具箱建立RRRPRR机械臂，已知P值，求建立运动学正解

对于一个RRRPRR机械臂，其运动学正解可以通过以下步骤求解：

1. 定义机械臂的DH参数，确定各个关节的坐标系。

2. 根据DH参数计算出各个关节的变换矩阵，构成正运动学变换矩阵。

3. 将正运动学变换矩阵相乘，得到机械臂的正运动学矩阵。

4. 将机械臂的正运动学矩阵与末端执行器的位姿矩阵相乘，得到机械臂末端执行器的位置和姿态。

具体来说，对于一个RRRPRR机械臂，其DH参数可以定义为：

| i | alpha(i-1) | a(i-1) | d(i) | theta(i) |
| - | ---------- | ------ | ---- | -------- |
| 1 | 0 | 0 | 0 | q1 |
| 2 | -pi/2 | 0 | 0 | q2 |
| 3 | 0 | a2 | 0 | q3 |
| 4 | -pi/2 | a3 | d4 | q4 |
| 5 | pi/2 | 0 | d5 | q5 |
| 6 | -pi/2 | 0 | d6 | q6 |

其中，q1-q6为各个关节角度，a2、a3、d4、d5、d6为机械臂的尺寸参数。

根据DH参数，可以计算出各个关节的变换矩阵：

T01 = DH(0, 0, 0, q1)
T12 = DH(-pi/2, 0, 0, q2)
T23 = DH(0, a2, 0, q3)
T34 = DH(-pi/2, a3, d4, q4)
T45 = DH(pi/2, 0, d5, q5)
T56 = DH(-pi/2, 0, d6, q6)

其中，DH函数用于计算DH变换矩阵，具体实现如下：

function T = DH(alpha, a, d, theta)
T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
     sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
     0, sin(alpha), cos(alpha), d;
     0, 0, 0, 1];
end

将各个变换矩阵相乘，得到机械臂的正运动学变换矩阵：

T06 = T01 * T12 * T23 * T34 * T45 * T56

最后，将机械臂的正运动学矩阵与末端执行器的位姿矩阵相乘，得到机械臂末端执行器的位置和姿态。例如，如果末端执行器的位置为P=[x,y,z]，姿态为R=[r11,r12,r13;r21,r22,r23;r31,r32,r33]，则机械臂的正运动学正解可以表示为：

T06 = [r11, r12, r13, x;
       r21, r22, r23, y;
       r31, r32, r33, z;
       0,   0,   0,   1]

其中，r11-r33为旋转矩阵元素，表示末端执行器的姿态。