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

1.反向运动学概述

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

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

定义齐次变换如下：

其中l,m和n是正交向量，代表沿着局部坐标系的X，Y，Z轴的末端执行器的方向。
由于n是沿着的z轴的向量 $gripper\_link$ ,我们可以得出：

其中,

$p_x$ ， $p_y$ ， $p_z$ =末端执行器位置

$w_x$ ， $w_y$ ， $w_z$ =手腕位置

$d_6$ =来自DH参数表

$l$ =末端执行器长度

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

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

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

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

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

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

然后可以从此Rrpy矩阵中提取 $n_x$ ， $n_y$ 和 $n_z$ 的值，以获取手腕中心位置。

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

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

$2$ 、 $3$ 、 $w c$ 分别为关节2、关节3、手腕中心。如果把关节投射到z-y平面上，就可以得到，或者更形象地说，三个关节之间的三角形。根据DH参数，可以计算上面每个关节之间的距离。利用三角函数，特别是余弦定理，可以计算出 $θ2\theta_2$ 和 $θ3\theta_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
# request
geometry_msgs/Pose[] poses
---
# response
trajectory_msgs/JointTrajectoryPoint[] points

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

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

# import modules
import rospy
import tf
from kuka_arm.srv import *
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from geometry_msgs.msg import Pose
from mpmath import *
from sympy import *

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

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

来自 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 python
import rospy
import tf
from kuka_arm.srv import *
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from geometry_msgs.msg import Pose
from 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 time
from mpmath import radians
import 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$ ~ $θ6\theta6$ 的弧度。

或者可以通过启动正向运动学演示来生成其他测试用例，如“ 调试正向运动学”部分中所述。使用的滑块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来计算相应的误差。

（3）优化

在进行此项目时，即使完成拾取或放下单个对象的任何特定动作，也要花费大量时间才能完成模拟。可以通过某些方法来改善总仿真时间。

Sympy计算
在当前IK_server.py脚本中，大多数涉及sympy计算的代码都在main之外for loop。Sympy可能需要花费大量时间来乘法矩阵，因此，实现的正向运动学部分会降低速度。因此，为了确保不会发生这种情况，最好遵循当前结构并使FK实现在之外for loop。

对于IK，可能不需要为每个接收到的姿势运行某些矩阵乘法，因此可以在循环外进行处理。例如，获得围绕任何轴的通用旋转矩阵。
除调试目的外，不需要此simplify功能。有时这可能会增加计算量，可以考虑不使用它。
一个有用的技巧很有帮助，那就是在乘以任何矩阵之前，将所有符号变量（例如旋转角度）替换为其对应的值。

虚拟机
与本地设置相比，虚拟机在计算能力方面往往会受到限制。最小化此差距的最佳方法是确保分配尽可能多的资源。增加内存（RAM），处理器数量和图形内存通常可以帮助缩小差距。

5.运行及结果

运行过程可以参考这里。

可以通过以下方式启动项目

$ CD  〜 / catkin_ws / src目录/ ROBOND运动学项目/ kuka_arm /脚本
$ ./safe_spawner.sh

要运行自己的Inverse Kinematics代码，请将/RoboND-Kinematics-Project/kuka_arm/launch文件夹下target_description.launch 中的demo标记全更改为“ false”，然后通过以下方式运行代码（一旦成功加载了项目）：

$ CD  〜 / catkin_ws / src目录/ ROBOND运动学项目/ kuka_arm /脚本
$ rosrun kuka_arm IK_server.py

最终效果如下：

ROS机械臂仿真控制-基于Udacity的Kuka KR210仿真模拟器

Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反向运动学相关推荐

Udacity机器人软件工程师课程笔记（八）-ROS Turtlesim 包的相关命令
Turtlesim 包的相关命令这个部分包含五个子主题,分别是列出所有活动节点列出所有主题获取有关主题的信息显示消息信息实时回应消息 1.列出所有的活动节点为了获取所用的活动且向ROS ...
Udacity机器人软件工程师课程笔记（五）-样本搜索和找回-基于漫游者号模拟器-自主驾驶
9.自主驾驶在接下来的环节中,我们要实现漫游者号的自动驾驶功能. 完成这个功能我们需要四个程序,第一个为感知程序,其对摄像头输入的图片进行变换处理和坐标变换使用.第二个程序为决策程序,功能是帮助漫游 ...
Udacity机器人软件工程师课程笔记（十六）-机械臂仿真控制实例（其一）-Gazebo、RViz和Moveit！
机械臂仿真控制实例目录环境设置项目工具介绍 Gazebo (1)Gazebo组件 (2)Gazebo界面统一机器人描述格式(URDF) RViz Moveit! 1.环境设置对于此项目,使用 ...
Udacity机器人软件工程师课程笔记（七）-ROS介绍和Turtlesim包的使用
Robotics Software engineer笔记 1.ROS简介与虚拟机配置 (1)ROS简介 ROS是一款机器人软件框架,即机器人操作系统(Robot Operating System). ...
Udacity机器人软件工程师课程笔记（二十八) - 卷积神经网络实例 - Fashion-MNIST数据集
1.Fashion-MNIST数据集 Fashion-MNIST数据集包括一个包含60,000个示例的训练集和一个包含10,000个示例的测试集.每个示例是一个28x28灰度图像,与来自以下10个类的 ...
Udacity机器人软件工程师课程笔记（十）-ROS-Catkin-包（package）和gazebo
包和gazebo仿真 1.添加包 (1)克隆simple_arm包克隆现有的包并将其添加到我们新创建的工作区. 首先导航到src目录,然后从其github仓库克隆本课程 simple_arm 的包. ...
Udacity机器人软件工程师课程笔记（二十四) - 控制（其二） - PID优化，梯度下降算法，带噪声的PID控制
7.非理想情况 (1)积分饱和到目前为止,我们一直使用的"理想"形式的PID控制器很少用于工业中."时间常数"形式更为常见. 当前说明了理想形式的一些重大缺陷 ...
Udacity机器人软件工程师课程笔记（十五）-运动学-正向运动学和反向运动学(其二)-DH参数等
正向运动学和反向运动学目录 2D中的旋转矩阵 sympy包旋转的合成旋转矩阵中的欧拉角平移齐次变换及其逆变换齐次变换的合成 Denavit-Hartenberg 参数 DH参数分配算法正 ...
Udacity机器人软件工程师课程笔记（三十六) - GraphSLAM
一.引入 GraphSLAM是解决完整的slam问题的slam算法.这意味着该算法将恢复整个路径和地图,而不仅仅是最近的姿势和地图.这种差异使它可以考虑当前姿势与先前姿势之间的依赖性.适用于我们的Gr ...

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