Guest User

Untitled

a guest
Jan 16th, 2018
84
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 5.24 KB | None | 0 0
  1. Code:
  2.  
  3. ```python
  4. theta5 = atan2(norm(R3_6[0,2], R3_6[2,2]), R3_6[1,2])
  5.  
  6. # prevent wrist flaps by using consistently one of the multiple solutions
  7. if sin(theta5) < 0.:
  8. theta4 = atan2(-R3_6[2,2], R3_6[0,2])
  9. theta6 = atan2(R3_6[1,1], -R3_6[1,0])
  10. rospy.loginfo("Negative branch, theta4=%04.8f, theta5= %04.8f, theta6=%04.8f" % (theta4, theta5, theta6))
  11. else:
  12. theta4 = atan2(R3_6[2,2], -R3_6[0,2])
  13. theta6 = atan2(-R3_6[1,1], R3_6[1,0])
  14. rospy.loginfo("Positive branch, theta4=%04.8f, theta5= %04.8f, theta6=%04.8f" % (theta4, theta5, theta6))
  15. ```
  16.  
  17. Result:
  18.  
  19. ```bash
  20. [INFO] [1515296620.711290]: Received 10 eef-poses from the plan
  21. [INFO] [1515296622.091448]: Positive branch, theta4=0.00000000, theta5= 1.57079633, theta6=0.00000015
  22. [INFO] [1515296622.336308]: Positive branch, theta4=3.04619374, theta5= 0.07653129, theta6=2.48070150
  23. [INFO] [1515296622.814664]: Positive branch, theta4=2.88591735, theta5= 0.11432489, theta6=-2.89691175
  24. [INFO] [1515296623.151579]: Positive branch, theta4=2.76165392, theta5= 0.17294709, theta6=-2.77831331
  25. [INFO] [1515296623.537406]: Positive branch, theta4=3.10917248, theta5= 1.39209392, theta6=1.59041056
  26. [INFO] [1515296624.011607]: Positive branch, theta4=2.04795072, theta5= 0.19098979, theta6=2.76478810
  27. [INFO] [1515296624.227802]: Positive branch, theta4=1.64354934, theta5= 0.23452094, theta6=2.83982764
  28. [INFO] [1515296624.571575]: Positive branch, theta4=2.26014052, theta5= 0.40430168, theta6=-2.30169624
  29. [INFO] [1515296624.873025]: Positive branch, theta4=2.13303477, theta5= 0.46109383, theta6=-2.18133842
  30. [INFO] [1515296625.194482]: Positive branch, theta4=0.09899684, theta5= 1.15552980, theta6=-1.68595180
  31. [INFO] [1515296625.197542]: length of Joint Trajectory List: 10
  32. [INFO] [1515296819.804877]: Received 34 eef-poses from the plan
  33. [INFO] [1515296820.493005]: Positive branch, theta4=0.09940830, theta5= 1.15546806, theta6=-3.13828782
  34. [INFO] [1515296820.810797]: Positive branch, theta4=3.02433816, theta5= 1.57012099, theta6=-3.13923899
  35. [INFO] [1515296821.064625]: Positive branch, theta4=0.90623955, theta5= 0.51392552, theta6=-3.13058199
  36. [INFO] [1515296821.354005]: Positive branch, theta4=1.87483665, theta5= 0.46755769, theta6=-1.93742377
  37. [INFO] [1515296821.661567]: Positive branch, theta4=1.77785699, theta5= 0.42915022, theta6=-1.84564470
  38. [INFO] [1515296822.011201]: Positive branch, theta4=1.51646121, theta5= 0.32776982, theta6=-1.59796114
  39. [INFO] [1515296822.320521]: Positive branch, theta4=0.67555900, theta5= 0.32062229, theta6=0.88177037
  40. [INFO] [1515296822.664673]: Positive branch, theta4=1.06471786, theta5= 0.16322217, theta6=-1.16900148
  41. [INFO] [1515296822.966211]: Positive branch, theta4=0.02606275, theta5= 0.97514972, theta6=0.59660692
  42. [INFO] [1515296823.282765]: Positive branch, theta4=-2.25885420, theta5= 0.03338622, theta6=2.12703898
  43. [INFO] [1515296823.624056]: Positive branch, theta4=-2.67713859, theta5= 0.12642208, theta6=2.53310800
  44. [INFO] [1515296824.029298]: Positive branch, theta4=-0.01104541, theta5= 0.95565049, theta6=0.98480602
  45. [INFO] [1515296824.276248]: Positive branch, theta4=-3.04277505, theta5= 0.09457323, theta6=2.67425932
  46. [INFO] [1515296824.535916]: Positive branch, theta4=2.80369387, theta5= 0.15178119, theta6=3.01032135
  47. [INFO] [1515296824.805015]: Positive branch, theta4=2.68716364, theta5= 0.19375563, theta6=-3.12142687
  48. [INFO] [1515296825.165374]: Positive branch, theta4=2.84980073, theta5= 0.46551786, theta6=-3.03571392
  49. [INFO] [1515296825.416462]: Positive branch, theta4=2.81793174, theta5= 0.53198907, theta6=-3.00240362
  50. [INFO] [1515296825.662737]: Positive branch, theta4=3.07823853, theta5= 1.75247140, theta6=-1.76330273
  51. [INFO] [1515296825.999230]: Positive branch, theta4=3.07572899, theta5= 1.84768804, theta6=-1.77098064
  52. [INFO] [1515296826.395250]: Positive branch, theta4=2.68611674, theta5= 0.88755618, theta6=-2.86039418
  53. [INFO] [1515296826.730216]: Positive branch, theta4=2.70010727, theta5= 1.05505045, theta6=-2.86151165
  54. [INFO] [1515296827.109111]: Positive branch, theta4=2.94943277, theta5= 1.70691599, theta6=2.95304055
  55. [INFO] [1515296827.398127]: Positive branch, theta4=2.81966476, theta5= 1.22071125, theta6=2.63089577
  56. [INFO] [1515296827.821995]: Positive branch, theta4=3.08710850, theta5= 1.75758232, theta6=2.97161351
  57. [INFO] [1515296828.162249]: Positive branch, theta4=-2.83732393, theta5= 1.11502729, theta6=2.59243100
  58. [INFO] [1515296828.456853]: Positive branch, theta4=-2.77730073, theta5= 1.71373069, theta6=2.77510751
  59. [INFO] [1515296828.717037]: Positive branch, theta4=-2.53229213, theta5= 1.72747883, theta6=2.56041837
  60. [INFO] [1515296829.029624]: Positive branch, theta4=-2.32159317, theta5= 1.74613129, theta6=2.32697032
  61. [INFO] [1515296829.455979]: Positive branch, theta4=-0.00651232, theta5= 1.36596995, theta6=-2.87513722
  62. [INFO] [1515296829.893171]: Positive branch, theta4=-1.06147598, theta5= 1.62170686, theta6=-2.83781771
  63. [INFO] [1515296830.234233]: Positive branch, theta4=-1.78805480, theta5= 1.76541313, theta6=1.87712627
  64. [INFO] [1515296830.538411]: Positive branch, theta4=-1.68085903, theta5= 1.77028451, theta6=1.77554188
  65. [INFO] [1515296830.783437]: Positive branch, theta4=-1.61771887, theta5= 1.77087114, theta6=1.71549611
  66. [INFO] [1515296831.219722]: Positive branch, theta4=-1.55460159, theta5= 1.77172240, theta6=1.65544093
  67. [INFO] [1515296831.220440]: length of Joint Trajectory List: 34
  68.  
  69. ```
Add Comment
Please, Sign In to add comment