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

最新推荐文章于 2024-03-07 10:30:13 发布

Stan Fu

最新推荐文章于 2024-03-07 10:30:13 发布

阅读量1.5k

点赞数 1

分类专栏：机器人软件工程文章标签： ROS 机械臂运动学仿真机器人

本文链接：https://blog.csdn.net/qq_37266917/article/details/103090113

版权

机器人软件工程专栏收录该内容

35 篇文章 50 订阅

订阅专栏

机械臂仿真控制实例（其二）-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$ 的值，以获取手腕中心位置。

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

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

在这里插入图片描述 $2$ 、 $3$ 、 $w c$ 分别为关节2、关节3、手腕中心。如果把关节投射到z-y平面上，就可以得到，或者更形象地说，三个关节之间的三角形。根据DH参数，可以计算上面每个关节之间的距离。利用三角函数，特别是余弦定理，可以计算出 $\theta_2$ 和 $\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 service
    rospy.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 -1
    else:
        ...
        joint_trajectory_list = []
        for x in xrange(0, len(req.poses)):
            # IK code starts here
            joint_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 symbols
        q1, 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_matrix

        def rot_mat(martix):
            rot_mat = martix[0:2, 0:2]
            return rot_mat

        T0_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

        joint_trajectory_list = []
        for x in xrange(0, len(req.poses)):
            # IK code starts here
            joint_trajectory_point = JointTrajectoryPoint()

            # px,py,pz = end-effector position
            # roll, pitch, yaw = end-effector orientation
            px = req.poses[x].position.x
            py = req.poses[x].position.y
            pz = 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_Rrror
            G_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.501
            side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))
            side_c = 1.25

            angle_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_rot

            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])

            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 = theta
                else:
                    if theta > theta_ver:
                        theta = theta_ver + g
                    else:
                        theta = theta_ver - g
                return theta

            if x == 0:
                theta_com = theta_table
            else:
                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_table
            joint_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 orientation
            px = req.poses[x].position.x
            py = req.poses[x].position.y
            pz = 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.501
            side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))
            side_c = 1.25

            angle_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_rot

            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])

以上计算的是球型手腕的组成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 = theta
                else:
                    if theta > theta_ver:
                        theta = theta_ver + g
                    else:
                        theta = theta_ver - g
                return theta

            if x == 0:
                theta_com = theta_table
            else:
                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 tf


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: []}


def test_code(test_case):
    # Set up code
    # Do not modify!
    x = 0

    class 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 = position
            self.orientation = orientation

    comb = Combine(position, orientation)

    class Pose:
        def __init__(self, comb):
            self.poses = [comb]

    req = Pose(comb)
    start_time = time()

    ########################################################################################
    # Create symbols
    q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')  # theta_1
    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')

    # Create Modified DH parameters
    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}

    # Define Modified DH Transformation matrix
    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_matrix

    # Create individual transformation matrices
    T0_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 orientation
    px = req.poses[x].position.x
    py = req.poses[x].position.y
    pz = 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 matrix
    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))

    G_rot = G_rot * Rot_Rrror
    G_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 KR210
    theta1 = atan2(WC[1], WC[2])

    # 计算theta1，theta2，theta3
    side_a = 1.501
    side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))
    side_c = 1.25

    angle_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.054m

    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})

    R3_6 = R0_3.inv("LU") * G_rot
    '''
    # 找到最小误差对应的旋转矩阵的值
    def err_s(A, theta_test, S):
        theta_err = 10
        for 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:
                                    continue
        print('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 scenarios
    test_case_number = 1

    test_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是腕部中心，

关节角度为 $\theta1$ ~ $\theta6$ 的弧度。

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

IK代码

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

    q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')  # theta_1
    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_matrix

    T0_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
    
    px = req.poses[x].position.x
    py = req.poses[x].position.y
    pz = 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_rot

    Rot_Rrror = z_rot.subs(y, radians(180)) * y_rot.subs(p, radians(-90))

    G_rot = G_rot * Rot_Rrror
    G_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.501
    side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) + pow((WC[2] - 0.75), 2))
    side_c = 1.25

    angle_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.054m

    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})

    R3_6 = R0_3.inv("LU") * G_rot

    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}

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

计算得出最小误差

    # 找到最小误差对应的旋转矩阵的值
    def err_s(A, theta_test, S):
        theta_err = 10
        for 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:
                                    continue
        print('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仿真模拟器

Stan Fu

关注

1
点赞
踩
8

收藏

觉得还不错? 一键收藏
打赏
0
评论
Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反向运动学

机械臂仿真控制实例（其二）-KR210正向运动学目录反向运动学概述为Kuka KR210创建IK解算器1.反向运动学概述KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节。这种设计称为球形腕，而相交的公共点称为腕中心。这种设计的优点在于，它在运动学上解耦了末端执行器的位置和方向。joint_5joint\_5joint_5是球形手腕的公共交点，因此是手腕的中心。...
复制链接

扫一扫