600字范文 > Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反

Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反

时间：2021-06-09 08:24:50

机械臂仿真控制实例（其二）-KR210正向运动学

1.反向运动学概述

KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节。这种设计称为球形腕，而相交的公共点称为腕中心。这种设计的优点在于，它在运动学上解耦了末端执行器的位置和方向。joint_5joint\_5joint_5是球形手腕的公共交点，因此是手腕的中心。

首先我们要解反向位置问题，我们涉及到球形手腕joint4,5,6joint 4,5,6joint4,5,6，手腕的中心位置由前三个关节控制，通过使用基于末端执行器得出的完整的齐次变换矩阵，我们可以获得手腕中心的位置。

定义齐次变换如下：

其中l,m和n是正交向量，代表沿着局部坐标系的X，Y，Z轴的末端执行器的方向。

由于n是沿着的z轴的向量gripper_linkgripper\_linkgripper_link,我们可以得出：

其中,

pxp_xpx，pyp_ypy，pzp_zpz=末端执行器位置

wxw_xwx，wyw_ywy，wzw_zwz =手腕位置

d6d_6d6 =来自DH参数表

lll =末端执行器长度

现在，为了计算nxn_xnx，nyn_yny和nzn_znz，让我们从上个笔记继续，在上个笔记中我们计算了旋转矩阵，并更正末端执行器的URDF和DH参数之间的差异。

获取此校正后的旋转矩阵后，需要接下来计算相对于的末端执行器base_linkbase\_linkbase_link的姿态。在“ 旋转的合成”部分中，已经说过有关欧拉角的不同约定以及如何选择正确的约定。

一种约定是xyz外在旋转。使用此约定将一个固定坐标系转换为另一固定坐标系的结果的旋转矩阵为：

Rrpy = Rot(Z, yaw) * Rot(Y, pitch) * Rot(X, roll) * R_corr

其中，R_corr是校正旋转矩阵。

抓取器的侧倾，俯仰和偏航值是从ROS中的模拟器中获得的。但是，由于这些值以四元数返回，因此可以使用transformations.pyTF包中的模块。所述euler_from_quaternions（）方法将输出侧倾，俯仰和偏航值。

然后可以从此Rrpy矩阵中提取nxn_xnx，nyn_yny和nzn_znz的值，以获取手腕中心位置。

现在已经获得了中心位置，按照上面介绍的公式得出前三个关节的方程式。

θ1\theta_1θ1一旦从上方获得手腕中心位置，则计算将相对简单。θ2\theta_2θ2h和θ3\theta_3θ3可视化相对比较麻烦。下图描绘了相关角度。

222、333、wcwcwc分别为关节2、关节3、手腕中心。如果把关节投射到z-y平面上，就可以得到，或者更形象地说，三个关节之间的三角形。根据DH参数，可以计算上面每个关节之间的距离。利用三角函数，特别是余弦定理，可以计算出θ2\theta_2θ2和θ3\theta_3θ3。

对于逆向问题，我们需要找到最后三个联合变量的值。

使用单个DH变换，我们可以通过以下方法获得结果变换，从而得出结果旋转：

R0_6 = R0_1*R1_2*R2_3*R3_4*R4_5*R5_6

