网站制作自学百度云,如何开一家公司流程,浙江 网站建设,环保材料 技术支持 东莞网站建设机械臂仿真控制实例#xff08;其二#xff09;-KR210正向运动学
目录
反向运动学概述为Kuka KR210创建IK解算器
1.反向运动学概述
KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节。这种设计称为球形腕#xff0c;而相交的公共点称为腕中心。这种设…机械臂仿真控制实例其二-KR210正向运动学
目录
反向运动学概述为Kuka KR210创建IK解算器
1.反向运动学概述
KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节。这种设计称为球形腕而相交的公共点称为腕中心。这种设计的优点在于它在运动学上解耦了末端执行器的位置和方向。 j o i n t _ 5 joint\_5 joint_5是球形手腕的公共交点因此是手腕的中心。
首先我们要解反向位置问题我们涉及到球形手腕 j o i n t 4 , 5 , 6 joint 4,5,6 joint4,5,6手腕的中心位置由前三个关节控制通过使用基于末端执行器得出的完整的齐次变换矩阵我们可以获得手腕中心的位置。
定义齐次变换如下
其中l,m和n是正交向量代表沿着局部坐标系的XYZ轴的末端执行器的方向。 由于n是沿着的z轴的向量 g r i p p e r _ l i n k gripper\_link gripper_link,我们可以得出 其中, p x p_x px p y p_y py p z p_z pz末端执行器位置 w x w_x wx w y w_y wy w z w_z wz 手腕位置 d 6 d_6 d6 来自DH参数表 l l l 末端执行器长度
现在为了计算 n x n_x nx n y n_y ny和 n z n_z nz让我们从上个笔记继续在上个笔记中我们计算了旋转矩阵并更正末端执行器的URDF和DH参数之间的差异。
获取此校正后的旋转矩阵后需要接下来计算相对于的末端执行器 b a s e _ l i n k base\_link 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_x nx n y n_y ny和 n z n_z nz的值以获取手腕中心位置。
现在已经获得了中心位置按照上面介绍的公式得出前三个关节的方程式。 θ 1 \theta_1 θ1一旦从上方获得手腕中心位置则计算将相对简单。 θ 2 \theta_2 θ2h和 θ 3 \theta_3 θ3可视化相对比较麻烦。下图描绘了相关角度。 2 2 2、 3 3 3、 w c wc wc分别为关节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的计算值替换为它们各自的旋转矩阵并用invR0_3乘以上述方程式的两边得出
R3_6 inv(R0_3) * Rrpy在RHSRight 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 requestrospy.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 receivedreturn -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:printNo valid poses receivedreturn [] # 如果按教程给的-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]printmodification 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)printReady to receive an IK requestrospy.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]printmodification 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])# 计算theta1theta2theta3side_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(substheta_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来计算相应的误差。
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仿真模拟器
