Downloads: 1
Diff of
/URBasic/kinematic.py
[000000]
..
[c1b1c5]
Switch to side-by-side view
--- a +++ b/URBasic/kinematic.py @@ -0,0 +1,583 @@ +''' +Python 3.x library to control an UR robot through its TCP/IP interfaces +Copyright (C) 2017 Martin Huus Bjerge, Rope Robotics ApS, Denmark + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software +and associated documentation files (the "Software"), to deal in the Software without restriction, +including without limitation the rights to use, copy, modify, merge, publish, distribute, +sublicense, and/or sell copies of the Software, and to permit persons to whom the Software +is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies +or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR +PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used +in advertising or otherwise to promote the sale, use or other dealings in this Software +without prior written authorization from "Rope Robotics ApS". +''' +__author__ = "Martin Huus Bjerge" +__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark" +__license__ = "MIT License" + +import ikpy as ik +import numpy as np +import sympy as sp +import math +import scipy +from scipy import linalg +from URBasic.manipulation import * + +pi = np.pi + + +# Disable the logging stream from ikpy +# ik.logs.manager.removeHandler(ik.logs.stream_handler) + + +def Forwardkin_manip(joints, rob='ur10'): + ''' + This function solves forward kinematics, it returns pose vector + ''' + M, Slist = Robot_parameter_screw_axes(rob) + thetalist = joints + fk = FKinFixed(M, Slist, thetalist) + return np.round(Tran_Mat2Pose(Tran_Mat=fk), 4) + + +def Invkine_manip(target_pos, init_joint_pos=[0, 0, 0, 0, 0, 0], rob='ur10', tcpOffset=[0, 0, 0, 0, 0, 0]): + ''' + A numerical inverse kinematics routine based on Newton-Raphson method. + Takes a list of fixed screw axes (Slist) expressed in end-effector body frame, the end-effector zero + configuration (M), the desired end-effector configuration (T_sd), an initial guess of joint angles + (thetalist_init), and small positive scalar thresholds (wthresh, vthresh) controlling how close the + final solution thetas must be to the desired thetas. + ''' + M, Slist = Robot_parameter_screw_axes(rob) + wthresh = 0.001 + vthresh = 0.0001 + # T_sd = Pose2Tran_Mat(pose=target_pos) + T_target = Pose2Tran_Mat(pose=target_pos) + T_tcp = Pose2Tran_Mat(pose=tcpOffset) + T_sd = np.matmul(T_target, np.linalg.inv(T_tcp)) + thetalist_init = init_joint_pos + + ik_init = np.round(IKinFixed(Slist, M, T_sd, thetalist_init, wthresh, vthresh), 3) + ik = ik_init[-1][:] # choose the closest solution + + error = [] + for kk in ik_init: + out = [] + for ii in range(6): + k = np.round((kk[ii] - init_joint_pos[ii]) / 2 / np.pi) + out.append(kk[ii] - k * 2 * np.pi) + # change them to numpy array for np.sum operation + init_joint_pos = np.array(init_joint_pos) + out = np.array(out) + error.append(np.abs(np.sum(init_joint_pos - out))) + + ix = error == np.min(error) + ix = np.where(ix == True)[0][0] + ik = ik_init[ix][:] # choose the closest solution + + out = [] + for ii in range(6): + k = np.round((ik[ii] - init_joint_pos[ii]) / 2 / np.pi) + out.append(ik[ii] - k * 2 * np.pi) + + # error = (pi+ik-init_joint_pos)%(2*pi)-pi + + # print('***************************************************************************') + # print('Pose : [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*target_pos)) + # print('Init q: [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*init_joint_pos)) + # print('---------------------------------------------------------------------------') + # print(ik_init) + # print('---------------------------------------------------------------------------') + # # print(error) + # # print('---------------------------------------------------------------------------') + # print('Joints Candidate: [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*ik)) + # print('Joints Returned: [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*out)) #init_joint_pos+error)) + # print('***************************************************************************') + + return out + # return init_joint_pos+error + + +def rotate_tcp(gradient_vec=[0, 0, 0]): + ''' + Convert gradient vector to axis angle vector + + Parameters: + gradient_vec (3D float): [dx, dy, dz] displacement in x, y, z + + Return value: + axis vector (3D float): [rx, ry, rz] + + Exampel: + Rotate 5deg around x axis and 10 deg around y axis + dz = 0.2 + dy = np.tan(10/180*np.pi)*dz + dx = np.tan(5/180*np.pi)*dz + + r = rotate_tcp(gradient_vec=[dx,dy,dz]) + ''' + + v_0 = [0, 0, 1] # this is the UR zero degree angle in the base frame + v_in_u = gradient_vec / np.linalg.norm(gradient_vec) + + v1_u = v_in_u / np.linalg.norm(v_in_u) + v2_u = v_0 / np.linalg.norm(v_0) + rot_angle = np.arccos(np.clip(np.dot(v1_u, v2_u), -1.0, 1.0)) + + min_angle = 0.01 + + if rot_angle < min_angle: + rotation_vec = [0, 0, 0] + elif rot_angle > np.pi - min_angle: + rotation_vec = [0, -np.pi, 0] + else: + vec_norm = np.cross(v_0, v_in_u) + vec_norm = vec_norm / np.linalg.norm(vec_norm) + rotation_vec = vec_norm * rot_angle + + return rotation_vec + + +def Robot_parameter_screw_axes(rob='ur10'): + ''' + This function defines robot with fixed screw axes(used in manipulation.py) + rob='ur5' : ur5 + rob='ur10' : ur10 + rob='ur3e' : ur3e + https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/actual-center-of-mass-for-robot-17264/ + ''' + if str(rob).lower() == 'ur5': + M_ur5 = [[1, 0, 0, -.81725], [0, 0, -1, -.19145], [0, 1, 0, -.0055], [0, 0, 0, 1]] + S1_ur5 = [0, 0, 1, 0, 0, 0] + S2_ur5 = [0, -1, 0, .089159, 0, 0] + S3_ur5 = [0, -1, 0, .089159, 0, .425] + S4_ur5 = [0, -1, 0, .089159, 0, .81725] + S5_ur5 = [0, 0, -1, .10915, -.81725, 0] + S6_ur5 = [0, -1, 0, -.0055, 0, .81725] + Slist_ur5 = [S1_ur5, S2_ur5, S3_ur5, S4_ur5, S5_ur5, S6_ur5] + return M_ur5, Slist_ur5 + elif str(rob).lower() == 'ur10': + M_ur10 = [[1, 0, 0, -1.1843], [0, 0, -1, -0.2561], [0, 1, 0, 0.0116], [0, 0, 0, 1]] + S1_ur10 = [0, 0, 1, 0, 0, 0] + S2_ur10 = [0, -1, 0, .1273, 0, 0] + S3_ur10 = [0, -1, 0, .1273, 0, .612] + S4_ur10 = [0, -1, 0, .1273, 0, 1.1843] + S5_ur10 = [0, 0, -1, .16394, -1.1843, 0] + S6_ur10 = [0, -1, 0, 0.01165, 0, 1.1843] + Slist_ur10 = [S1_ur10, S2_ur10, S3_ur10, S4_ur10, S5_ur10, S6_ur10] + return M_ur10, Slist_ur10 + elif str(rob).lower() == 'ur3e': + M_ur3e = [[1, 0, 0, -0.45675], [0, 0, -1, -0.22315], [0, 1, 0, 0.06650], [0, 0, 0, 1]] + S1_ur3e = [0, 0, 1, 0, 0, 0] + S2_ur3e = [0, -1, 0, .15185, 0, 0] + S3_ur3e = [0, -1, 0, .15185, 0, .24355] + S4_ur3e = [0, -1, 0, .15185, 0, 0.45675] + S5_ur3e = [0, 0, -1, .13105, -0.45675, 0] + S6_ur3e = [0, -1, 0, 0.06655, 0, 0.45675] + Slist_ur3e = [S1_ur3e, S2_ur3e, S3_ur3e, S4_ur3e, S5_ur3e, S6_ur3e] + return M_ur3e, Slist_ur3e + else: + print('Wrong robot selected') + return False + + +def Robot_DH_Numerical(rob='ur10', joint=[0, 0, 0, 0, 0, 0]): + ''' + This function returns the DH parameter of a robot + rob='ur5' : ur5 + rob='ur10' : ur10 + joint: the robot joint vectors + ''' + j = joint + if str(rob).lower() == 'ur5': + dh_ur = np.matrix([ + [0, pi / 2, 0.089159, j[0]], + [-0.425, 0, 0, j[1]], + [-0.39225, 0, 0, j[2]], + [0, pi / 2, 0.10915, j[3]], + [0, -pi / 2, 0.09465, j[4]], + [0, 0, 0.0823, j[5]]]) + return dh_ur + elif str(rob).lower() == 'ur10': + dh_ur = np.matrix([ + [0, pi / 2, 0.1273, j[0]], + [-0.612, 0, 0, j[1]], + [-0.5723, 0, 0, j[2]], + [0, pi / 2, 0.163941, j[3]], + [0, -pi / 2, 0.1157, j[4]], + [0, 0, 0.0922, j[5]]]) + return dh_ur + else: + print('Wrong robot selected') + return + + +def Robot_DH_Symbol(rob='ur10'): + ''' + This function returns the DH parameter of a robot + rob='ur5' : ur5 + rob='ur10' : ur10 + ''' + # set up our joint angle symbols (6th angle doesn't affect any kinematics) + q = [sp.Symbol('q%i' % ii) for ii in range(6)] + if str(rob).lower() == 'ur5': + dh_ur = np.matrix([ + [0, pi / 2, 0.089159, q[0]], + [-0.425, 0, 0, q[1]], + [-0.39225, 0, 0, q[2]], + [0, pi / 2, 0.10915, q[3]], + [0, -pi / 2, 0.09465, q[4]], + [0, 0, 0.0823, q[5]]]) + return dh_ur + elif str(rob).lower() == 'ur10': + dh_ur = np.matrix([ + [0, pi / 2, 0.1273, q[0]], + [-0.612, 0, 0, q[1]], + [-0.5723, 0, 0, q[2]], + [0, pi / 2, 0.163941, q[3]], + [0, -pi / 2, 0.1157, q[4]], + [0, 0, 0.0922, q[5]]]) + return dh_ur + else: + print('Wrong robot selected') + return + + +def TransMatrix_DH_Symbol(rob='ur10', joint_num=6): + ''' + This function gives the transfer matrix of robot(DH method) + rob='ur5' : ur5 + rob='ur10' : ur10 + joint_num: the transform matrix for joint_num (from 1 to 6) + ''' + T = [] + # set up our joint angle symbols (6th angle doesn't affect any kinematics) + q = [sp.Symbol('q%i' % ii) for ii in range(joint_num)] + dh_ur = Robot_DH_Symbol(rob) + for ii in range(joint_num): + # descriptions terms in dh_ur + # dh_ur[:,0] = a or r + # dh_ur[:,1] = alpha + # dh_ur[:,2] = d + # dh_ur[:,3] = theta (q) + T.append(sp.Matrix([[sp.cos(dh_ur[ii, 3]), -sp.sin(dh_ur[ii, 3]) * sp.cos(dh_ur[ii, 1]), + sp.sin(dh_ur[ii, 3]) * sp.sin(dh_ur[ii, 1]), sp.cos(dh_ur[ii, 3]) * dh_ur[ii, 0]], + [sp.sin(dh_ur[ii, 3]), sp.cos(dh_ur[ii, 3]) * sp.cos(dh_ur[ii, 1]), + -sp.cos(dh_ur[ii, 3]) * sp.sin(dh_ur[ii, 1]), sp.sin(dh_ur[ii, 3]) * dh_ur[ii, 0]], + [0, sp.sin(dh_ur[ii, 1]), sp.cos(dh_ur[ii, 1]), dh_ur[ii, 2]], + [0, 0, 0, 1] + ])) + if joint_num == 1: + return T[0] + elif joint_num == 2: + return T[0] * T[1] + elif joint_num == 3: + return T[0] * T[1] * T[2] + elif joint_num == 4: + return T[0] * T[1] * T[2] * T[3] + elif joint_num == 5: + return T[0] * T[1] * T[2] * T[3] * T[4] + elif joint_num == 6: + return T[0] * T[1] * T[2] * T[3] * T[4] * T[5] + else: + print('Wrong joint number input') + return + + +def TransMatrix_DH_Numerical(rob='ur10', joint=[0, 0, 0, 0, 0, 0]): + ''' + This function gives the transfer matrix of robot(DH method) + rob='ur5' : ur5 + rob='ur10' : ur10 + joint: the robot joint vectors + ''' + T = [] + dh_ur = Robot_DH_Numerical(rob, joint) + for ii in range(6): + # descriptions terms in dh_ur + # dh_ur[:,0] = a or r + # dh_ur[:,1] = alpha + # dh_ur[:,2] = d + # dh_ur[:,3] = theta + T.append(np.matrix([[np.cos(dh_ur[ii, 3]), -np.sin(dh_ur[ii, 3]) * np.cos(dh_ur[ii, 1]), + np.sin(dh_ur[ii, 3]) * np.sin(dh_ur[ii, 1]), np.cos(dh_ur[ii, 3]) * dh_ur[ii, 0]], + [np.sin(dh_ur[ii, 3]), np.cos(dh_ur[ii, 3]) * np.cos(dh_ur[ii, 1]), + -np.cos(dh_ur[ii, 3]) * np.sin(dh_ur[ii, 1]), np.sin(dh_ur[ii, 3]) * dh_ur[ii, 0]], + [0, np.sin(dh_ur[ii, 1]), np.cos(dh_ur[ii, 1]), dh_ur[ii, 2]], + [0, 0, 0, 1] + ])) + return np.round(T[0] * T[1] * T[2] * T[3] * T[4] * T[5], 4) + + +def Jacobian_Symbol(rob='ur10', joint_num=6): + ''' + This function returns a 6*6 symbolic jacobian matrix + Tx: transfermation matrix + ''' + # set up our joint angle symbols (6th angle doesn't affect any kinematics) + q = [sp.Symbol('q%i' % ii) for ii in range(joint_num)] + + Tx = TransMatrix_DH_Symbol(rob, joint_num) + J = [] + # calculate derivative of (x,y,z) wrt to each joint, linear velocity + for ii in range(joint_num): + J.append([]) + J[ii].append(sp.simplify(Tx[0, 3].diff(q[ii]))) # dx/dq[ii] + J[ii].append(sp.simplify(Tx[1, 3].diff(q[ii]))) # dy/dq[ii] + J[ii].append(sp.simplify(Tx[2, 3].diff(q[ii]))) # dz/dq[ii] + # orientation part of the Jacobian (compensating for orientations) + J_orientation = [ + [0, 0, 1], # joint 0 rotates around z axis + [1, 0, 0], # joint 1 rotates around x axis + [1, 0, 0], # joint 2 rotates around x axis + [1, 0, 0], # joint 3 rotates around x axis + [0, 0, 1], # joint 4 rotates around z axis + [1, 0, 0]] # joint 5 rotates around x axis + # add on the orientation information up to the last joint + for ii in range(joint_num): + J[ii] = J[ii] + J_orientation[ii] + return J + + +def Jacobian_Numerical(rob='ur10', joint=[0, 0, 0, 0, 0, 0]): + ''' + This function returns the numerical result of Jacobian + joint: joint vector + J is from Jacobian_Symbol + ''' + q0 = joint[0] + q1 = joint[1] + q2 = joint[2] + q3 = joint[3] + q4 = joint[4] + q5 = joint[5] + if str(rob).lower() == 'ur5': + J = np.matrix([[0.0823 * np.sin(q0) * np.sin(q4) * np.cos(q1 + q2 + q3) - 0.09465 * np.sin(q0) * np.sin( + q1 + q2 + q3) + 0.425 * np.sin(q0) * np.cos(q1) + 0.39225 * np.sin(q0) * np.cos(q1 + q2) + 0.0823 * np.cos( + q0) * np.cos(q4) + 0.10915 * np.cos(q0), + 0.0823 * np.sin(q0) * np.cos(q4) + 0.10915 * np.sin(q0) - 0.0823 * np.sin(q4) * np.cos( + q0) * np.cos(q1 + q2 + q3) + 0.09465 * np.sin(q1 + q2 + q3) * np.cos(q0) - 0.425 * np.cos( + q0) * np.cos(q1) - 0.39225 * np.cos(q0) * np.cos(q1 + q2), 0, 0, 0, 1], + [0.425 * np.sin(q1) * np.cos(q0) + 0.0823 * np.sin(q4) * np.sin(q1 + q2 + q3) * np.cos( + q0) + 0.39225 * np.sin(q1 + q2) * np.cos(q0) + 0.09465 * np.cos(q0) * np.cos(q1 + q2 + q3), + 0.425 * np.sin(q0) * np.sin(q1) + 0.0823 * np.sin(q0) * np.sin(q4) * np.sin( + q1 + q2 + q3) + 0.39225 * np.sin(q0) * np.sin(q1 + q2) + 0.09465 * np.sin(q0) * np.cos( + q1 + q2 + q3), + -0.0823 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.09465 * np.sin(q1 + q2 + q3) - 0.425 * np.cos( + q1) - 0.39225 * np.cos(q1 + q2), 1, 0, 0], + [0.0823 * np.sin(q4) * np.sin(q1 + q2 + q3) * np.cos(q0) + 0.39225 * np.sin(q1 + q2) * np.cos( + q0) + 0.09465 * np.cos(q0) * np.cos(q1 + q2 + q3), + 0.0823 * np.sin(q0) * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.39225 * np.sin(q0) * np.sin( + q1 + q2) + 0.09465 * np.sin(q0) * np.cos(q1 + q2 + q3), + -0.0823 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.09465 * np.sin(q1 + q2 + q3) - 0.39225 * np.cos( + q1 + q2), 1, 0, 0], + [(0.0823 * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.09465 * np.cos(q1 + q2 + q3)) * np.cos(q0), + (0.0823 * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.09465 * np.cos(q1 + q2 + q3)) * np.sin(q0), + -0.0823 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.09465 * np.sin(q1 + q2 + q3), 1, 0, 0], + [-0.0823 * (1.0 * np.sin(q0)) * np.sin(q4) - 0.0823 * np.cos(q0) * np.cos(q4) * np.cos( + q1 + q2 + q3), + 0.0823 * (1.0 * np.cos(q0)) * np.sin(q4) - 0.0823 * np.sin(q0) * np.cos(q4) * np.cos( + q1 + q2 + q3), - 0.0823 * np.sin(q1 + q2 + q3) * np.cos(q4), 0, 0, 1], + [0, 0, 0, 1, 0, 0]]) + elif str(rob).lower() == 'ur10': + J = np.matrix([[0.0922 * np.sin(q0) * np.sin(q4) * np.cos(q1 + q2 + q3) - 0.1157 * np.sin(q0) * np.sin( + q1 + q2 + q3) + 0.612 * np.sin(q0) * np.cos(q1) + 0.5723 * np.sin(q0) * np.cos(q1 + q2) + 0.0922 * np.cos( + q0) * np.cos(q4) + 0.163941 * np.cos(q0), + 0.0922 * np.sin(q0) * np.cos(q4) + 0.163941 * np.sin(q0) - 0.0922 * np.sin(q4) * np.cos( + q0) * np.cos(q1 + q2 + q3) + 0.1157 * np.sin(q1 + q2 + q3) * np.cos(q0) - 0.612 * np.cos( + q0) * np.cos(q1) - 0.5723 * np.cos(q0) * np.cos(q1 + q2), 0, 0, 0, 1], + [0.612 * np.sin(q1) * np.cos(q0) + 0.0922 * np.sin(q4) * np.sin(q1 + q2 + q3) * np.cos( + q0) + 0.5723 * np.sin(q1 + q2) * np.cos(q0) + 0.1157 * np.cos(q0) * np.cos(q1 + q2 + q3), + 0.612 * np.sin(q0) * np.sin(q1) + 0.0922 * np.sin(q0) * np.sin(q4) * np.sin( + q1 + q2 + q3) + 0.5723 * np.sin(q0) * np.sin(q1 + q2) + 0.1157 * np.sin(q0) * np.cos( + q1 + q2 + q3), + -0.0922 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.1157 * np.sin(q1 + q2 + q3) - 0.612 * np.cos( + q1) - 0.5723 * np.cos(q1 + q2), 1, 0, 0], + [0.0922 * np.sin(q4) * np.sin(q1 + q2 + q3) * np.cos(q0) + 0.5723 * np.sin(q1 + q2) * np.cos( + q0) + 0.1157 * np.cos(q0) * np.cos(q1 + q2 + q3), + 0.0922 * np.sin(q0) * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.5723 * np.sin(q0) * np.sin( + q1 + q2) + 0.1157 * np.sin(q0) * np.cos(q1 + q2 + q3), + -0.0922 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.1157 * np.sin(q1 + q2 + q3) - 0.5723 * np.cos( + q1 + q2), 1, 0, 0], + [(0.0922 * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.1157 * np.cos(q1 + q2 + q3)) * np.cos(q0), + (0.0922 * np.sin(q4) * np.sin(q1 + q2 + q3) + 0.1157 * np.cos(q1 + q2 + q3)) * np.sin(q0), + -0.0922 * np.sin(q4) * np.cos(q1 + q2 + q3) + 0.1157 * np.sin(q1 + q2 + q3), 1, 0, 0], + [-0.0922 * (1.0 * np.sin(q0)) * np.sin(q4) - 0.0922 * np.cos(q0) * np.cos(q4) * np.cos( + q1 + q2 + q3), + 0.0922 * (1.0 * np.cos(q0)) * np.sin(q4) - 0.0922 * np.sin(q0) * np.cos(q4) * np.cos( + q1 + q2 + q3), - 0.0922 * np.sin(q1 + q2 + q3) * np.cos(q4), 0, 0, 1], + [0, 0, 0, 1, 0, 0]]) + return J + + +def RotatMatr2AxisAng(Matrix): + ''' + Convert the rotation matrix to axis angle + ''' + R = Matrix + theta = np.arccos(0.5 * (R[0, 0] + R[1, 1] + R[2, 2] - 1)) + e1 = (R[2, 1] - R[1, 2]) / (2 * np.sin(theta)) + e2 = (R[0, 2] - R[2, 0]) / (2 * np.sin(theta)) + e3 = (R[1, 0] - R[0, 1]) / (2 * np.sin(theta)) + axis_ang = np.array([theta * e1, theta * e2, theta * e3]) + return axis_ang + + +def AxisAng2RotaMatri(angle_vec): + ''' + Convert an Axis angle to rotation matrix + AxisAng2Matrix(angle_vec) + angle_vec need to be a 3D Axis angle + ''' + theta = math.sqrt(angle_vec[0] ** 2 + angle_vec[1] ** 2 + angle_vec[2] ** 2) + if theta == 0.: + return np.identity(3, dtype=float) + + cs = np.cos(theta) + si = np.sin(theta) + e1 = angle_vec[0] / theta + e2 = angle_vec[1] / theta + e3 = angle_vec[2] / theta + + R = np.zeros((3, 3)) + R[0, 0] = (1 - cs) * e1 ** 2 + cs + R[0, 1] = (1 - cs) * e1 * e2 - e3 * si + R[0, 2] = (1 - cs) * e1 * e3 + e2 * si + R[1, 0] = (1 - cs) * e1 * e2 + e3 * si + R[1, 1] = (1 - cs) * e2 ** 2 + cs + R[1, 2] = (1 - cs) * e2 * e3 - e1 * si + R[2, 0] = (1 - cs) * e1 * e3 - e2 * si + R[2, 1] = (1 - cs) * e2 * e3 + e1 * si + R[2, 2] = (1 - cs) * e3 ** 2 + cs + return R + + +def Rotat2TransMarix(Rota_Matrix, pose): + ''' + convert the rotation matrix and pose to transformation matrix + ''' + tran_mat = np.zeros((4, 4)) + tran_mat[:3, :3] = Rota_Matrix[:3, :3] + tran_mat[3, 3] = 1 + tran_mat[:3, 3] = pose[:3] + return tran_mat + + +def Pose2Tran_Mat(pose): + ''' + Convert an pose to a transformation matrix + AxisAng2Matrix(pose) + ''' + rot_mat = AxisAng2RotaMatri(pose[-3:]) + tran_mat = Rotat2TransMarix(rot_mat, pose) + return tran_mat + + +def Tran_Mat2Pose(Tran_Mat): + ''' + Convert a transformation matrix to pose + ''' + pose = np.zeros([6]) + pose[:3] = Tran_Mat[:3, 3] + Rota_mat = np.zeros([3, 3]) + Rota_mat[:3, :3] = Tran_Mat[:3, :3] + angle_vec = RotatMatr2AxisAng(Rota_mat) + pose[-3:] = angle_vec + return pose + + +def cmpleate_rotation_matrix(start_vector): + ''' This function make rotation matrix where the first column is the + input vector. + ''' + + a = np.matrix(start_vector) + a = a / np.linalg.norm(a) + U, s, Vh = linalg.svd(a, full_matrices=True) + + Vh[0] = a # to ensure that the virst vector is not -a + Vh[2] = np.cross(a, Vh[1]) + Vh = np.transpose(Vh) + return np.array(Vh) + + +def Inverse_kin(target_pos, init_joint_pos=[0, 0, 0, 0, 0, 0], tcpOffset=[0, 0, 0, 0, 0, 0]): + ''' + Find the inverse kinematics + target_pos = the target pos vector + init_joint_pos (optional) = the initial joint vector + ''' + # Define a robot from URDF file + my_chain = ik.chain.Chain.from_urdf_file('URDF/UR5.URDF') + # Convert pos to transfer matrix + # Mar = Pose2Tran_Mat(target_pos) + + T_target = Pose2Tran_Mat(pose=target_pos) + T_tcp = Pose2Tran_Mat(pose=tcpOffset) + Mar = np.matmul(T_target, np.linalg.inv(T_tcp)) + + # Inverse kinematics + if len(init_joint_pos) < 6: + return None + else: + joint_init = init_joint_pos.copy() + # add a [0] at the base frame + joint_initadd = np.zeros([7]) + joint_initadd[1:] = joint_init[:] + ikin = my_chain.inverse_kinematics(target=Mar, initial_position=joint_initadd.copy()) + + print('***************************************************************************') + print('Pose: [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*target_pos)) + print('---------------------------------------------------------------------------') + print(ikin) + print('---------------------------------------------------------------------------') + print('Joints Returned: [{: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}, {: 06.3f}]'.format(*ikin[1:])) + print('***************************************************************************') + + return ikin[1:] + + +def Forward_kin(joint): + ''' + Find the forward kinematics + ''' + # Define a robot from URDF file + my_chain = ik.chain.Chain.from_urdf_file('URDF/UR5.URDF') + # add a [0] in joint anlges, due to the defination of URDF + joint_new = np.zeros([7]) + joint_new[1:] = joint[:] + fk = my_chain.forward_kinematics(joint_new) + # convert transfer matrix to pos + axis_ang = Tran_Mat2Pose(fk) + return axis_ang + + +def Vektor_from_Base_to_TCP(vectorBase, axisangle): + """ + Function to calculate the rotation of a vector with an axis angle. This function can be used to transform a vector from the base coordinate system to the tcp coordinate system. + (To go from tcp to Base use -1*le as input) + + Parameters: + vectorBase (np.array, 3*1): the vector to be rotated + axisangle (np.array, 3*1): the axis angle between the base and the tcp + """ + # calculating the rotation angle by finding the length of the axis angle + angle = axisangle[0] * axisangle[0] + axisangle[1] * axisangle[1] + axisangle[2] * axisangle[2] + angle = np.sqrt(angle) + # Defining the unit rotation angle + eRot = axisangle / angle + # calculation of the tcp vector using Rodrigues' rotation formula + vectorBase = np.array(vectorBase) + vectorTCP = np.cos(angle) * vectorBase + np.sin(angle) * np.cross(eRot, vectorBase) + (1 - np.cos(angle)) * np.dot( + eRot, vectorBase) * eRot + return vectorTCP