由于整体RPY（(Roll Pitch Yaw）旋转base_link和gripper_link必须等于各个链接之间的单独旋转的结果，因此：

R0_6 = Rrpy

其中，

上面计算的base_link和gripper_link之间的同构RPY旋转。

我们可以将关节1到3的计算值替换为它们各自的旋转矩阵，并用inv（R0_3）乘以上述方程式的两边，得出：

R3_6 = inv(R0_3) * Rrpy

在RHS（Right Hand Side of the equation）不具有任何变量代关节角度的值，因此比较LHS后（Left Hand Side of the equation）与RHS将导致方程式用于关节4、5和6。

因为数据量比较庞大，所以我们我们使用LU分解法来计算逆矩阵。用法如下：

M.inv("LU")

最后，我们有了描述所有六个关节变量的方程，接下来我们将把运动学分析转换成ROS python节点，该节点将执行取放操作的逆向运动学。

2.为Kuka KR210创建IK解算器

我们已经进行了运动分析，并且已经有了六个关节变量的方程式。现在，我们将要如何将运动分析转换为python程序以执行逆运动学。

在项目文件夹中，您可以找到以下名称的模板文件IK_server.py：

~/catkin_ws/src/RoboND-Kinematics-Project/kuka_arm/scripts/

此文件实现了可满足该CalculateIK.srv服务的ROS服务器节点：

# CalculateIK.srv file# requestgeometry_msgs/Pose[] poses---# responsetrajectory_msgs/JointTrajectoryPoint[] points

本质上是IK_server.py从拾取和放置模拟器接收末端执行器姿势，并负责执行逆向运动学，以计算出的关节变量值（在本例中为六个关节角度）向模拟器提供响应。

来看一下此模板的不同部分：

# import modulesimport rospyimport tffrom kuka_arm.srv import *from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPointfrom geometry_msgs.msg import Posefrom mpmath import *from sympy import *

我们首先导入用于ros的python库，然后是前面介绍的tf包。因为这是一个服务器节点，所以我们从kuka_arm包中导入服务消息。

当设置在测试模式时，我们将收到一个IK请求从带有末端执行器姿态模拟器使用来自geometry_msgs的posemsg。。

来自trajectory_msgs的JointTrajectoryPoint将用于将单个联合变量(在完成IK之后获得)打包成单个消息。

此外，我们还导入sympy和mpmath模块，以简化python中的符号数学。

def IK_server():# initialize node and create calculate_ik servicerospy.init_node('IK_server')s = rospy.Service('calculate_ik', CalculateIK, handle_calculate_IK)print "Ready to receive an IK request"rospy.spin()

该函数初始化我们的IK_server节点并创建一个CalculateIK，名称为calculate_ik的类型服务。

函数handle_calculate_IK用作接收此服务类型请求的回调函数。

rospy.spin()回调函数，只需保持节点不退出，直到关闭这个节点。

def handle_calculate_IK(req):rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))if len(req.poses) < 1:print "No valid poses received"return -1else:...joint_trajectory_list = []for x in xrange(0, len(req.poses)):# IK code starts herejoint_trajectory_point = JointTrajectoryPoint()...joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]joint_trajectory_list.append(joint_trajectory_point)rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))return CalculateIKResponse(joint_trajectory_list)

这是处理CalculateIK服务请求的函数，我们将在这里编写所有IK代码。

首先，我们打印出从请求接收到的末端执行器姿态的数量，并检查请求是否为空，在这种情况下，我们打印适当的消息并从该函数返回一个错误代码。

如果请求具有有效数量的末端执行器姿态，我们将初始化一个名为joint_tory_list的空列表，该列表将包含您的代码计算的关节角度值。

最后，我们开始一个循环，遍历从请求中收到的所有末端执行器姿势。joint_trajectory_point是JointTrajectoryPoint.msg的实例，它包含关节的角度，位置，速度，加速度和作用力，但我们仅使用位置字段来获取末端执行器位置。

在我们的代码计算出给定eef_pose的各个关节角度之后，我们填充joint_trajectory_list并作为响应发送回模拟器。

3.IK_server.py代码部分

(1)IK_server.py

