API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 22nd, 2017
65
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 8.09 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python
  2.  
  3. import numpy as np
  4. from numpy import array
  5. from sympy import symbols, cos, sin, pi, simplify
  6. from sympy.matrices import Matrix
  7.  
  8. import rospy
  9. import tf
  10. #from kuka_aoutput.srv import *
  11. from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
  12. from geometry_msgs.msg import Pose
  13. from mpmath import *
  14. from sympy import *
  15.  
  16.  
  17. ### symbols for DH params
  18. ### Create symbols for joint variables
  19. #roll,pitch,yaw = symbols('roll pitch yaw')
  20.  
  21. q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
  22. d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
  23. a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
  24. alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
  25.  
  26. # DH Parameters
  27. s = {alpha0: 0,  a0:0, d1: 0.75,
  28.      alpha1: -pi/2,  a1: 0.35, d2: 0, q2:q2 -pi/2,
  29.      alpha2: 0,  a2: 1.25, d3: 0,
  30.      alpha3: -pi/2,  a3:   -0.054, d4: 1.5,
  31.      alpha4: pi/2,  a4:0, d5: 0,
  32.      alpha5: -pi/2,  a5:   0, d6: 0,
  33.      alpha6: 0,  a6:   0, d7: 0.303, q7:0}
  34.  
  35. #### Homogeneous Transfooutputs
  36.  
  37.  
  38.  
  39. T0_1 = Matrix([[             cos(q1),            -sin(q1),            0,              a0],
  40.                [ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
  41.                [ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0),  cos(alpha0),  cos(alpha0)*d1],
  42.                [                   0,                   0,            0,               1]])
  43. T0_1 = T0_1.subs(s)
  44.  
  45. T1_2 = Matrix([[             cos(q2),            -sin(q2),            0,              a1],
  46.                [ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
  47.                [ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1),  cos(alpha1),  cos(alpha1)*d2],
  48.                [                   0,                   0,            0,               1]])
  49. T1_2 = T1_2.subs(s)
  50.  
  51. T2_3 = Matrix([[             cos(q3),            -sin(q3),            0,              a2],
  52.                [ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
  53.                [ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2),  cos(alpha2),  cos(alpha2)*d3],
  54.                [                   0,                   0,            0,               1]])
  55. T2_3 = T2_3.subs(s)
  56.  
  57. T3_4 = Matrix([[             cos(q4),            -sin(q4),            0,              a3],
  58.                [ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
  59.                [ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3),  cos(alpha3),  cos(alpha3)*d4],
  60.                [                   0,                   0,            0,               1]])
  61. T3_4 = T3_4.subs(s)
  62.  
  63. T4_5 = Matrix([[             cos(q5),            -sin(q5),            0,              a4],
  64.                [ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],
  65.                [ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4),  cos(alpha4),  cos(alpha4)*d5],
  66.                [                   0,                   0,            0,               1]])
  67. T4_5 = T4_5.subs(s)
  68.  
  69. T5_6 = Matrix([[             cos(q6),            -sin(q6),            0,              a5],
  70.                [ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],
  71.                [ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5),  cos(alpha5),  cos(alpha5)*d6],
  72.                [                   0,                   0,            0,               1]])
  73. T5_6 = T5_6.subs(s)
  74.  
  75. T6_7 = Matrix([[             cos(q7),            -sin(q7),            0,              a6],
  76.                [ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],
  77.                [ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6),  cos(alpha6),  cos(alpha6)*d7],
  78.                [                   0,                   0,            0,               1]])
  79. T6_7 = T6_7.subs(s)
  80.  
  81.  
  82.  
  83. #transfooutputs
  84.  
  85. T0_3 = simplify(T0_1*T1_2*T2_3)
  86. #print("T0_3")
  87. #print(T0_3)
  88.  
  89.  
  90.  
  91. T3_7 = simplify(T3_4*T4_5*T5_6)#*T6_7
  92. #print("T3_7")
  93. #print(T3_7)
  94.  
  95. #T0_5 = simplify(T0_4*T4_5)
  96. #T0_6 = simplify(T0_5*T5_6)
  97. #T0_7 = simplify(T0_6*T6_7)
  98.  
  99.  
  100. px, py, pz = -0.366, -0.238, 3.226
  101. roll,pitch,yaw = -1.108, -0.511, -1.741
  102.  
  103. #--------------Rrpy----------------
  104.  
  105.  
  106. R_z_y = Matrix([[cos(yaw),     -sin(yaw),             0,              0],
  107.         [      sin(yaw),      cos(yaw),             0,              0],
  108.         [            0,            0,             1,              0],
  109.         [            0,            0,             0,              1]])
  110.        
  111. R_y_p = Matrix([[cos(pitch),                  0,  sin(pitch),              0],
  112.         [            0,                  1,        0,              0],
  113.         [     -sin(pitch),                  0,  cos(pitch),              0],
  114.         [            0,                      0,              0,              1]])
  115.        
  116. R_x_r = Matrix([[      1,                      0,              0,              0],
  117.         [            0,                cos(roll),       -sin(roll),              0],
  118.         [            0,                sin(roll),        cos(roll),              0],
  119.         [            0,                      0,              0,              1]])
  120.  
  121.  
  122. R_z = Matrix([[cos(np.pi),     -sin(np.pi),             0,              0],
  123.         [      sin(np.pi),      cos(np.pi),             0,              0],
  124.         [            0,            0,             1,              0],
  125.         [            0,            0,             0,              1]])
  126.        
  127. R_y = Matrix([[cos(-np.pi/2),                  0,  sin(-np.pi/2),              0],
  128.         [            0,                  1,        0,              0],
  129.         [     -sin(-np.pi/2),                  0,  cos(-np.pi/2),              0],
  130.         [            0,                      0,              0,              1]])
  131.  
  132. Rrpy = R_z_y*R_y_p*R_x_r
  133.  
  134.  
  135.  
  136. #----------------------------------------------------------------------------------------------------------
  137. R_corr = R_z * R_y
  138. R_corrI = R_corr.inv()
  139. R_gripper = Rrpy*R_corrI
  140.  
  141. #print(R_gripper)
  142.  
  143. #T_total = simplify(T0_7*R_corr)
  144. #Rrpy = simplify(R_z*R_y*R_x)
  145. #print(Rrpy)
  146. l = 0.303
  147. nx = R_gripper[0,2]
  148. ny = R_gripper[1,2]
  149. nz = R_gripper[2,2]
  150. wx = px - l*nx #should be the centre of joint#5
  151. wy = py - l*ny
  152. wz = pz - l*nz
  153.  
  154. #print("link 5 xpos", wx)
  155. #print("link 5 ypos", wy)
  156. #print("link 5 zpos", wz)
  157.  
  158. theta1 = atan2(wy,wx)
  159.  
  160. wx_corr = wx - 0.35*cos(theta1)
  161. wy_corr = wy - 0.35*sin(theta1)
  162. wz_corr = wz - 0.75
  163.  
  164. a = 1.25
  165. c = (wx_corr**2 + wy_corr**2 + wz_corr**2)**0.5
  166.  
  167. b_corr = (1.5**2+0.054**2)**0.5
  168.  
  169. #theta 3
  170. s_theta3 = -(c**2 - a**2 - b_corr**2)/(2*a*b_corr)
  171. c_theta3 = (1 - s_theta3**2)**0.5
  172. theta3 = atan2(s_theta3,c_theta3)
  173.  
  174. #theta 2
  175. s_beta = wz_corr
  176. c_beta = (wy_corr**2 + wx_corr**2)**0.5
  177. alpha = -theta3 -3.14159265359/2
  178. s_psi = b_corr*sin(alpha)
  179. c_psi = a + b_corr*cos(alpha)
  180.  
  181. theta2 = -atan2(s_beta,c_beta) + atan2(s_psi,c_psi) + 3.14159265359/2
  182.  
  183. print("jointangle1: " ,theta1.evalf())
  184. print("jointangle2: " ,theta2.evalf())
  185. print("jointangle3: " ,theta3.evalf())
  186.  
  187. q1 , q2, q3 = theta1, theta2, theta3
  188. R03 = Matrix([
  189. [sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
  190. [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
  191. [        cos(q2 + q3),        -sin(q2 + q3),        0]])
  192. #print(Rrpy)
  193.  
  194. Rrpy.col_del(3)
  195. Rrpy.row_del(3)
  196.  
  197. R_corrI.col_del(3)
  198. R_corrI.row_del(3)
  199.  
  200. R37 = (R03.T)*Rrpy*R_corrI
  201.  
  202. q6= atan2(-R37[1,1],R37[1,0])
  203. q4= atan2(R37[2,2],-R37[0,2])
  204. q5= atan2(-R37[0,2]/cos(q4),R37[1,2])
  205.  
  206. print("inverse orientation")
  207. print("jointangle4: " ,(q4-pi).evalf())
  208. print("jointangle5: " ,-q5.evalf())
  209. print("jointangle6: " ,(q6+pi).evalf())
  210.  
  211.  
  212. '''
  213. print(T_total.evalf(subs={q1: 1.8, q2: -0.32, q3: -1.93, q4: 3.76, q5: -2.13, q6: -1.99}, chop = True))
  214.  
  215.  
  216.  
  217. a = 1.25
  218. c = (wx_corr**2 + wy_corr**2 + wz_corr**2)**0.5
  219.  
  220. b_corr = (1.5**2+0.054**2)**0.5
  221.  
  222. #theta 3
  223. s_theta3 = -(c**2 - a**2 - b_corr**2)/(2*a*b_corr)
  224. c_theta3 = (1 - s_theta3**2)**0.5
  225. theta3 = atan2(s_theta3, c_theta3)
  226.  
  227. alpha = -theta3 - np.pi/2
  228.  
  229.  
  230. #theta 2
  231. s_beta = wz_corr
  232. c_beta = (wy_corr**2 + wx_corr**2)**0.5
  233.  
  234. s_psi = b_corr*sin(alpha)
  235. c_psi = a + b_corr*cos(alpha)
  236.  
  237. theta2 = -atan2(s_beta,c_beta) + atan2(s_psi,c_psi) + np.pi/2
  238.  
  239. rtd = 1#80/3.1415926535
  240.  
  241. print(theta1*rtd, theta2*rtd, theta3*rtd)
  242.  
  243.  
  244. '''
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!