UR机械臂位置变换与力变换

UR机械臂位置变换与力变换

• UR机械臂位置变换与力变换
  • 位置变换
  • 力变换

开始之前，你已经实例化ur_control和ur_receive,代码如下.

ur_control = RTDEControl(your_ip, your_frequency)
ur_receive = RTDEReceive(your_ip, your_frequency)

位置变换

​ 在对UR机械臂位置控制时，我们需要沿着工具端坐标系对机械臂进行控制，但是，对于python发送的命令，是基于基坐标系的位置坐标，因此我们需要获取当前TCP的位置，和需要移动的偏移量，最后计算到对应极坐标系下的绝对位置。

有两种代码形式实现当前功能。

方法一：获取当前TCP位置和相对于工具端的偏移量offset。

current_pose = ur_receive.getActualTCPForce()
offset_pose = [0,0,0.2,0,0,0]
target_pose = ur_control.poseTrans(current_pose, offset_pose)

​ 在这段代码中，代码getActualTCPForce()返回相对于TCP 的位置，是一个六维的向量，其中包含[x,y,z,rx,ry,rz]。poseTrans()进行位置变换，第一个参数表示相对于某个平面，可以是空间中任意一个平面，在这里，我们输入的是当前工具端所在的平面。

方法二：获取当前TCP位置和相对于工具端的偏移量offset。

current_pose = ur_control.getForwardKinematics()
offset_pose = [0,0,0.2,0,0,0]
target_pose = ur_control.poseTrans(current_pose, offset_pose)

​ 在这段代码中，首先，先介绍以下getForwardKinematics函数的使用方法。后面代码与方法一相同。

    def getForwardKinematics(self, q, p_float=None, *args, **kwargs): # real signature unknown; NOTE: unreliably restored from __doc__ 
        """
        getForwardKinematics(self: rtde_control.RTDEControlInterface, q: List[float] = [], tcp_offset: List[float] = []) -> List[float]
        
        Calculate the forward kinematic transformation (joint space -> tool
        space) using the calibrated robot kinematics. If no joint position
        vector is provided the current joint angles of the robot arm will be
        used. If no tcp is provided the currently active tcp of the controller
        will be used.
        
        NOTICE! If you specify the tcp_offset you must also specify the q.
        
        Parameter ``q``:
            joint position vector (Optional)
        
        Parameter ``tcp_offset``:
            tcp offset pose (Optional)
        
        Returns:
            the forward kinematic transformation as a pose
        """
        pass

​ 该函数是将关节位置转变为工具位置，如果没有输入关节位置q，则q会赋值为当前的关节位置，返回相对于工具在基坐标系下的位置坐标。

力变换

​ 在UR机械臂中，我们使用内置函数返回的是相对于基坐标系下的数据，我们需要将该数据转换为相对于工具端力的数据。

​ 同样，我们使用两种不同的语言实现该功能。

方法一：使用UR 脚本语言实现。

​ 参考UR论坛连接：https://www.universal-robots.com/articles/ur/programming/get-force-and-torque-values-in-tool-coordinate-system/

force_torque≔get_tcp_force()
force_B≔p[force_torque[0],force_torque[1],force_torque[2],0,0,0]
torque_B≔p[force_torque[3],force_torque[4],force_torque[5],0,0,0]
tcp≔get_actual_tcp_pose()
rotation_BT≔p[0,0,0,tcp[3],tcp[4],tcp[5]]
force_T≔pose_trans(pose_inv(rotation_BT), force_B)
torque_T≔pose_trans(pose_inv(rotation_BT), torque_B)
force_torque_T≔p[force_T[0],force_T[1],force_T[2],torque_T[0],torque_T[1],torque_T[2]]

方法二：在python和c++中，rtde没有实现上述pose_inv的内置函数，因此需要自己修改代码实现该功能。

​ 参考UR论坛链接：https://forum.universal-robots.com/t/pose-trans-and-pose-inv-functions-in-urcap/1257/5

​ 参考Github代码链接：https://gist.github.com/pweids/8f1b041ed4c308c40c8dda5df0721f31

​ 下面是我使用python代码实现了该功能：

#获得旋转矩阵
def get_RotationMatrix(current_pose):
    angle = np.sqrt(
        current_pose[3] * current_pose[3] + current_pose[4] * current_pose[4] + current_pose[5] * current_pose[5])
    if angle == 0.0:
        rotation_matrix_y = 0
        rotation_matrix_z = 0
        rotation_matrix_x = 1
    else:
        rotation_matrix_x = current_pose[3] / angle
        rotation_matrix_y = current_pose[4] / angle
        rotation_matrix_z = current_pose[5] / angle

    cos_data = np.cos(angle)
    sin_data = np.sin(angle)
    t = 1.0 - cos_data

    rotation_matrix = (cos_data * np.eye(3) +t * np.array(
                [[rotation_matrix_x * rotation_matrix_x, rotation_matrix_x * rotation_matrix_y,rotation_matrix_x * rotation_matrix_z],
                 [rotation_matrix_x * rotation_matrix_y, rotation_matrix_y * rotation_matrix_y,rotation_matrix_y * rotation_matrix_z],
                 [rotation_matrix_x * rotation_matrix_z, rotation_matrix_y * rotation_matrix_z,rotation_matrix_z * rotation_matrix_z]]) +
                       sin_data * np.array(
                [[0, -rotation_matrix_z, rotation_matrix_y],
                 [rotation_matrix_z, 0, -rotation_matrix_x],
                 [-rotation_matrix_y, rotation_matrix_x, 0]]))

    return rotation_matrix
    
#进行力变换，代码结构与方法一相同
force_B, torque_B = current_force[:3], current_force[3:6]
rotation_matrix = get_RotationMatrix(current_pose)
#求逆运算
rotation_matrix_inv = np.linalg.inv(rotation_matrix)
force_T, torque_T = np.dot(rotation_matrix_inv, force_B.T), np.dot(rotation_matrix_inv, torque_B.T)
force_torque_T = np.hstack((force_T, torque_T))

​ 若有表述错误，请指正，谢谢。