#!/usr/bin/env pythonimport rospyimport tffrom kuka_arm.srv import *from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPointfrom geometry_msgs.msg import Posefrom mpmath import *from sympy import *def handle_calculate_IK(req):rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))if len(req.poses) < 1:print"No valid poses received"return [] # 如果按教程给的-1，就会有报错else:# Create symbolsq1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')dh = {alpha0: 0, a0: 0, d1: 0.75,alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2,alpha2: 0, a2: 1.25, d3: 0,alpha3: - pi / 2, a3: -0.054, d4: 0,alpha4: pi / 2, a4: 0, d5: 1.50,alpha5: -pi / 2, a5: 0, d6: 0,alpha6: 0, a6: 0, d7: 0.303, q7: 0}def dh_tf(alpha, a, d, q):dh_matrix = Matrix([[cos(q), -sin(q), 0, a],[sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d],[sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d],[0, 0, 0, 1]])return dh_matrixdef rot_mat(martix):rot_mat = martix[0:2, 0:2]return rot_matT0_1 = dh_tf(alpha0, a0, d1, q1).subs(dh)T1_2 = dh_tf(alpha1, a1, d2, q2).subs(dh)T2_3 = dh_tf(alpha2, a2, d3, q3).subs(dh)T3_4 = dh_tf(alpha3, a3, d4, q4).subs(dh)T4_5 = dh_tf(alpha4, a4, d5, q5).subs(dh)T5_6 = dh_tf(alpha5, a5, d6, q6).subs(dh)T6_G = dh_tf(alpha6, a6, d7, q7).subs(dh)T0_G = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_Gjoint_trajectory_list = []for x in xrange(0, len(req.poses)):# IK code starts herejoint_trajectory_point = JointTrajectoryPoint()# px,py,pz = end-effector position# roll, pitch, yaw = end-effector orientationpx = req.poses[x].position.xpy = req.poses[x].position.ypz = req.poses[x].position.z(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([req.poses[x].orientation.x, req.poses[x].orientation.y,req.poses[x].orientation.z, req.poses[x].orientation.w])# IK code# 建立旋转矩阵r, p, y = symbols('r p y')x_rot = Matrix([[1, 0, 0],[0, cos(r), -sin(r)],[0, sin(r), cos(r)]])y_rot = Matrix([[cos(p), 0, sin(p)],[0, 1, 0],[-sin(p), 0, cos(p)]])z_rot = Matrix([[cos(y), -sin(y), 0],[sin(y), cos(y), 0],[0, 0, 1]])G_rot = x_rot * y_rot * z_rot# 建立旋转修正矩阵Rot_Rrror = z_rot.subs(y, radians(180)) * y_rot.subs(p, radians(-90))# Gripper的完整旋转矩阵G_rot = G_rot * Rot_RrrorG_rot = G_rot.subs({'r': roll, 'p': pitch, 'y': yaw})Gripper = Matrix([[px],[py],[pz]])# 计算球型手臂中心位置的坐标WC = Gripper - (0.303) * G_rot[:, 2]# 计算相关的theta值theta1 = atan2(WC[1], WC[0])# 借助三角形解出相关theta角的坐标side_a = 1.501side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))side_c = 1.25angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))angle_c = acos((side_b * side_b + side_a * side_a - side_c * side_c) / (2 * side_a * side_b))theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)theta3 = pi / 2 - (angle_b + 0.036)# 计算球型手臂的旋转矩阵R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})# 利用LU解算法计算逆矩阵R3_6 = R0_3.inv("LU") * G_rottheta4 = atan2(R3_6[2, 2], -R3_6[0, 2])theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2])theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])print[theta1, theta2, theta3, theta4, theta5, theta6]theta_table = [theta1, theta2, theta3, theta4, theta5, theta6]# filter 为了给输出的角度进行平滑处理，所以对角度的变化进行了限制def theta_error(theta, theta_ver, g):dis = abs(theta - theta_ver)if dis < g:theta = thetaelse:if theta > theta_ver:theta = theta_ver + gelse:theta = theta_ver - greturn thetaif x == 0:theta_com = theta_tableelse:for n in xrange(3, 6):theta_table[n] = theta_error(theta_table[n], theta_com[n], 0.5)theta_com[n] = theta_table[n]print'modification OK!'# joint_trajectory_point.positions = [theta1, theta2, theta3, theta4, theta5, theta6]joint_trajectory_point.positions = theta_tablejoint_trajectory_list.append(joint_trajectory_point)rospy.loginfo("length of Joint Trajectory List: %s" % len(joint_trajectory_list))return CalculateIKResponse(joint_trajectory_list)def IK_server():# 初始化节点并声明calculate_ik服务rospy.init_node('IK_server')s = rospy.Service('calculate_ik', CalculateIK, handle_calculate_IK)print"Ready to receive an IK request"rospy.spin()if __name__ == "__main__":IK_server()

(2)部分代码解释

处理接受数据

# px,py,pz = end-effector position# roll, pitch, yaw = end-effector orientationpx = req.poses[x].position.xpy = req.poses[x].position.ypz = req.poses[x].position.z(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([req.poses[x].orientation.x, req.poses[x].orientation.y,req.poses[x].orientation.z, req.poses[x].orientation.w])

此处理从模拟器中接收到的数据。其中，旋转是以四元数的形式给出的，我们使用euler_from_quaternion函数来将其转换成RPY（ Roll, Pitch, Yaw）角度。

计算除球型手臂外的theta值

# 借助三角形解出相关theta角的坐标side_a = 1.501side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))side_c = 1.25angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))angle_c = acos((side_b * side_b + side_a * side_a - side_c * side_c) / (2 * side_a * side_b))theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)theta3 = pi / 2 - (angle_b + 0.036)

如图所示，依照图中所示的三角形关系来计算其对应的角度的值。joint2、joint3和手腕中心位置WC已经标出。

如果将模型投影到yz平面上，则其相关的角度就如下图所示。

计算球型手腕的theta值

# 计算球型手臂的旋转矩阵R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})# 利用LU解算法计算逆矩阵R3_6 = R0_3.inv("LU") * G_rottheta4 = atan2(R3_6[2, 2], -R3_6[0, 2])theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2])theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])

以上计算的是球型手腕的组成theta4, theta5, theta6的值了。我感觉这几个角度非常棘手，如果我不使用对反向滤波器的结果的平滑处理的话，整个机械手的运动完全处于一个不稳的状态之中，尤其是theta4, theta5, theta6。

在进行任务的时候，我对实例所编写的计算欧拉角的程序很不能理解，这和前面所讲的欧拉角的计算所得出的结果是不一样的，而且误差都很大。因此我在IK_debug.py程序中试图找出一个最适合欧拉角计算方式，但是每次得出的计算结果很不一样，所以我只能对相关角度加上平滑处理，以得到一个较好的效果。

输出角度平滑处理

# filter 为了给输出的角度进行平滑处理，所以对角度的变化进行了限制def theta_error(theta, theta_ver, g):dis = abs(theta - theta_ver)if dis < g:theta = thetaelse:if theta > theta_ver:theta = theta_ver + gelse:theta = theta_ver - greturn thetaif x == 0:theta_com = theta_tableelse:for n in xrange(3, 6):theta_table[n] = theta_error(theta_table[n], theta_com[n], 0.5)theta_com[n] = theta_table[n]print'modification OK!'

这是对输出的角度经行平滑处理的过程，如果两个误差角度大于所设定的误差g，那么角度就会被限制与上个角度相差正负g的范围之内。然后对所有在pose中的角度进行处理。

4.反向运动学的调试与优化

(1)调试IK IK_debug.py

from sympy import *from time import timefrom mpmath import radiansimport tftest_cases = {1: [[[2.16135, -1.42635, 1.55109],[0.708611, 0.186356, -0.157931, 0.661967]],[1.89451, -1.44302, 1.69366],[-0.65, 0.45, -0.36, 0.95, 0.79, 0.49]],2: [[[-0.56754, 0.93663, 3.0038],[0.62073, 0.48318, 0.38759, 0.480629]],[-0.638, 0.64198, 2.9988],[-0.79, -0.11, -2.33, 1.94, 1.14, -3.68]],3: [[[-1.3863, 0.02074, 0.90986],[0.01735, -0.2179, 0.9025, 0.371016]],[-1.1669, -0.17989, 0.85137],[-2.99, -0.12, 0.94, 4.06, 1.29, -4.12]],4: [],5: []}def test_code(test_case):# Set up code# Do not modify!x = 0class Position:def __init__(self, EE_pos):self.x = EE_pos[0]self.y = EE_pos[1]self.z = EE_pos[2]class Orientation:def __init__(self, EE_ori):self.x = EE_ori[0]self.y = EE_ori[1]self.z = EE_ori[2]self.w = EE_ori[3]position = Position(test_case[0][0])orientation = Orientation(test_case[0][1])class Combine:def __init__(self, position, orientation):self.position = positionself.orientation = orientationcomb = Combine(position, orientation)class Pose:def __init__(self, comb):self.poses = [comb]req = Pose(comb)start_time = time()######################################################################################### Create symbolsq1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_1d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')# Create Modified DH parametersdh = {alpha0: 0, a0: 0, d1: 0.75,alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2,alpha2: 0, a2: 1.25, d3: 0,alpha3: - pi / 2, a3: -0.054, d4: 0,alpha4: pi / 2, a4: 0, d5: 1.50,alpha5: -pi / 2, a5: 0, d6: 0,alpha6: 0, a6: 0, d7: 0.303, q7: 0}# Define Modified DH Transformation matrixdef dh_tf(alpha, a, d, q):dh_matrix = Matrix([[cos(q), -sin(q), 0, a],[sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d],[sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d],[0, 0, 0, 1]])return dh_matrix# Create individual transformation matricesT0_1 = dh_tf(alpha0, a0, d1, q1).subs(dh)T1_2 = dh_tf(alpha1, a1, d2, q2).subs(dh)T2_3 = dh_tf(alpha2, a2, d3, q3).subs(dh)T3_4 = dh_tf(alpha3, a3, d4, q4).subs(dh)T4_5 = dh_tf(alpha4, a4, d5, q5).subs(dh)T5_6 = dh_tf(alpha5, a5, d6, q6).subs(dh)T6_G = dh_tf(alpha6, a6, d7, q7).subs(dh)T0_G = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_G# Extract rotation matrices from the transformation matrices# px, py, pz = end - effector position# roll, pitch, yaw = end - effector orientationpx = req.poses[x].position.xpy = req.poses[x].position.ypz = req.poses[x].position.z(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z,req.poses[x].orientation.w])# Find EE rotation matrixr, p, y = symbols('r p y')x_rot = Matrix([[1, 0, 0],[0, cos(r), -sin(r)],[0, sin(r), cos(r)]])y_rot = Matrix([[cos(p), 0, sin(p)],[0, 1, 0],[-sin(p), 0, cos(p)]])z_rot = Matrix([[cos(y), -sin(y), 0],[sin(y), cos(y), 0],[0, 0, 1]])G_rot = x_rot * y_rot * z_rotRot_Rrror = z_rot.subs(y, radians(180)) * y_rot.subs(p, radians(-90))G_rot = G_rot * Rot_RrrorG_rot = G_rot.subs({'r': roll, 'p': pitch, 'y': yaw})Gripper = Matrix([[px],[py],[pz]])WC = Gripper - (0.303) * G_rot[:, 2]# Calculate joint angles using Geometric IK method# More information van be found in the Inverse Kinematics with Kuka KR210theta1 = atan2(WC[1], WC[2])# 计算theta1，theta2，theta3side_a = 1.501side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))side_c = 1.25angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))angle_c = acos((side_b * side_b + side_a * side_a - side_c * side_c) / (2 * side_a * side_b))theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)theta3 = pi / 2 - (angle_b + 0.036) # 0.036 accounts for sag in link4 of -0.054mR0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})R3_6 = R0_3.inv("LU") * G_rot'''# 找到最小误差对应的旋转矩阵的值def err_s(A, theta_test, S):theta_err = 10for i in xrange(0, 3):for j in xrange(0, 3):for m in xrange(0, 3):for n in xrange(0, 3):for p in [1, -1]:for q in [1, -1]:theta = atan2(p * A[i, j], q * A[m, n])if abs(theta - theta_test) < theta_err:end = [i, j, m, n, p, q]theta_err = abs(theta - theta_test)else:continueprint('result', end, 'err:', abs(theta_err - theta_test))return atan2(end[4] * A[end[0], end[1]], end[5] * A[end[2], end[3]])theta4 = err_s(R3_6, test_case[2][3], 1)theta6 = err_s(R3_6, test_case[2][5], 1)'''# 旋转矩阵的欧拉角，在前面的笔记中提到过# 从旋转矩阵中提取欧拉角，然后得出theta4， 5， 6的值theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2])theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])theta_table = {q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6}######################################################################################### 正向运动学FK = T0_G.evalf(subs=theta_table)######################################################################################### 机械手中心位置坐标your_wc = [WC[0], WC[1], WC[2]]# 夹持器坐标your_ee = [FK[0, 3], FK[1, 3],FK[2, 3]]######################################################################################### 误差分析print("\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time() - start_time))# 手腕中心的误差if not (sum(your_wc) == 3):wc_x_e = abs(your_wc[0] - test_case[1][0])wc_y_e = abs(your_wc[1] - test_case[1][1])wc_z_e = abs(your_wc[2] - test_case[1][2])wc_offset = sqrt(wc_x_e ** 2 + wc_y_e ** 2 + wc_z_e ** 2)print("\nWrist error for x position is: %04.8f" % wc_x_e)print("Wrist error for y position is: %04.8f" % wc_y_e)print("Wrist error for z position is: %04.8f" % wc_z_e)print("Overall wrist offset is: %04.8f units" % wc_offset)# 由反向运动学计算得出的角度误差t_1_e = abs(theta1 - test_case[2][0])t_2_e = abs(theta2 - test_case[2][1])t_3_e = abs(theta3 - test_case[2][2])t_4_e = abs(theta4 - test_case[2][3])t_5_e = abs(theta5 - test_case[2][4])t_6_e = abs(theta6 - test_case[2][5])print("\nTheta 1 error is: %04.8f" % t_1_e)print("Theta 2 error is: %04.8f" % t_2_e)print("Theta 3 error is: %04.8f" % t_3_e)print("Theta 4 error is: %04.8f" % t_4_e)print("Theta 5 error is: %04.8f" % t_5_e)print("Theta 6 error is: %04.8f" % t_6_e)print("\n**These theta errors may not be a correct representation of your code, due to the fact \\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \\nconfirm whether your code is working or not**")print(" ")# 由正向运动学计算得出的夹持器误差if not (sum(your_ee) == 3):ee_x_e = abs(your_ee[0] - test_case[0][0][0])ee_y_e = abs(your_ee[1] - test_case[0][0][1])ee_z_e = abs(your_ee[2] - test_case[0][0][2])ee_offset = sqrt(ee_x_e ** 2 + ee_y_e ** 2 + ee_z_e ** 2)print("\nEnd effector error for x position is: %04.8f" % ee_x_e)print("End effector error for y position is: %04.8f" % ee_y_e)print("End effector error for z position is: %04.8f" % ee_z_e)print("Overall end effector offset is: %04.8f units \n" % ee_offset)if __name__ == "__main__":# Change test case number for different scenariostest_case_number = 1test_code(test_cases[test_case_number])

(2)相关代码解释

测试用例

在调试脚本中，我们为您提供了一些测试用例，您可以根据这些测试用例交叉检查已实现的IK代码。

test_cases = {1:[[[2.16135,-1.42635,1.55109],[0.708611,0.186356,-0.157931,0.661967]],[1.89451,-1.44302,1.69366],[-0.65,0.45,-0.36,0.95,0.79,0.49]],2:[[[-0.56754,0.93663,3.0038],[0.62073, 0.48318,0.38759,0.480629]],[-0.638,0.64198,2.9988],[-0.79,-0.11,-2.33,1.94,1.14,-3.68]],3:[[[-1.3863,0.02074,0.90986],[0.01735,-0.2179,0.9025,0.371016]],[-1.1669,-0.17989,0.85137],[-2.99,-0.12,0.94,4.06,1.29,-4.12]],4:[],5:[]}

每种情况的格式结构如下：

[[[EE position], [EE orientation as quaternions]], [WC location], [Joint angles]]

其中，

EE是夹持器，

WC是腕部中心，

关节角度为θ1\theta1θ1~θ6\theta6θ6的弧度。

或者可以通过启动正向运动学演示来生成其他测试用例，如“ 调试正向运动学”部分中所述。使用的滑块joint_state_publisher设置一组特定的关节角度，并获得相应的EE位置和方向，如下所示：

IK代码

该test_code()函数将特定的测试用例作为输入，并打印出所提供的测试值与从逆运动学代码获得的计算值之间的相应误差。一旦添加了IK代码（以及相关的FK代码）并计算了必要的关节角度，就可以在提供的变量中替换计算出的手腕中心位置。

q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_1d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')dh = {alpha0: 0, a0: 0, d1: 0.75,alpha1: -pi / 2, a1: 0.35, d2: 0, q2: q2 - pi / 2,alpha2: 0, a2: 1.25, d3: 0,alpha3: - pi / 2, a3: -0.054, d4: 0,alpha4: pi / 2, a4: 0, d5: 1.50,alpha5: -pi / 2, a5: 0, d6: 0,alpha6: 0, a6: 0, d7: 0.303, q7: 0}def dh_tf(alpha, a, d, q):dh_matrix = Matrix([[cos(q), -sin(q), 0, a],[sin(q) * cos(alpha), cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d],[sin(q) * sin(alpha), cos(q) * sin(alpha), cos(alpha), cos(alpha) * d],[0, 0, 0, 1]])return dh_matrixT0_1 = dh_tf(alpha0, a0, d1, q1).subs(dh)T1_2 = dh_tf(alpha1, a1, d2, q2).subs(dh)T2_3 = dh_tf(alpha2, a2, d3, q3).subs(dh)T3_4 = dh_tf(alpha3, a3, d4, q4).subs(dh)T4_5 = dh_tf(alpha4, a4, d5, q5).subs(dh)T5_6 = dh_tf(alpha5, a5, d6, q6).subs(dh)T6_G = dh_tf(alpha6, a6, d7, q7).subs(dh)T0_G = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_Gpx = req.poses[x].position.xpy = req.poses[x].position.ypz = req.poses[x].position.z(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([req.poses[x].orientation.x, req.poses[x].orientation.y, req.poses[x].orientation.z,req.poses[x].orientation.w])r, p, y = symbols('r p y')x_rot = Matrix([[1, 0, 0],[0, cos(r), -sin(r)],[0, sin(r), cos(r)]])y_rot = Matrix([[cos(p), 0, sin(p)],[0, 1, 0],[-sin(p), 0, cos(p)]])z_rot = Matrix([[cos(y), -sin(y), 0],[sin(y), cos(y), 0],[0, 0, 1]])G_rot = x_rot * y_rot * z_rotRot_Rrror = z_rot.subs(y, radians(180)) * y_rot.subs(p, radians(-90))G_rot = G_rot * Rot_RrrorG_rot = G_rot.subs({'r': roll, 'p': pitch, 'y': yaw})Gripper = Matrix([[px],[py],[pz]])WC = Gripper - (0.303) * G_rot[:, 2]theta1 = atan2(WC[1], WC[2])side_a = 1.501side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))side_c = 1.25angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))angle_c = acos((side_b * side_b + side_a * side_a - side_c * side_c) / (2 * side_a * side_b))theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)theta3 = pi / 2 - (angle_b + 0.036) # 0.036 accounts for sag in link4 of -0.054mR0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})R3_6 = R0_3.inv("LU") * G_rottheta4 = atan2(R3_6[2, 2], -R3_6[0, 2])theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]), R3_6[1, 2])theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])theta_table = {q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6}

此部分是IK的代码，和IK_server.py中几乎完全一样

计算得出最小误差

# 找到最小误差对应的旋转矩阵的值def err_s(A, theta_test, S):theta_err = 10for i in xrange(0, 3):for j in xrange(0, 3):for m in xrange(0, 3):for n in xrange(0, 3):for p in [1, -1]:for q in [1, -1]:theta = atan2(p * A[i, j], q * A[m, n])if abs(theta - theta_test) < theta_err:end = [i, j, m, n, p, q]theta_err = abs(theta - theta_test)else:continueprint('result', end, 'err:', abs(theta_err - theta_test))return atan2(end[4] * A[end[0], end[1]], end[5] * A[end[2], end[3]])theta4 = err_s(R3_6, test_case[2][3], 1)theta6 = err_s(R3_6, test_case[2][5], 1)

上面说过，计算球型手臂的theta值是一个很棘手的问题。样例给出的计算方法得出的误差还是很大。于是我利用穷举法来计算得到最小误差时使用的矩阵的值。但是每次计算出来的结果都不相同，最终我放弃了这个想法，在这里码一下，或许以后有机会能解决这个问题。

其余代码将利用手腕中心位置和theta来计算相应的误差。