Advertisement
vaidyasm

cartesian control logs

Mar 19th, 2018
161
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 64.18 KB | None | 0 0
  1. [rosout][INFO] 2018-03-19 23:27:18,491: =============== cartesian_control() ===============
  2. [rosout][DEBUG] 2018-03-19 23:27:18,492:
  3.  
  4. joint_transforms:
  5. [rosout][DEBUG] 2018-03-19 23:27:18,492:
  6.  
  7. joint_transforms[0]:
  8. [[ 1.000 0.000 0.000 0.000]
  9. [ 0.000 1.000 0.000 0.000]
  10. [ 0.000 0.000 1.000 0.310]
  11. [ 0.000 0.000 0.000 1.000]]
  12. [rosout][DEBUG] 2018-03-19 23:27:18,493:
  13.  
  14. joint_transforms[1]:
  15. [[ 1.000 0.000 0.000 0.000]
  16. [ 0.000 0.000 1.000 0.000]
  17. [ 0.000 -1.000 0.000 0.510]
  18. [ 0.000 0.000 0.000 1.000]]
  19. [rosout][DEBUG] 2018-03-19 23:27:18,493:
  20.  
  21. joint_transforms[2]:
  22. [[ 1.000 0.000 0.000 0.000]
  23. [ 0.000 1.000 0.000 0.000]
  24. [ 0.000 0.000 1.000 0.710]
  25. [ 0.000 0.000 0.000 1.000]]
  26. [rosout][DEBUG] 2018-03-19 23:27:18,502:
  27.  
  28. joint_transforms[3]:
  29. [[ 1.000 0.000 0.000 0.000]
  30. [ 0.000 0.000 -1.000 0.000]
  31. [ 0.000 1.000 0.000 0.910]
  32. [ 0.000 0.000 0.000 1.000]]
  33. [rosout][DEBUG] 2018-03-19 23:27:18,505:
  34.  
  35. joint_transforms[4]:
  36. [[ 1.000 0.000 0.000 0.000]
  37. [ 0.000 1.000 0.000 0.000]
  38. [ 0.000 0.000 1.000 1.110]
  39. [ 0.000 0.000 0.000 1.000]]
  40. [rosout][DEBUG] 2018-03-19 23:27:18,511:
  41.  
  42. joint_transforms[5]:
  43. [[ 1.000 0.000 0.000 0.000]
  44. [ 0.000 0.000 1.000 0.000]
  45. [ 0.000 -1.000 0.000 1.300]
  46. [ 0.000 0.000 0.000 1.000]]
  47. [rosout][DEBUG] 2018-03-19 23:27:18,513:
  48.  
  49. joint_transforms[6]:
  50. [[ 1.000 0.000 0.000 0.000]
  51. [ 0.000 1.000 0.000 0.000]
  52. [ 0.000 0.000 1.000 1.378]
  53. [ 0.000 0.000 0.000 1.000]]
  54. [rosout][DEBUG] 2018-03-19 23:27:18,513: ----------------------------------------------
  55. [rosout][DEBUG] 2018-03-19 23:27:18,518:
  56.  
  57. b_T_ee_current:
  58. [[ 1.000 0.000 0.000 0.000]
  59. [ 0.000 1.000 0.000 0.000]
  60. [ 0.000 0.000 1.000 1.378]
  61. [ 0.000 0.000 0.000 1.000]]
  62. [rosout][DEBUG] 2018-03-19 23:27:18,518: ----------------------------------------------
  63. [rosout][DEBUG] 2018-03-19 23:27:18,519:
  64.  
  65. b_T_ee_desired:
  66. [[ 1.000 0.000 0.000 -0.076]
  67. [ 0.000 1.000 0.000 0.000]
  68. [ 0.000 0.000 1.000 1.378]
  69. [ 0.000 0.000 0.000 1.000]]
  70. [rosout][DEBUG] 2018-03-19 23:27:18,519: ----------------------------------------------
  71. [rosout][DEBUG] 2018-03-19 23:27:18,520:
  72.  
  73. red_control:
  74. False
  75. [rosout][DEBUG] 2018-03-19 23:27:18,520: ----------------------------------------------
  76. [rosout][DEBUG] 2018-03-19 23:27:18,520:
  77.  
  78. q_current:
  79. (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
  80. [rosout][DEBUG] 2018-03-19 23:27:18,521: ----------------------------------------------
  81. [rosout][DEBUG] 2018-03-19 23:27:18,521:
  82.  
  83. q0_desired:
  84. 0
  85. [rosout][DEBUG] 2018-03-19 23:27:18,521: ----------------------------------------------
  86. [rosout][DEBUG] 2018-03-19 23:27:18,522:
  87.  
  88. b_p_current:
  89. [ 0.000 0.000 1.378 1.000]
  90. [rosout][DEBUG] 2018-03-19 23:27:18,522: ----------------------------------------------
  91. [rosout][DEBUG] 2018-03-19 23:27:18,522:
  92.  
  93. b_p_desired:
  94. [-0.076 0.000 1.378 1.000]
  95. [rosout][DEBUG] 2018-03-19 23:27:18,523: ----------------------------------------------
  96. [rosout][DEBUG] 2018-03-19 23:27:18,523:
  97.  
  98. x_current:
  99. [ 0.000 0.000 1.378 0.000 -0.000 0.000]
  100. [rosout][DEBUG] 2018-03-19 23:27:18,524: ----------------------------------------------
  101. [rosout][DEBUG] 2018-03-19 23:27:18,524:
  102.  
  103. x_desired:
  104. [-0.076 0.000 1.378 0.000 -0.000 0.000]
  105. [rosout][DEBUG] 2018-03-19 23:27:18,525: ----------------------------------------------
  106. [rosout][DEBUG] 2018-03-19 23:27:18,525:
  107.  
  108. b_dX:
  109. [-0.076 0.000 -0.000 0.000 0.000 0.000]
  110. [rosout][DEBUG] 2018-03-19 23:27:18,526: ----------------------------------------------
  111. [rosout][DEBUG] 2018-03-19 23:27:18,526:
  112.  
  113. b_X_dot:
  114. [-0.038 0.000 -0.000 0.000 0.000 0.000]
  115. [rosout][DEBUG] 2018-03-19 23:27:18,527: ----------------------------------------------
  116. [rosout][DEBUG] 2018-03-19 23:27:18,532:
  117.  
  118. V:
  119. [[ 1.000 0.000 0.000 0.000 0.000 0.000]
  120. [ 0.000 1.000 0.000 0.000 0.000 0.000]
  121. [ 0.000 0.000 1.000 0.000 0.000 0.000]
  122. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  123. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  124. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  125. [rosout][DEBUG] 2018-03-19 23:27:18,533: ----------------------------------------------
  126. [rosout][DEBUG] 2018-03-19 23:27:18,533:
  127.  
  128. V_ee:
  129. [-0.038 0.000 -0.000 0.000 0.000 0.000]
  130. [rosout][DEBUG] 2018-03-19 23:27:18,534: ----------------------------------------------
  131. [rosout][DEBUG] 2018-03-19 23:27:18,534: ++++++++++++++++++++++++++++++++++++++++++++++
  132. [rosout][DEBUG] 2018-03-19 23:27:18,536:
  133.  
  134. ee_T_ee_desired:
  135. [[ 1.000 0.000 0.000 -0.076]
  136. [ 0.000 1.000 0.000 0.000]
  137. [ 0.000 0.000 1.000 -0.000]
  138. [ 0.000 0.000 0.000 1.000]]
  139. [rosout][DEBUG] 2018-03-19 23:27:18,536: ----------------------------------------------
  140. [rosout][DEBUG] 2018-03-19 23:27:18,537:
  141.  
  142. delta_X:
  143. [-0.038 0.000 -0.000]
  144. [rosout][DEBUG] 2018-03-19 23:27:18,537: ----------------------------------------------
  145. [rosout][DEBUG] 2018-03-19 23:27:18,538:
  146.  
  147. delta_X:
  148. [-0.038 0.000 -0.000]
  149. [rosout][DEBUG] 2018-03-19 23:27:18,538: ----------------------------------------------
  150. [rosout][DEBUG] 2018-03-19 23:27:18,539:
  151.  
  152. delta_W:
  153. [ 0.000 0.000 0.000]
  154. [rosout][DEBUG] 2018-03-19 23:27:18,539: ----------------------------------------------
  155. [rosout][DEBUG] 2018-03-19 23:27:18,540:
  156.  
  157. delta_W:
  158. [ 0.000 0.000 0.000]
  159. [rosout][DEBUG] 2018-03-19 23:27:18,540: ----------------------------------------------
  160. [rosout][DEBUG] 2018-03-19 23:27:18,541:
  161.  
  162. V_ee:
  163. [-0.038 0.000 -0.000 0.000 0.000 0.000]
  164. [rosout][DEBUG] 2018-03-19 23:27:18,541: ----------------------------------------------
  165. [rosout][DEBUG] 2018-03-19 23:27:18,542: ----------------------------------------------
  166. [rosout][DEBUG] 2018-03-19 23:27:18,542: ----------------------------------------------
  167. [rosout][DEBUG] 2018-03-19 23:27:18,544:
  168.  
  169. j_T_ee:
  170. [rosout][DEBUG] 2018-03-19 23:27:18,545:
  171.  
  172. j_T_ee[0]:
  173. [[ 1.000 0.000 0.000 0.000]
  174. [ 0.000 1.000 0.000 0.000]
  175. [ 0.000 0.000 1.000 1.068]
  176. [ 0.000 0.000 0.000 1.000]]
  177. [rosout][DEBUG] 2018-03-19 23:27:18,546:
  178.  
  179. j_T_ee[1]:
  180. [[ 1.000 0.000 0.000 0.000]
  181. [ 0.000 0.000 -1.000 -0.868]
  182. [ 0.000 1.000 0.000 0.000]
  183. [ 0.000 0.000 0.000 1.000]]
  184. [rosout][DEBUG] 2018-03-19 23:27:18,550:
  185.  
  186. j_T_ee[2]:
  187. [[ 1.000 0.000 0.000 0.000]
  188. [ 0.000 1.000 0.000 0.000]
  189. [ 0.000 0.000 1.000 0.668]
  190. [ 0.000 0.000 0.000 1.000]]
  191. [rosout][DEBUG] 2018-03-19 23:27:18,551:
  192.  
  193. j_T_ee[3]:
  194. [[ 1.000 0.000 0.000 0.000]
  195. [ 0.000 0.000 1.000 0.468]
  196. [ 0.000 -1.000 0.000 0.000]
  197. [ 0.000 0.000 0.000 1.000]]
  198. [rosout][DEBUG] 2018-03-19 23:27:18,551:
  199.  
  200. j_T_ee[4]:
  201. [[ 1.000 0.000 0.000 0.000]
  202. [ 0.000 1.000 0.000 0.000]
  203. [ 0.000 0.000 1.000 0.268]
  204. [ 0.000 0.000 0.000 1.000]]
  205. [rosout][DEBUG] 2018-03-19 23:27:18,552:
  206.  
  207. j_T_ee[5]:
  208. [[ 1.000 0.000 0.000 0.000]
  209. [ 0.000 0.000 -1.000 -0.078]
  210. [ 0.000 1.000 0.000 0.000]
  211. [ 0.000 0.000 0.000 1.000]]
  212. [rosout][DEBUG] 2018-03-19 23:27:18,553:
  213.  
  214. j_T_ee[6]:
  215. [[ 1.000 0.000 0.000 0.000]
  216. [ 0.000 1.000 0.000 0.000]
  217. [ 0.000 0.000 1.000 0.000]
  218. [ 0.000 0.000 0.000 1.000]]
  219. [rosout][DEBUG] 2018-03-19 23:27:18,553: ----------------------------------------------
  220. [rosout][DEBUG] 2018-03-19 23:27:18,554:
  221.  
  222. ee_T_j:
  223. [rosout][DEBUG] 2018-03-19 23:27:18,554:
  224.  
  225. ee_T_j[0]:
  226. [[ 1.000 0.000 0.000 0.000]
  227. [ 0.000 1.000 0.000 0.000]
  228. [ 0.000 0.000 1.000 -1.068]
  229. [ 0.000 0.000 0.000 1.000]]
  230. [rosout][DEBUG] 2018-03-19 23:27:18,554:
  231.  
  232. ee_T_j[1]:
  233. [[ 1.000 0.000 0.000 0.000]
  234. [ 0.000 0.000 1.000 0.000]
  235. [-0.000 -1.000 0.000 -0.868]
  236. [ 0.000 0.000 0.000 1.000]]
  237. [rosout][DEBUG] 2018-03-19 23:27:18,555:
  238.  
  239. ee_T_j[2]:
  240. [[ 1.000 0.000 0.000 0.000]
  241. [ 0.000 1.000 0.000 0.000]
  242. [ 0.000 0.000 1.000 -0.668]
  243. [ 0.000 0.000 0.000 1.000]]
  244. [rosout][DEBUG] 2018-03-19 23:27:18,556:
  245.  
  246. ee_T_j[3]:
  247. [[ 1.000 0.000 0.000 0.000]
  248. [-0.000 0.000 -1.000 -0.000]
  249. [ 0.000 1.000 0.000 -0.468]
  250. [ 0.000 0.000 0.000 1.000]]
  251. [rosout][DEBUG] 2018-03-19 23:27:18,556:
  252.  
  253. ee_T_j[4]:
  254. [[ 1.000 0.000 0.000 0.000]
  255. [ 0.000 1.000 0.000 0.000]
  256. [ 0.000 0.000 1.000 -0.268]
  257. [ 0.000 0.000 0.000 1.000]]
  258. [rosout][DEBUG] 2018-03-19 23:27:18,557:
  259.  
  260. ee_T_j[5]:
  261. [[ 1.000 0.000 0.000 0.000]
  262. [ 0.000 0.000 1.000 0.000]
  263. [-0.000 -1.000 0.000 -0.078]
  264. [ 0.000 0.000 0.000 1.000]]
  265. [rosout][DEBUG] 2018-03-19 23:27:18,557:
  266.  
  267. ee_T_j[6]:
  268. [[ 1.000 0.000 0.000 0.000]
  269. [ 0.000 1.000 0.000 0.000]
  270. [ 0.000 0.000 1.000 0.000]
  271. [ 0.000 0.000 0.000 1.000]]
  272. [rosout][DEBUG] 2018-03-19 23:27:18,558: ----------------------------------------------
  273. [rosout][DEBUG] 2018-03-19 23:27:18,558:
  274.  
  275. V_j:
  276. [rosout][DEBUG] 2018-03-19 23:27:18,559:
  277.  
  278. V_j[0]:
  279. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  280. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  281. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  282. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  283. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  284. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  285. [rosout][DEBUG] 2018-03-19 23:27:18,559:
  286.  
  287. V_j[1]:
  288. [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
  289. [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
  290. [-0.000 -1.000 0.000 0.000 0.000 -0.000]
  291. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  292. [ 0.000 0.000 0.000 0.000 0.000 1.000]
  293. [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
  294. [rosout][DEBUG] 2018-03-19 23:27:18,560:
  295.  
  296. V_j[2]:
  297. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  298. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  299. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  300. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  301. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  302. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  303. [rosout][DEBUG] 2018-03-19 23:27:18,565:
  304.  
  305. V_j[3]:
  306. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  307. [-0.000 0.000 -1.000 0.000 -0.000 -0.000]
  308. [ 0.000 1.000 0.000 0.000 -0.000 -0.000]
  309. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  310. [ 0.000 0.000 0.000 -0.000 0.000 -1.000]
  311. [ 0.000 0.000 0.000 0.000 1.000 0.000]]
  312. [rosout][DEBUG] 2018-03-19 23:27:18,566:
  313.  
  314. V_j[4]:
  315. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  316. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  317. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  318. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  319. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  320. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  321. [rosout][DEBUG] 2018-03-19 23:27:18,569:
  322.  
  323. V_j[5]:
  324. [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
  325. [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
  326. [-0.000 -1.000 0.000 0.000 0.000 -0.000]
  327. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  328. [ 0.000 0.000 0.000 0.000 0.000 1.000]
  329. [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
  330. [rosout][DEBUG] 2018-03-19 23:27:18,569:
  331.  
  332. V_j[6]:
  333. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  334. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  335. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  336. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  337. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  338. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  339. [rosout][DEBUG] 2018-03-19 23:27:18,570: ----------------------------------------------
  340. [rosout][DEBUG] 2018-03-19 23:27:18,570: ----------------------------------------------
  341. [rosout][DEBUG] 2018-03-19 23:27:18,571: ----------------------------------------------
  342. [rosout][DEBUG] 2018-03-19 23:27:18,571:
  343.  
  344. J:
  345. [[-0.000 0.000 -0.000 -0.000 -0.000 0.000 -0.000]
  346. [ 0.000 0.000 0.000 -0.000 0.000 0.000 0.000]
  347. [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
  348. [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
  349. [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
  350. [ 1.000 0.000 1.000 0.000 1.000 0.000 1.000]]
  351. [rosout][DEBUG] 2018-03-19 23:27:18,572: ----------------------------------------------
  352. [rosout][DEBUG] 2018-03-19 23:27:18,573:
  353.  
  354. J_p:
  355. [[ 0.000 0.000 0.000 0.000 0.000 0.250]
  356. [ 0.000 0.000 0.000 0.000 0.333 0.000]
  357. [ 0.000 0.000 0.000 0.000 0.000 0.250]
  358. [ 0.000 0.000 0.000 0.000 -0.333 0.000]
  359. [ 0.000 0.000 0.000 0.000 0.000 0.250]
  360. [ 0.000 0.000 0.000 0.000 0.333 0.000]
  361. [ 0.000 0.000 0.000 0.000 0.000 0.250]]
  362. [rosout][DEBUG] 2018-03-19 23:27:18,573: ----------------------------------------------
  363. [rosout][DEBUG] 2018-03-19 23:27:18,574:
  364.  
  365. J . J_p:
  366. [[ 0.000 0.000 0.000 0.000 0.000 0.000]
  367. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  368. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  369. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  370. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  371. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  372. [rosout][DEBUG] 2018-03-19 23:27:18,574: ----------------------------------------------
  373. [rosout][DEBUG] 2018-03-19 23:27:18,577:
  374.  
  375. dq:
  376. [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
  377. [rosout][DEBUG] 2018-03-19 23:27:18,579: ----------------------------------------------
  378. [rosout][DEBUG] 2018-03-19 23:27:18,579:
  379.  
  380. dq:
  381. [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
  382. [rosout][DEBUG] 2018-03-19 23:27:18,582: ----------------------------------------------
  383.  
  384.  
  385.  
  386.  
  387.  
  388.  
  389.  
  390.  
  391.  
  392.  
  393.  
  394.  
  395.  
  396.  
  397.  
  398.  
  399. [rosout][INFO] 2018-03-19 23:28:52,212: =============== cartesian_control() ===============
  400. [rosout][DEBUG] 2018-03-19 23:28:52,213:
  401.  
  402. joint_transforms:
  403. [rosout][DEBUG] 2018-03-19 23:28:52,214:
  404.  
  405. joint_transforms[0]:
  406. [[ 1.000 0.000 0.000 0.000]
  407. [ 0.000 1.000 0.000 0.000]
  408. [ 0.000 0.000 1.000 0.310]
  409. [ 0.000 0.000 0.000 1.000]]
  410. [rosout][DEBUG] 2018-03-19 23:28:52,215:
  411.  
  412. joint_transforms[1]:
  413. [[ 1.000 0.000 0.000 0.000]
  414. [ 0.000 0.000 1.000 0.000]
  415. [ 0.000 -1.000 0.000 0.510]
  416. [ 0.000 0.000 0.000 1.000]]
  417. [rosout][DEBUG] 2018-03-19 23:28:52,215:
  418.  
  419. joint_transforms[2]:
  420. [[ 1.000 0.000 0.000 0.000]
  421. [ 0.000 1.000 0.000 0.000]
  422. [ 0.000 0.000 1.000 0.710]
  423. [ 0.000 0.000 0.000 1.000]]
  424. [rosout][DEBUG] 2018-03-19 23:28:52,216:
  425.  
  426. joint_transforms[3]:
  427. [[ 1.000 0.000 0.000 0.000]
  428. [ 0.000 0.000 -1.000 0.000]
  429. [ 0.000 1.000 0.000 0.910]
  430. [ 0.000 0.000 0.000 1.000]]
  431. [rosout][DEBUG] 2018-03-19 23:28:52,219:
  432.  
  433. joint_transforms[4]:
  434. [[ 1.000 0.000 0.000 0.000]
  435. [ 0.000 1.000 0.000 0.000]
  436. [ 0.000 0.000 1.000 1.110]
  437. [ 0.000 0.000 0.000 1.000]]
  438. [rosout][DEBUG] 2018-03-19 23:28:52,219:
  439.  
  440. joint_transforms[5]:
  441. [[ 1.000 0.000 0.000 0.000]
  442. [ 0.000 0.000 1.000 0.000]
  443. [ 0.000 -1.000 0.000 1.300]
  444. [ 0.000 0.000 0.000 1.000]]
  445. [rosout][DEBUG] 2018-03-19 23:28:52,220:
  446.  
  447. joint_transforms[6]:
  448. [[ 1.000 0.000 0.000 0.000]
  449. [ 0.000 1.000 0.000 0.000]
  450. [ 0.000 0.000 1.000 1.378]
  451. [ 0.000 0.000 0.000 1.000]]
  452. [rosout][DEBUG] 2018-03-19 23:28:52,221: ----------------------------------------------
  453. [rosout][DEBUG] 2018-03-19 23:28:52,221:
  454.  
  455. b_T_ee_current:
  456. [[ 1.000 0.000 0.000 0.000]
  457. [ 0.000 1.000 0.000 0.000]
  458. [ 0.000 0.000 1.000 1.378]
  459. [ 0.000 0.000 0.000 1.000]]
  460. [rosout][DEBUG] 2018-03-19 23:28:52,222: ----------------------------------------------
  461. [rosout][DEBUG] 2018-03-19 23:28:52,222:
  462.  
  463. b_T_ee_desired:
  464. [[ 0.894 0.000 -0.447 0.000]
  465. [ 0.000 1.000 0.000 0.000]
  466. [ 0.447 -0.000 0.894 1.378]
  467. [ 0.000 0.000 0.000 1.000]]
  468. [rosout][DEBUG] 2018-03-19 23:28:52,223: ----------------------------------------------
  469. [rosout][DEBUG] 2018-03-19 23:28:52,223:
  470.  
  471. red_control:
  472. False
  473. [rosout][DEBUG] 2018-03-19 23:28:52,224: ----------------------------------------------
  474. [rosout][DEBUG] 2018-03-19 23:28:52,224:
  475.  
  476. q_current:
  477. (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
  478. [rosout][DEBUG] 2018-03-19 23:28:52,225: ----------------------------------------------
  479. [rosout][DEBUG] 2018-03-19 23:28:52,225:
  480.  
  481. q0_desired:
  482. 0
  483. [rosout][DEBUG] 2018-03-19 23:28:52,227: ----------------------------------------------
  484. [rosout][DEBUG] 2018-03-19 23:28:52,229:
  485.  
  486. b_p_current:
  487. [ 0.000 0.000 1.378 1.000]
  488. [rosout][DEBUG] 2018-03-19 23:28:52,229: ----------------------------------------------
  489. [rosout][DEBUG] 2018-03-19 23:28:52,230:
  490.  
  491. b_p_desired:
  492. [ 0.000 0.000 1.378 1.000]
  493. [rosout][DEBUG] 2018-03-19 23:28:52,230: ----------------------------------------------
  494. [rosout][DEBUG] 2018-03-19 23:28:52,231:
  495.  
  496. x_current:
  497. [ 0.000 0.000 1.378 0.000 -0.000 0.000]
  498. [rosout][DEBUG] 2018-03-19 23:28:52,231: ----------------------------------------------
  499. [rosout][DEBUG] 2018-03-19 23:28:52,232:
  500.  
  501. x_desired:
  502. [ 0.000 0.000 1.378 -0.000 -0.464 0.000]
  503. [rosout][DEBUG] 2018-03-19 23:28:52,232: ----------------------------------------------
  504. [rosout][DEBUG] 2018-03-19 23:28:52,233:
  505.  
  506. b_dX:
  507. [ 0.000 0.000 -0.000 -0.000 -0.464 0.000]
  508. [rosout][DEBUG] 2018-03-19 23:28:52,234: ----------------------------------------------
  509. [rosout][DEBUG] 2018-03-19 23:28:52,234:
  510.  
  511. b_X_dot:
  512. [ 0.000 0.000 -0.000 -0.000 -0.232 0.000]
  513. [rosout][DEBUG] 2018-03-19 23:28:52,234: ----------------------------------------------
  514. [rosout][DEBUG] 2018-03-19 23:28:52,241:
  515.  
  516. V:
  517. [[ 1.000 0.000 0.000 0.000 0.000 0.000]
  518. [ 0.000 1.000 0.000 0.000 0.000 0.000]
  519. [ 0.000 0.000 1.000 0.000 0.000 0.000]
  520. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  521. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  522. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  523. [rosout][DEBUG] 2018-03-19 23:28:52,244: ----------------------------------------------
  524. [rosout][DEBUG] 2018-03-19 23:28:52,247:
  525.  
  526. V_ee:
  527. [ 0.000 0.000 -0.000 -0.000 -0.232 0.000]
  528. [rosout][DEBUG] 2018-03-19 23:28:52,249: ----------------------------------------------
  529. [rosout][DEBUG] 2018-03-19 23:28:52,250: ++++++++++++++++++++++++++++++++++++++++++++++
  530. [rosout][DEBUG] 2018-03-19 23:28:52,252:
  531.  
  532. ee_T_ee_desired:
  533. [[ 0.894 0.000 -0.447 0.000]
  534. [ 0.000 1.000 0.000 0.000]
  535. [ 0.447 -0.000 0.894 -0.000]
  536. [ 0.000 0.000 0.000 1.000]]
  537. [rosout][DEBUG] 2018-03-19 23:28:52,253: ----------------------------------------------
  538. [rosout][DEBUG] 2018-03-19 23:28:52,254:
  539.  
  540. delta_X:
  541. [ 0.000 0.000 -0.000]
  542. [rosout][DEBUG] 2018-03-19 23:28:52,256: ----------------------------------------------
  543. [rosout][DEBUG] 2018-03-19 23:28:52,257:
  544.  
  545. delta_X:
  546. [ 0.000 0.000 -0.000]
  547. [rosout][DEBUG] 2018-03-19 23:28:52,258: ----------------------------------------------
  548. [rosout][DEBUG] 2018-03-19 23:28:52,262:
  549.  
  550. delta_W:
  551. [-0.000 -0.464 -0.000]
  552. [rosout][DEBUG] 2018-03-19 23:28:52,264: ----------------------------------------------
  553. [rosout][DEBUG] 2018-03-19 23:28:52,264:
  554.  
  555. delta_W:
  556. [-0.000 -0.464 -0.000]
  557. [rosout][DEBUG] 2018-03-19 23:28:52,265: ----------------------------------------------
  558. [rosout][DEBUG] 2018-03-19 23:28:52,265:
  559.  
  560. V_ee:
  561. [ 0.000 0.000 -0.000 -0.000 -0.464 -0.000]
  562. [rosout][DEBUG] 2018-03-19 23:28:52,267: ----------------------------------------------
  563. [rosout][DEBUG] 2018-03-19 23:28:52,268: ----------------------------------------------
  564. [rosout][DEBUG] 2018-03-19 23:28:52,269: ----------------------------------------------
  565. [rosout][DEBUG] 2018-03-19 23:28:52,269:
  566.  
  567. j_T_ee:
  568. [rosout][DEBUG] 2018-03-19 23:28:52,270:
  569.  
  570. j_T_ee[0]:
  571. [[ 1.000 0.000 0.000 0.000]
  572. [ 0.000 1.000 0.000 0.000]
  573. [ 0.000 0.000 1.000 1.068]
  574. [ 0.000 0.000 0.000 1.000]]
  575. [rosout][DEBUG] 2018-03-19 23:28:52,271:
  576.  
  577. j_T_ee[1]:
  578. [[ 1.000 0.000 0.000 0.000]
  579. [ 0.000 0.000 -1.000 -0.868]
  580. [ 0.000 1.000 0.000 0.000]
  581. [ 0.000 0.000 0.000 1.000]]
  582. [rosout][DEBUG] 2018-03-19 23:28:52,273:
  583.  
  584. j_T_ee[2]:
  585. [[ 1.000 0.000 0.000 0.000]
  586. [ 0.000 1.000 0.000 0.000]
  587. [ 0.000 0.000 1.000 0.668]
  588. [ 0.000 0.000 0.000 1.000]]
  589. [rosout][DEBUG] 2018-03-19 23:28:52,274:
  590.  
  591. j_T_ee[3]:
  592. [[ 1.000 0.000 0.000 0.000]
  593. [ 0.000 0.000 1.000 0.468]
  594. [ 0.000 -1.000 0.000 0.000]
  595. [ 0.000 0.000 0.000 1.000]]
  596. [rosout][DEBUG] 2018-03-19 23:28:52,278:
  597.  
  598. j_T_ee[4]:
  599. [[ 1.000 0.000 0.000 0.000]
  600. [ 0.000 1.000 0.000 0.000]
  601. [ 0.000 0.000 1.000 0.268]
  602. [ 0.000 0.000 0.000 1.000]]
  603. [rosout][DEBUG] 2018-03-19 23:28:52,281:
  604.  
  605. j_T_ee[5]:
  606. [[ 1.000 0.000 0.000 0.000]
  607. [ 0.000 0.000 -1.000 -0.078]
  608. [ 0.000 1.000 0.000 0.000]
  609. [ 0.000 0.000 0.000 1.000]]
  610. [rosout][DEBUG] 2018-03-19 23:28:52,282:
  611.  
  612. j_T_ee[6]:
  613. [[ 1.000 0.000 0.000 0.000]
  614. [ 0.000 1.000 0.000 0.000]
  615. [ 0.000 0.000 1.000 0.000]
  616. [ 0.000 0.000 0.000 1.000]]
  617. [rosout][DEBUG] 2018-03-19 23:28:52,284: ----------------------------------------------
  618. [rosout][DEBUG] 2018-03-19 23:28:52,285:
  619.  
  620. ee_T_j:
  621. [rosout][DEBUG] 2018-03-19 23:28:52,285:
  622.  
  623. ee_T_j[0]:
  624. [[ 1.000 0.000 0.000 0.000]
  625. [ 0.000 1.000 0.000 0.000]
  626. [ 0.000 0.000 1.000 -1.068]
  627. [ 0.000 0.000 0.000 1.000]]
  628. [rosout][DEBUG] 2018-03-19 23:28:52,287:
  629.  
  630. ee_T_j[1]:
  631. [[ 1.000 0.000 0.000 0.000]
  632. [ 0.000 0.000 1.000 0.000]
  633. [-0.000 -1.000 0.000 -0.868]
  634. [ 0.000 0.000 0.000 1.000]]
  635. [rosout][DEBUG] 2018-03-19 23:28:52,288:
  636.  
  637. ee_T_j[2]:
  638. [[ 1.000 0.000 0.000 0.000]
  639. [ 0.000 1.000 0.000 0.000]
  640. [ 0.000 0.000 1.000 -0.668]
  641. [ 0.000 0.000 0.000 1.000]]
  642. [rosout][DEBUG] 2018-03-19 23:28:52,289:
  643.  
  644. ee_T_j[3]:
  645. [[ 1.000 0.000 0.000 0.000]
  646. [-0.000 0.000 -1.000 -0.000]
  647. [ 0.000 1.000 0.000 -0.468]
  648. [ 0.000 0.000 0.000 1.000]]
  649. [rosout][DEBUG] 2018-03-19 23:28:52,291:
  650.  
  651. ee_T_j[4]:
  652. [[ 1.000 0.000 0.000 0.000]
  653. [ 0.000 1.000 0.000 0.000]
  654. [ 0.000 0.000 1.000 -0.268]
  655. [ 0.000 0.000 0.000 1.000]]
  656. [rosout][DEBUG] 2018-03-19 23:28:52,294:
  657.  
  658. ee_T_j[5]:
  659. [[ 1.000 0.000 0.000 0.000]
  660. [ 0.000 0.000 1.000 0.000]
  661. [-0.000 -1.000 0.000 -0.078]
  662. [ 0.000 0.000 0.000 1.000]]
  663. [rosout][DEBUG] 2018-03-19 23:28:52,295:
  664.  
  665. ee_T_j[6]:
  666. [[ 1.000 0.000 0.000 0.000]
  667. [ 0.000 1.000 0.000 0.000]
  668. [ 0.000 0.000 1.000 0.000]
  669. [ 0.000 0.000 0.000 1.000]]
  670. [rosout][DEBUG] 2018-03-19 23:28:52,296: ----------------------------------------------
  671. [rosout][DEBUG] 2018-03-19 23:28:52,297:
  672.  
  673. V_j:
  674. [rosout][DEBUG] 2018-03-19 23:28:52,298:
  675.  
  676. V_j[0]:
  677. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  678. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  679. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  680. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  681. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  682. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  683. [rosout][DEBUG] 2018-03-19 23:28:52,301:
  684.  
  685. V_j[1]:
  686. [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
  687. [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
  688. [-0.000 -1.000 0.000 0.000 0.000 -0.000]
  689. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  690. [ 0.000 0.000 0.000 0.000 0.000 1.000]
  691. [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
  692. [rosout][DEBUG] 2018-03-19 23:28:52,302:
  693.  
  694. V_j[2]:
  695. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  696. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  697. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  698. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  699. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  700. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  701. [rosout][DEBUG] 2018-03-19 23:28:52,304:
  702.  
  703. V_j[3]:
  704. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  705. [-0.000 0.000 -1.000 0.000 -0.000 -0.000]
  706. [ 0.000 1.000 0.000 0.000 -0.000 -0.000]
  707. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  708. [ 0.000 0.000 0.000 -0.000 0.000 -1.000]
  709. [ 0.000 0.000 0.000 0.000 1.000 0.000]]
  710. [rosout][DEBUG] 2018-03-19 23:28:52,305:
  711.  
  712. V_j[4]:
  713. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  714. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  715. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  716. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  717. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  718. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  719. [rosout][DEBUG] 2018-03-19 23:28:52,308:
  720.  
  721. V_j[5]:
  722. [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
  723. [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
  724. [-0.000 -1.000 0.000 0.000 0.000 -0.000]
  725. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  726. [ 0.000 0.000 0.000 0.000 0.000 1.000]
  727. [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
  728. [rosout][DEBUG] 2018-03-19 23:28:52,313:
  729.  
  730. V_j[6]:
  731. [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
  732. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  733. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  734. [ 0.000 0.000 0.000 1.000 0.000 0.000]
  735. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  736. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  737. [rosout][DEBUG] 2018-03-19 23:28:52,315: ----------------------------------------------
  738. [rosout][DEBUG] 2018-03-19 23:28:52,316: ----------------------------------------------
  739. [rosout][DEBUG] 2018-03-19 23:28:52,317: ----------------------------------------------
  740. [rosout][DEBUG] 2018-03-19 23:28:52,318:
  741.  
  742. J:
  743. [[-0.000 0.000 -0.000 -0.000 -0.000 0.000 -0.000]
  744. [ 0.000 0.000 0.000 -0.000 0.000 0.000 0.000]
  745. [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
  746. [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
  747. [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
  748. [ 1.000 0.000 1.000 0.000 1.000 0.000 1.000]]
  749. [rosout][DEBUG] 2018-03-19 23:28:52,320: ----------------------------------------------
  750. [rosout][DEBUG] 2018-03-19 23:28:52,321:
  751.  
  752. J_p:
  753. [[ 0.000 0.000 0.000 0.000 0.000 0.250]
  754. [ 0.000 0.000 0.000 0.000 0.333 0.000]
  755. [ 0.000 0.000 0.000 0.000 0.000 0.250]
  756. [ 0.000 0.000 0.000 0.000 -0.333 0.000]
  757. [ 0.000 0.000 0.000 0.000 0.000 0.250]
  758. [ 0.000 0.000 0.000 0.000 0.333 0.000]
  759. [ 0.000 0.000 0.000 0.000 0.000 0.250]]
  760. [rosout][DEBUG] 2018-03-19 23:28:52,323: ----------------------------------------------
  761. [rosout][DEBUG] 2018-03-19 23:28:52,325:
  762.  
  763. J . J_p:
  764. [[ 0.000 0.000 0.000 0.000 0.000 0.000]
  765. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  766. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  767. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  768. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  769. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  770. [rosout][DEBUG] 2018-03-19 23:28:52,327: ----------------------------------------------
  771. [rosout][DEBUG] 2018-03-19 23:28:52,328:
  772.  
  773. dq:
  774. [-0.000 -0.155 -0.000 0.155 -0.000 -0.155 -0.000]
  775. [rosout][DEBUG] 2018-03-19 23:28:52,331: ----------------------------------------------
  776. [rosout][DEBUG] 2018-03-19 23:28:52,331:
  777.  
  778. dq:
  779. [-0.000 -0.155 -0.000 0.155 -0.000 -0.155 -0.000]
  780. [rosout][DEBUG] 2018-03-19 23:28:52,332: ----------------------------------------------
  781.  
  782.  
  783.  
  784.  
  785.  
  786.  
  787.  
  788.  
  789.  
  790.  
  791.  
  792.  
  793.  
  794.  
  795.  
  796. [rosout][INFO] 2018-03-19 23:28:52,499: =============== cartesian_control() ===============
  797. [rosout][DEBUG] 2018-03-19 23:28:52,500:
  798.  
  799. joint_transforms:
  800. [rosout][DEBUG] 2018-03-19 23:28:52,502:
  801.  
  802. joint_transforms[0]:
  803. [[ 1.000 0.000 0.000 0.000]
  804. [ 0.000 1.000 0.000 0.000]
  805. [ 0.000 0.000 1.000 0.310]
  806. [ 0.000 0.000 0.000 1.000]]
  807. [rosout][DEBUG] 2018-03-19 23:28:52,504:
  808.  
  809. joint_transforms[1]:
  810. [[ 1.000 0.000 0.000 0.000]
  811. [-0.000 0.000 1.000 0.000]
  812. [ 0.000 -1.000 0.000 0.510]
  813. [ 0.000 0.000 0.000 1.000]]
  814. [rosout][DEBUG] 2018-03-19 23:28:52,505:
  815.  
  816. joint_transforms[2]:
  817. [[ 1.000 0.000 -0.009 -0.002]
  818. [-0.000 1.000 0.000 0.000]
  819. [ 0.009 0.000 1.000 0.710]
  820. [ 0.000 0.000 0.000 1.000]]
  821. [rosout][DEBUG] 2018-03-19 23:28:52,506:
  822.  
  823. joint_transforms[3]:
  824. [[ 1.000 -0.009 -0.000 -0.004]
  825. [-0.000 0.000 -1.000 0.000]
  826. [ 0.009 1.000 -0.000 0.910]
  827. [ 0.000 0.000 0.000 1.000]]
  828. [rosout][DEBUG] 2018-03-19 23:28:52,507:
  829.  
  830. joint_transforms[4]:
  831. [[ 1.000 0.000 -0.018 -0.007]
  832. [-0.000 1.000 0.000 0.000]
  833. [ 0.018 0.000 1.000 1.110]
  834. [ 0.000 0.000 0.000 1.000]]
  835. [rosout][DEBUG] 2018-03-19 23:28:52,509:
  836.  
  837. joint_transforms[5]:
  838. [[ 1.000 0.018 0.000 -0.010]
  839. [-0.000 -0.000 1.000 0.000]
  840. [ 0.018 -1.000 0.000 1.300]
  841. [ 0.000 0.000 0.000 1.000]]
  842. [rosout][DEBUG] 2018-03-19 23:28:52,511:
  843.  
  844. joint_transforms[6]:
  845. [[ 1.000 0.000 -0.026 -0.012]
  846. [-0.000 1.000 0.000 0.000]
  847. [ 0.026 0.000 1.000 1.378]
  848. [ 0.000 0.000 0.000 1.000]]
  849. [rosout][DEBUG] 2018-03-19 23:28:52,512: ----------------------------------------------
  850. [rosout][DEBUG] 2018-03-19 23:28:52,513:
  851.  
  852. b_T_ee_current:
  853. [[ 1.000 0.000 -0.026 -0.012]
  854. [-0.000 1.000 0.000 0.000]
  855. [ 0.026 0.000 1.000 1.378]
  856. [ 0.000 0.000 0.000 1.000]]
  857. [rosout][DEBUG] 2018-03-19 23:28:52,515: ----------------------------------------------
  858. [rosout][DEBUG] 2018-03-19 23:28:52,516:
  859.  
  860. b_T_ee_desired:
  861. [[ 0.866 0.000 -0.500 0.000]
  862. [ 0.000 1.000 0.000 0.000]
  863. [ 0.500 -0.000 0.866 1.378]
  864. [ 0.000 0.000 0.000 1.000]]
  865. [rosout][DEBUG] 2018-03-19 23:28:52,518: ----------------------------------------------
  866. [rosout][DEBUG] 2018-03-19 23:28:52,518:
  867.  
  868. red_control:
  869. False
  870. [rosout][DEBUG] 2018-03-19 23:28:52,519: ----------------------------------------------
  871. [rosout][DEBUG] 2018-03-19 23:28:52,520:
  872.  
  873. q_current:
  874. (-1.8967685315544776e-12, -0.008808642438297074, -1.8967693784890137e-12, 0.008808642438297074, -1.8967693784890137e-12, -0.008808642438297074, -1.8967693784890137e-12)
  875. [rosout][DEBUG] 2018-03-19 23:28:52,522: ----------------------------------------------
  876. [rosout][DEBUG] 2018-03-19 23:28:52,523:
  877.  
  878. q0_desired:
  879. 0
  880. [rosout][DEBUG] 2018-03-19 23:28:52,524: ----------------------------------------------
  881. [rosout][DEBUG] 2018-03-19 23:28:52,525:
  882.  
  883. b_p_current:
  884. [-0.012 0.000 1.378 1.000]
  885. [rosout][DEBUG] 2018-03-19 23:28:52,526: ----------------------------------------------
  886. [rosout][DEBUG] 2018-03-19 23:28:52,526:
  887.  
  888. b_p_desired:
  889. [ 0.000 0.000 1.378 1.000]
  890. [rosout][DEBUG] 2018-03-19 23:28:52,527: ----------------------------------------------
  891. [rosout][DEBUG] 2018-03-19 23:28:52,528:
  892.  
  893. x_current:
  894. [-0.012 0.000 1.378 0.000 -0.026 -0.000]
  895. [rosout][DEBUG] 2018-03-19 23:28:52,529: ----------------------------------------------
  896. [rosout][DEBUG] 2018-03-19 23:28:52,530:
  897.  
  898. x_desired:
  899. [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
  900. [rosout][DEBUG] 2018-03-19 23:28:52,531: ----------------------------------------------
  901. [rosout][DEBUG] 2018-03-19 23:28:52,532:
  902.  
  903. b_dX:
  904. [ 0.012 -0.000 0.000 -0.000 -0.497 0.000]
  905. [rosout][DEBUG] 2018-03-19 23:28:52,533: ----------------------------------------------
  906. [rosout][DEBUG] 2018-03-19 23:28:52,536:
  907.  
  908. b_X_dot:
  909. [ 0.006 -0.000 0.000 -0.000 -0.248 0.000]
  910. [rosout][DEBUG] 2018-03-19 23:28:52,536: ----------------------------------------------
  911. [rosout][DEBUG] 2018-03-19 23:28:52,537:
  912.  
  913. V:
  914. [[ 1.000 -0.000 0.026 0.000 0.000 0.000]
  915. [ 0.000 1.000 0.000 0.000 0.000 0.000]
  916. [-0.026 0.000 1.000 0.000 0.000 0.000]
  917. [ 0.000 0.000 0.000 1.000 -0.000 0.026]
  918. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  919. [ 0.000 0.000 0.000 -0.026 0.000 1.000]]
  920. [rosout][DEBUG] 2018-03-19 23:28:52,539: ----------------------------------------------
  921. [rosout][DEBUG] 2018-03-19 23:28:52,541:
  922.  
  923. V_ee:
  924. [ 0.006 0.000 -0.000 -0.000 -0.248 0.000]
  925. [rosout][DEBUG] 2018-03-19 23:28:52,544: ----------------------------------------------
  926. [rosout][DEBUG] 2018-03-19 23:28:52,544: ++++++++++++++++++++++++++++++++++++++++++++++
  927. [rosout][DEBUG] 2018-03-19 23:28:52,552:
  928.  
  929. ee_T_ee_desired:
  930. [[ 0.879 0.000 -0.476 0.012]
  931. [ 0.000 1.000 0.000 0.000]
  932. [ 0.476 -0.000 0.879 -0.000]
  933. [ 0.000 0.000 0.000 1.000]]
  934. [rosout][DEBUG] 2018-03-19 23:28:52,553: ----------------------------------------------
  935. [rosout][DEBUG] 2018-03-19 23:28:52,554:
  936.  
  937. delta_X:
  938. [ 0.006 0.000 -0.000]
  939. [rosout][DEBUG] 2018-03-19 23:28:52,555: ----------------------------------------------
  940. [rosout][DEBUG] 2018-03-19 23:28:52,556:
  941.  
  942. delta_X:
  943. [ 0.006 0.000 -0.000]
  944. [rosout][DEBUG] 2018-03-19 23:28:52,556: ----------------------------------------------
  945. [rosout][DEBUG] 2018-03-19 23:28:52,558:
  946.  
  947. delta_W:
  948. [-0.000 -0.497 0.000]
  949. [rosout][DEBUG] 2018-03-19 23:28:52,558: ----------------------------------------------
  950. [rosout][DEBUG] 2018-03-19 23:28:52,559:
  951.  
  952. delta_W:
  953. [-0.000 -0.497 0.000]
  954. [rosout][DEBUG] 2018-03-19 23:28:52,560: ----------------------------------------------
  955. [rosout][DEBUG] 2018-03-19 23:28:52,561:
  956.  
  957. V_ee:
  958. [ 0.006 0.000 -0.000 -0.000 -0.497 0.000]
  959. [rosout][DEBUG] 2018-03-19 23:28:52,561: ----------------------------------------------
  960. [rosout][DEBUG] 2018-03-19 23:28:52,562: ----------------------------------------------
  961. [rosout][DEBUG] 2018-03-19 23:28:52,564: ----------------------------------------------
  962. [rosout][DEBUG] 2018-03-19 23:28:52,572:
  963.  
  964. j_T_ee:
  965. [rosout][DEBUG] 2018-03-19 23:28:52,575:
  966.  
  967. j_T_ee[0]:
  968. [[ 1.000 0.000 -0.026 -0.012]
  969. [-0.000 1.000 0.000 0.000]
  970. [ 0.026 0.000 1.000 1.068]
  971. [ 0.000 0.000 0.000 1.000]]
  972. [rosout][DEBUG] 2018-03-19 23:28:52,577:
  973.  
  974. j_T_ee[1]:
  975. [[ 1.000 0.000 -0.026 -0.012]
  976. [-0.026 -0.000 -1.000 -0.868]
  977. [-0.000 1.000 0.000 0.000]
  978. [ 0.000 0.000 0.000 1.000]]
  979. [rosout][DEBUG] 2018-03-19 23:28:52,579:
  980.  
  981. j_T_ee[2]:
  982. [[ 1.000 0.000 -0.018 -0.005]
  983. [-0.000 1.000 0.000 0.000]
  984. [ 0.018 0.000 1.000 0.668]
  985. [ 0.000 0.000 0.000 1.000]]
  986. [rosout][DEBUG] 2018-03-19 23:28:52,582:
  987.  
  988. j_T_ee[3]:
  989. [[ 1.000 0.000 -0.018 -0.005]
  990. [ 0.018 0.000 1.000 0.468]
  991. [ 0.000 -1.000 -0.000 -0.000]
  992. [ 0.000 0.000 0.000 1.000]]
  993. [rosout][DEBUG] 2018-03-19 23:28:52,584:
  994.  
  995. j_T_ee[4]:
  996. [[ 1.000 0.000 -0.009 -0.001]
  997. [-0.000 1.000 0.000 0.000]
  998. [ 0.009 0.000 1.000 0.268]
  999. [ 0.000 0.000 0.000 1.000]]
  1000. [rosout][DEBUG] 2018-03-19 23:28:52,585:
  1001.  
  1002. j_T_ee[5]:
  1003. [[ 1.000 0.000 -0.009 -0.001]
  1004. [-0.009 -0.000 -1.000 -0.078]
  1005. [-0.000 1.000 0.000 0.000]
  1006. [ 0.000 0.000 0.000 1.000]]
  1007. [rosout][DEBUG] 2018-03-19 23:28:52,586:
  1008.  
  1009. j_T_ee[6]:
  1010. [[ 1.000 0.000 0.000 -0.000]
  1011. [-0.000 1.000 0.000 0.000]
  1012. [-0.000 -0.000 1.000 0.000]
  1013. [ 0.000 0.000 0.000 1.000]]
  1014. [rosout][DEBUG] 2018-03-19 23:28:52,588: ----------------------------------------------
  1015. [rosout][DEBUG] 2018-03-19 23:28:52,588:
  1016.  
  1017. ee_T_j:
  1018. [rosout][DEBUG] 2018-03-19 23:28:52,589:
  1019.  
  1020. ee_T_j[0]:
  1021. [[ 1.000 -0.000 0.026 -0.016]
  1022. [ 0.000 1.000 0.000 -0.000]
  1023. [-0.026 0.000 1.000 -1.068]
  1024. [ 0.000 0.000 0.000 1.000]]
  1025. [rosout][DEBUG] 2018-03-19 23:28:52,590:
  1026.  
  1027. ee_T_j[1]:
  1028. [[ 1.000 -0.026 -0.000 -0.010]
  1029. [ 0.000 -0.000 1.000 -0.000]
  1030. [-0.026 -1.000 0.000 -0.868]
  1031. [ 0.000 0.000 0.000 1.000]]
  1032. [rosout][DEBUG] 2018-03-19 23:28:52,592:
  1033.  
  1034. ee_T_j[2]:
  1035. [[ 1.000 -0.000 0.018 -0.007]
  1036. [ 0.000 1.000 0.000 -0.000]
  1037. [-0.018 0.000 1.000 -0.668]
  1038. [ 0.000 0.000 0.000 1.000]]
  1039. [rosout][DEBUG] 2018-03-19 23:28:52,593:
  1040.  
  1041. ee_T_j[3]:
  1042. [[ 1.000 0.018 0.000 -0.003]
  1043. [ 0.000 0.000 -1.000 -0.000]
  1044. [-0.018 1.000 -0.000 -0.468]
  1045. [ 0.000 0.000 0.000 1.000]]
  1046. [rosout][DEBUG] 2018-03-19 23:28:52,594:
  1047.  
  1048. ee_T_j[4]:
  1049. [[ 1.000 -0.000 0.009 -0.002]
  1050. [ 0.000 1.000 0.000 -0.000]
  1051. [-0.009 0.000 1.000 -0.268]
  1052. [ 0.000 0.000 0.000 1.000]]
  1053. [rosout][DEBUG] 2018-03-19 23:28:52,597:
  1054.  
  1055. ee_T_j[5]:
  1056. [[ 1.000 -0.009 -0.000 -0.000]
  1057. [ 0.000 -0.000 1.000 0.000]
  1058. [-0.009 -1.000 0.000 -0.078]
  1059. [ 0.000 0.000 0.000 1.000]]
  1060. [rosout][DEBUG] 2018-03-19 23:28:52,598:
  1061.  
  1062. ee_T_j[6]:
  1063. [[ 1.000 -0.000 -0.000 0.000]
  1064. [ 0.000 1.000 -0.000 0.000]
  1065. [ 0.000 0.000 1.000 0.000]
  1066. [ 0.000 0.000 0.000 1.000]]
  1067. [rosout][DEBUG] 2018-03-19 23:28:52,600: ----------------------------------------------
  1068. [rosout][DEBUG] 2018-03-19 23:28:52,601:
  1069.  
  1070. V_j:
  1071. [rosout][DEBUG] 2018-03-19 23:28:52,603:
  1072.  
  1073. V_j[0]:
  1074. [[ 1.000 -0.000 0.026 -0.000 -0.000 -0.000]
  1075. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1076. [-0.026 0.000 1.000 -0.000 0.000 -0.000]
  1077. [ 0.000 0.000 0.000 1.000 -0.000 0.026]
  1078. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1079. [ 0.000 0.000 0.000 -0.026 0.000 1.000]]
  1080. [rosout][DEBUG] 2018-03-19 23:28:52,604:
  1081.  
  1082. V_j[1]:
  1083. [[ 1.000 -0.026 -0.000 -0.000 -0.000 -0.000]
  1084. [ 0.000 -0.000 1.000 -0.000 0.000 -0.012]
  1085. [-0.026 -1.000 0.000 0.023 -0.012 -0.000]
  1086. [ 0.000 0.000 0.000 1.000 -0.026 -0.000]
  1087. [ 0.000 0.000 0.000 0.000 -0.000 1.000]
  1088. [ 0.000 0.000 0.000 -0.026 -1.000 0.000]]
  1089. [rosout][DEBUG] 2018-03-19 23:28:52,608:
  1090.  
  1091. V_j[2]:
  1092. [[ 1.000 -0.000 0.018 -0.000 -0.000 -0.000]
  1093. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1094. [-0.018 0.000 1.000 -0.000 0.000 -0.000]
  1095. [ 0.000 0.000 0.000 1.000 -0.000 0.018]
  1096. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1097. [ 0.000 0.000 0.000 -0.018 0.000 1.000]]
  1098. [rosout][DEBUG] 2018-03-19 23:28:52,611:
  1099.  
  1100. V_j[3]:
  1101. [[ 1.000 0.018 0.000 -0.000 -0.000 -0.000]
  1102. [ 0.000 0.000 -1.000 0.000 -0.000 0.005]
  1103. [-0.018 1.000 -0.000 -0.008 0.005 0.000]
  1104. [ 0.000 0.000 0.000 1.000 0.018 0.000]
  1105. [ 0.000 0.000 0.000 0.000 0.000 -1.000]
  1106. [ 0.000 0.000 0.000 -0.018 1.000 -0.000]]
  1107. [rosout][DEBUG] 2018-03-19 23:28:52,614:
  1108.  
  1109. V_j[4]:
  1110. [[ 1.000 -0.000 0.009 -0.000 -0.000 -0.000]
  1111. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1112. [-0.009 0.000 1.000 -0.000 0.000 -0.000]
  1113. [ 0.000 0.000 0.000 1.000 -0.000 0.009]
  1114. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1115. [ 0.000 0.000 0.000 -0.009 0.000 1.000]]
  1116. [rosout][DEBUG] 2018-03-19 23:28:52,615:
  1117.  
  1118. V_j[5]:
  1119. [[ 1.000 -0.009 -0.000 -0.000 -0.000 -0.000]
  1120. [ 0.000 -0.000 1.000 -0.000 0.000 -0.001]
  1121. [-0.009 -1.000 0.000 0.001 -0.001 -0.000]
  1122. [ 0.000 0.000 0.000 1.000 -0.009 -0.000]
  1123. [ 0.000 0.000 0.000 0.000 -0.000 1.000]
  1124. [ 0.000 0.000 0.000 -0.009 -1.000 0.000]]
  1125. [rosout][DEBUG] 2018-03-19 23:28:52,616:
  1126.  
  1127. V_j[6]:
  1128. [[ 1.000 -0.000 -0.000 -0.000 -0.000 0.000]
  1129. [ 0.000 1.000 -0.000 -0.000 -0.000 0.000]
  1130. [ 0.000 0.000 1.000 0.000 0.000 -0.000]
  1131. [ 0.000 0.000 0.000 1.000 -0.000 -0.000]
  1132. [ 0.000 0.000 0.000 0.000 1.000 -0.000]
  1133. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  1134. [rosout][DEBUG] 2018-03-19 23:28:52,617: ----------------------------------------------
  1135. [rosout][DEBUG] 2018-03-19 23:28:52,618: ----------------------------------------------
  1136. [rosout][DEBUG] 2018-03-19 23:28:52,619: ----------------------------------------------
  1137. [rosout][DEBUG] 2018-03-19 23:28:52,619:
  1138.  
  1139. J:
  1140. [[-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 0.000]
  1141. [-0.000 -0.012 -0.000 0.005 -0.000 -0.001 0.000]
  1142. [-0.000 -0.000 -0.000 0.000 -0.000 -0.000 -0.000]
  1143. [ 0.026 -0.000 0.018 0.000 0.009 -0.000 -0.000]
  1144. [ 0.000 1.000 0.000 -1.000 0.000 1.000 -0.000]
  1145. [ 1.000 0.000 1.000 -0.000 1.000 0.000 1.000]]
  1146. [rosout][DEBUG] 2018-03-19 23:28:52,621: ----------------------------------------------
  1147. [rosout][DEBUG] 2018-03-19 23:28:52,622:
  1148.  
  1149. J_p:
  1150. [[-0.000 -0.000 -0.000 34.059 0.000 -0.200]
  1151. [-0.000 -90.747 -0.000 0.000 -0.210 -0.000]
  1152. [-0.000 -0.000 -0.000 11.355 0.000 0.100]
  1153. [-0.000 -16.469 -0.000 0.000 -0.432 -0.000]
  1154. [ 0.000 0.000 0.000 -11.349 -0.000 0.400]
  1155. [ 0.000 74.279 0.000 0.000 0.778 -0.000]
  1156. [ 0.000 0.000 0.000 -34.052 -0.000 0.700]]
  1157. [rosout][DEBUG] 2018-03-19 23:28:52,624: ----------------------------------------------
  1158. [rosout][DEBUG] 2018-03-19 23:28:52,625:
  1159.  
  1160. J . J_p:
  1161. [[ 0.000 0.000 0.000 -0.000 0.000 0.000]
  1162. [ 0.000 1.000 0.000 0.000 0.000 -0.000]
  1163. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  1164. [-0.000 0.000 -0.000 1.000 0.000 0.000]
  1165. [ 0.000 -0.000 0.000 0.000 1.000 -0.000]
  1166. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  1167. [rosout][DEBUG] 2018-03-19 23:28:52,627: ----------------------------------------------
  1168. [rosout][DEBUG] 2018-03-19 23:28:52,629:
  1169.  
  1170. dq:
  1171. [-0.000 0.104 -0.000 0.214 0.000 -0.386 0.000]
  1172. [rosout][DEBUG] 2018-03-19 23:28:52,633: ----------------------------------------------
  1173. [rosout][DEBUG] 2018-03-19 23:28:52,634:
  1174.  
  1175. dq:
  1176. [-0.000 0.104 -0.000 0.214 0.000 -0.386 0.000]
  1177. [rosout][DEBUG] 2018-03-19 23:28:52,635: ----------------------------------------------
  1178.  
  1179.  
  1180.  
  1181.  
  1182.  
  1183.  
  1184.  
  1185.  
  1186.  
  1187.  
  1188.  
  1189.  
  1190.  
  1191.  
  1192.  
  1193.  
  1194.  
  1195.  
  1196.  
  1197.  
  1198.  
  1199. [rosout][INFO] 2018-03-19 23:28:52,652: =============== cartesian_control() ===============
  1200. [rosout][DEBUG] 2018-03-19 23:28:52,652:
  1201.  
  1202. joint_transforms:
  1203. [rosout][DEBUG] 2018-03-19 23:28:52,653:
  1204.  
  1205. joint_transforms[0]:
  1206. [[ 1.000 0.000 0.000 0.000]
  1207. [ 0.000 1.000 0.000 0.000]
  1208. [ 0.000 0.000 1.000 0.310]
  1209. [ 0.000 0.000 0.000 1.000]]
  1210. [rosout][DEBUG] 2018-03-19 23:28:52,655:
  1211.  
  1212. joint_transforms[1]:
  1213. [[ 1.000 0.000 0.000 0.000]
  1214. [-0.000 0.000 1.000 0.000]
  1215. [ 0.000 -1.000 0.000 0.510]
  1216. [ 0.000 0.000 0.000 1.000]]
  1217. [rosout][DEBUG] 2018-03-19 23:28:52,656:
  1218.  
  1219. joint_transforms[2]:
  1220. [[ 1.000 0.000 -0.021 -0.004]
  1221. [-0.000 1.000 0.000 0.000]
  1222. [ 0.021 0.000 1.000 0.710]
  1223. [ 0.000 0.000 0.000 1.000]]
  1224. [rosout][DEBUG] 2018-03-19 23:28:52,658:
  1225.  
  1226. joint_transforms[3]:
  1227. [[ 1.000 -0.021 -0.000 -0.008]
  1228. [-0.000 0.000 -1.000 0.000]
  1229. [ 0.021 1.000 -0.000 0.910]
  1230. [ 0.000 0.000 0.000 1.000]]
  1231. [rosout][DEBUG] 2018-03-19 23:28:52,660:
  1232.  
  1233. joint_transforms[4]:
  1234. [[ 0.999 0.000 -0.042 -0.017]
  1235. [-0.000 1.000 0.000 0.000]
  1236. [ 0.042 0.000 0.999 1.110]
  1237. [ 0.000 0.000 0.000 1.000]]
  1238. [rosout][DEBUG] 2018-03-19 23:28:52,662:
  1239.  
  1240. joint_transforms[5]:
  1241. [[ 0.999 0.042 0.000 -0.025]
  1242. [-0.000 -0.000 1.000 0.000]
  1243. [ 0.042 -0.999 0.000 1.300]
  1244. [ 0.000 0.000 0.000 1.000]]
  1245. [rosout][DEBUG] 2018-03-19 23:28:52,664:
  1246.  
  1247. joint_transforms[6]:
  1248. [[ 0.998 0.000 -0.063 -0.030]
  1249. [-0.000 1.000 0.000 0.000]
  1250. [ 0.063 0.000 0.998 1.378]
  1251. [ 0.000 0.000 0.000 1.000]]
  1252. [rosout][DEBUG] 2018-03-19 23:28:52,665: ----------------------------------------------
  1253. [rosout][DEBUG] 2018-03-19 23:28:52,666:
  1254.  
  1255. b_T_ee_current:
  1256. [[ 0.998 0.000 -0.063 -0.030]
  1257. [-0.000 1.000 0.000 0.000]
  1258. [ 0.063 0.000 0.998 1.378]
  1259. [ 0.000 0.000 0.000 1.000]]
  1260. [rosout][DEBUG] 2018-03-19 23:28:52,668: ----------------------------------------------
  1261. [rosout][DEBUG] 2018-03-19 23:28:52,669:
  1262.  
  1263. b_T_ee_desired:
  1264. [[ 0.866 0.000 -0.500 0.000]
  1265. [ 0.000 1.000 0.000 0.000]
  1266. [ 0.500 -0.000 0.866 1.378]
  1267. [ 0.000 0.000 0.000 1.000]]
  1268. [rosout][DEBUG] 2018-03-19 23:28:52,672: ----------------------------------------------
  1269. [rosout][DEBUG] 2018-03-19 23:28:52,673:
  1270.  
  1271. red_control:
  1272. False
  1273. [rosout][DEBUG] 2018-03-19 23:28:52,675: ----------------------------------------------
  1274. [rosout][DEBUG] 2018-03-19 23:28:52,676:
  1275.  
  1276. q_current:
  1277. (-4.541408397491353e-12, -0.02109041882247827, -4.541410425295772e-12, 0.02109041882247827, -4.541410425295772e-12, -0.021090418822478262, -4.541410425295772e-12)
  1278. [rosout][DEBUG] 2018-03-19 23:28:52,678: ----------------------------------------------
  1279. [rosout][DEBUG] 2018-03-19 23:28:52,679:
  1280.  
  1281. q0_desired:
  1282. 0
  1283. [rosout][DEBUG] 2018-03-19 23:28:52,680: ----------------------------------------------
  1284. [rosout][DEBUG] 2018-03-19 23:28:52,681:
  1285.  
  1286. b_p_current:
  1287. [-0.030 0.000 1.378 1.000]
  1288. [rosout][DEBUG] 2018-03-19 23:28:52,682: ----------------------------------------------
  1289. [rosout][DEBUG] 2018-03-19 23:28:52,683:
  1290.  
  1291. b_p_desired:
  1292. [ 0.000 0.000 1.378 1.000]
  1293. [rosout][DEBUG] 2018-03-19 23:28:52,684: ----------------------------------------------
  1294. [rosout][DEBUG] 2018-03-19 23:28:52,685:
  1295.  
  1296. x_current:
  1297. [-0.030 0.000 1.378 0.000 -0.063 -0.000]
  1298. [rosout][DEBUG] 2018-03-19 23:28:52,686: ----------------------------------------------
  1299. [rosout][DEBUG] 2018-03-19 23:28:52,687:
  1300.  
  1301. x_desired:
  1302. [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
  1303. [rosout][DEBUG] 2018-03-19 23:28:52,688: ----------------------------------------------
  1304. [rosout][DEBUG] 2018-03-19 23:28:52,689:
  1305.  
  1306. b_dX:
  1307. [ 0.030 -0.000 0.001 -0.000 -0.460 0.000]
  1308. [rosout][DEBUG] 2018-03-19 23:28:52,690: ----------------------------------------------
  1309. [rosout][DEBUG] 2018-03-19 23:28:52,691:
  1310.  
  1311. b_X_dot:
  1312. [ 0.015 -0.000 0.000 -0.000 -0.230 0.000]
  1313. [rosout][DEBUG] 2018-03-19 23:28:52,693: ----------------------------------------------
  1314. [rosout][DEBUG] 2018-03-19 23:28:52,695:
  1315.  
  1316. V:
  1317. [[ 0.998 -0.000 0.063 0.000 0.000 0.000]
  1318. [ 0.000 1.000 0.000 0.000 0.000 0.000]
  1319. [-0.063 0.000 0.998 0.000 0.000 0.000]
  1320. [ 0.000 0.000 0.000 0.998 -0.000 0.063]
  1321. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1322. [ 0.000 0.000 0.000 -0.063 0.000 0.998]]
  1323. [rosout][DEBUG] 2018-03-19 23:28:52,696: ----------------------------------------------
  1324. [rosout][DEBUG] 2018-03-19 23:28:52,697:
  1325.  
  1326. V_ee:
  1327. [ 0.015 0.000 -0.001 -0.000 -0.230 0.000]
  1328. [rosout][DEBUG] 2018-03-19 23:28:52,698: ----------------------------------------------
  1329. [rosout][DEBUG] 2018-03-19 23:28:52,700: ++++++++++++++++++++++++++++++++++++++++++++++
  1330. [rosout][DEBUG] 2018-03-19 23:28:52,701:
  1331.  
  1332. ee_T_ee_desired:
  1333. [[ 0.896 0.000 -0.444 0.030]
  1334. [ 0.000 1.000 0.000 0.000]
  1335. [ 0.444 -0.000 0.896 -0.001]
  1336. [ 0.000 0.000 0.000 1.000]]
  1337. [rosout][DEBUG] 2018-03-19 23:28:52,704: ----------------------------------------------
  1338. [rosout][DEBUG] 2018-03-19 23:28:52,707:
  1339.  
  1340. delta_X:
  1341. [ 0.015 0.000 -0.001]
  1342. [rosout][DEBUG] 2018-03-19 23:28:52,708: ----------------------------------------------
  1343. [rosout][DEBUG] 2018-03-19 23:28:52,709:
  1344.  
  1345. delta_X:
  1346. [ 0.015 0.000 -0.001]
  1347. [rosout][DEBUG] 2018-03-19 23:28:52,712: ----------------------------------------------
  1348. [rosout][DEBUG] 2018-03-19 23:28:52,713:
  1349.  
  1350. delta_W:
  1351. [-0.000 -0.460 0.000]
  1352. [rosout][DEBUG] 2018-03-19 23:28:52,714: ----------------------------------------------
  1353. [rosout][DEBUG] 2018-03-19 23:28:52,715:
  1354.  
  1355. delta_W:
  1356. [-0.000 -0.460 0.000]
  1357. [rosout][DEBUG] 2018-03-19 23:28:52,716: ----------------------------------------------
  1358. [rosout][DEBUG] 2018-03-19 23:28:52,718:
  1359.  
  1360. V_ee:
  1361. [ 0.015 0.000 -0.001 -0.000 -0.460 0.000]
  1362. [rosout][DEBUG] 2018-03-19 23:28:52,719: ----------------------------------------------
  1363. [rosout][DEBUG] 2018-03-19 23:28:52,720: ----------------------------------------------
  1364. [rosout][DEBUG] 2018-03-19 23:28:52,721: ----------------------------------------------
  1365. [rosout][DEBUG] 2018-03-19 23:28:52,727:
  1366.  
  1367. j_T_ee:
  1368. [rosout][DEBUG] 2018-03-19 23:28:52,728:
  1369.  
  1370. j_T_ee[0]:
  1371. [[ 0.998 0.000 -0.063 -0.030]
  1372. [-0.000 1.000 0.000 0.000]
  1373. [ 0.063 0.000 0.998 1.068]
  1374. [ 0.000 0.000 0.000 1.000]]
  1375. [rosout][DEBUG] 2018-03-19 23:28:52,730:
  1376.  
  1377. j_T_ee[1]:
  1378. [[ 0.998 0.000 -0.063 -0.030]
  1379. [-0.063 -0.000 -0.998 -0.867]
  1380. [-0.000 1.000 0.000 0.000]
  1381. [ 0.000 0.000 0.000 1.000]]
  1382. [rosout][DEBUG] 2018-03-19 23:28:52,731:
  1383.  
  1384. j_T_ee[2]:
  1385. [[ 0.999 0.000 -0.042 -0.012]
  1386. [-0.000 1.000 0.000 0.000]
  1387. [ 0.042 0.000 0.999 0.668]
  1388. [ 0.000 0.000 0.000 1.000]]
  1389. [rosout][DEBUG] 2018-03-19 23:28:52,732:
  1390.  
  1391. j_T_ee[3]:
  1392. [[ 0.999 0.000 -0.042 -0.012]
  1393. [ 0.042 0.000 0.999 0.468]
  1394. [ 0.000 -1.000 -0.000 -0.000]
  1395. [ 0.000 0.000 0.000 1.000]]
  1396. [rosout][DEBUG] 2018-03-19 23:28:52,735:
  1397.  
  1398. j_T_ee[4]:
  1399. [[ 1.000 0.000 -0.021 -0.002]
  1400. [-0.000 1.000 0.000 0.000]
  1401. [ 0.021 0.000 1.000 0.268]
  1402. [ 0.000 0.000 0.000 1.000]]
  1403. [rosout][DEBUG] 2018-03-19 23:28:52,738:
  1404.  
  1405. j_T_ee[5]:
  1406. [[ 1.000 0.000 -0.021 -0.002]
  1407. [-0.021 -0.000 -1.000 -0.078]
  1408. [-0.000 1.000 0.000 0.000]
  1409. [ 0.000 0.000 0.000 1.000]]
  1410. [rosout][DEBUG] 2018-03-19 23:28:52,742:
  1411.  
  1412. j_T_ee[6]:
  1413. [[ 1.000 0.000 0.000 0.000]
  1414. [-0.000 1.000 -0.000 0.000]
  1415. [ 0.000 -0.000 1.000 0.000]
  1416. [ 0.000 0.000 0.000 1.000]]
  1417. [rosout][DEBUG] 2018-03-19 23:28:52,743: ----------------------------------------------
  1418. [rosout][DEBUG] 2018-03-19 23:28:52,746:
  1419.  
  1420. ee_T_j:
  1421. [rosout][DEBUG] 2018-03-19 23:28:52,747:
  1422.  
  1423. ee_T_j[0]:
  1424. [[ 0.998 -0.000 0.063 -0.038]
  1425. [ 0.000 1.000 0.000 -0.000]
  1426. [-0.063 0.000 0.998 -1.068]
  1427. [ 0.000 0.000 0.000 1.000]]
  1428. [rosout][DEBUG] 2018-03-19 23:28:52,749:
  1429.  
  1430. ee_T_j[1]:
  1431. [[ 0.998 -0.063 -0.000 -0.025]
  1432. [ 0.000 -0.000 1.000 -0.000]
  1433. [-0.063 -0.998 0.000 -0.868]
  1434. [ 0.000 0.000 0.000 1.000]]
  1435. [rosout][DEBUG] 2018-03-19 23:28:52,750:
  1436.  
  1437. ee_T_j[2]:
  1438. [[ 0.999 -0.000 0.042 -0.017]
  1439. [ 0.000 1.000 0.000 -0.000]
  1440. [-0.042 0.000 0.999 -0.668]
  1441. [ 0.000 0.000 0.000 1.000]]
  1442. [rosout][DEBUG] 2018-03-19 23:28:52,752:
  1443.  
  1444. ee_T_j[3]:
  1445. [[ 0.999 0.042 0.000 -0.008]
  1446. [ 0.000 0.000 -1.000 -0.000]
  1447. [-0.042 0.999 -0.000 -0.468]
  1448. [ 0.000 0.000 0.000 1.000]]
  1449. [rosout][DEBUG] 2018-03-19 23:28:52,753:
  1450.  
  1451. ee_T_j[4]:
  1452. [[ 1.000 -0.000 0.021 -0.004]
  1453. [ 0.000 1.000 0.000 -0.000]
  1454. [-0.021 0.000 1.000 -0.268]
  1455. [ 0.000 0.000 0.000 1.000]]
  1456. [rosout][DEBUG] 2018-03-19 23:28:52,756:
  1457.  
  1458. ee_T_j[5]:
  1459. [[ 1.000 -0.021 -0.000 0.000]
  1460. [ 0.000 -0.000 1.000 0.000]
  1461. [-0.021 -1.000 0.000 -0.078]
  1462. [ 0.000 0.000 0.000 1.000]]
  1463. [rosout][DEBUG] 2018-03-19 23:28:52,757:
  1464.  
  1465. ee_T_j[6]:
  1466. [[ 1.000 -0.000 -0.000 -0.000]
  1467. [ 0.000 1.000 0.000 -0.000]
  1468. [-0.000 0.000 1.000 0.000]
  1469. [ 0.000 0.000 0.000 1.000]]
  1470. [rosout][DEBUG] 2018-03-19 23:28:52,759: ----------------------------------------------
  1471. [rosout][DEBUG] 2018-03-19 23:28:52,761:
  1472.  
  1473. V_j:
  1474. [rosout][DEBUG] 2018-03-19 23:28:52,762:
  1475.  
  1476. V_j[0]:
  1477. [[ 0.998 -0.000 0.063 -0.000 -0.000 -0.000]
  1478. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1479. [-0.063 0.000 0.998 -0.000 0.000 -0.000]
  1480. [ 0.000 0.000 0.000 0.998 -0.000 0.063]
  1481. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1482. [ 0.000 0.000 0.000 -0.063 0.000 0.998]]
  1483. [rosout][DEBUG] 2018-03-19 23:28:52,764:
  1484.  
  1485. V_j[1]:
  1486. [[ 0.998 -0.063 -0.000 -0.000 -0.000 -0.000]
  1487. [ 0.000 -0.000 1.000 -0.000 0.000 -0.030]
  1488. [-0.063 -0.998 0.000 0.055 -0.030 -0.000]
  1489. [ 0.000 0.000 0.000 0.998 -0.063 -0.000]
  1490. [ 0.000 0.000 0.000 0.000 -0.000 1.000]
  1491. [ 0.000 0.000 0.000 -0.063 -0.998 0.000]]
  1492. [rosout][DEBUG] 2018-03-19 23:28:52,766:
  1493.  
  1494. V_j[2]:
  1495. [[ 0.999 -0.000 0.042 -0.000 -0.000 -0.000]
  1496. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1497. [-0.042 0.000 0.999 -0.000 0.000 -0.000]
  1498. [ 0.000 0.000 0.000 0.999 -0.000 0.042]
  1499. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1500. [ 0.000 0.000 0.000 -0.042 0.000 0.999]]
  1501. [rosout][DEBUG] 2018-03-19 23:28:52,773:
  1502.  
  1503. V_j[3]:
  1504. [[ 0.999 0.042 0.000 -0.000 -0.000 -0.000]
  1505. [ 0.000 0.000 -1.000 0.000 -0.000 0.012]
  1506. [-0.042 0.999 -0.000 -0.020 0.012 0.000]
  1507. [ 0.000 0.000 0.000 0.999 0.042 0.000]
  1508. [ 0.000 0.000 0.000 0.000 0.000 -1.000]
  1509. [ 0.000 0.000 0.000 -0.042 0.999 -0.000]]
  1510. [rosout][DEBUG] 2018-03-19 23:28:52,776:
  1511.  
  1512. V_j[4]:
  1513. [[ 1.000 -0.000 0.021 -0.000 -0.000 -0.000]
  1514. [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
  1515. [-0.021 0.000 1.000 -0.000 0.000 -0.000]
  1516. [ 0.000 0.000 0.000 1.000 -0.000 0.021]
  1517. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1518. [ 0.000 0.000 0.000 -0.021 0.000 1.000]]
  1519. [rosout][DEBUG] 2018-03-19 23:28:52,778:
  1520.  
  1521. V_j[5]:
  1522. [[ 1.000 -0.021 -0.000 -0.000 -0.000 -0.000]
  1523. [ 0.000 -0.000 1.000 -0.000 0.000 -0.002]
  1524. [-0.021 -1.000 0.000 0.002 -0.002 -0.000]
  1525. [ 0.000 0.000 0.000 1.000 -0.021 -0.000]
  1526. [ 0.000 0.000 0.000 0.000 -0.000 1.000]
  1527. [ 0.000 0.000 0.000 -0.021 -1.000 0.000]]
  1528. [rosout][DEBUG] 2018-03-19 23:28:52,781:
  1529.  
  1530. V_j[6]:
  1531. [[ 1.000 -0.000 -0.000 -0.000 -0.000 0.000]
  1532. [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
  1533. [-0.000 0.000 1.000 -0.000 -0.000 -0.000]
  1534. [ 0.000 0.000 0.000 1.000 -0.000 -0.000]
  1535. [ 0.000 0.000 0.000 0.000 1.000 0.000]
  1536. [ 0.000 0.000 0.000 -0.000 0.000 1.000]]
  1537. [rosout][DEBUG] 2018-03-19 23:28:52,783: ----------------------------------------------
  1538. [rosout][DEBUG] 2018-03-19 23:28:52,784: ----------------------------------------------
  1539. [rosout][DEBUG] 2018-03-19 23:28:52,785: ----------------------------------------------
  1540. [rosout][DEBUG] 2018-03-19 23:28:52,787:
  1541.  
  1542. J:
  1543. [[-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 0.000]
  1544. [-0.000 -0.030 -0.000 0.012 -0.000 -0.002 0.000]
  1545. [-0.000 -0.000 -0.000 0.000 -0.000 -0.000 -0.000]
  1546. [ 0.063 -0.000 0.042 0.000 0.021 -0.000 -0.000]
  1547. [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
  1548. [ 0.998 0.000 0.999 -0.000 1.000 0.000 1.000]]
  1549. [rosout][DEBUG] 2018-03-19 23:28:52,790: ----------------------------------------------
  1550. [rosout][DEBUG] 2018-03-19 23:28:52,791:
  1551.  
  1552. J_p:
  1553. [[-0.000 -0.000 -0.000 14.227 0.000 -0.200]
  1554. [-0.000 -37.911 0.000 0.000 -0.210 -0.000]
  1555. [-0.000 -0.000 -0.000 4.748 0.000 0.100]
  1556. [-0.000 -6.878 0.000 0.000 -0.432 -0.000]
  1557. [ 0.000 0.000 0.000 -4.733 -0.000 0.400]
  1558. [ 0.000 31.033 -0.000 0.000 0.778 -0.000]
  1559. [ 0.000 0.000 0.000 -14.211 -0.000 0.700]]
  1560. [rosout][DEBUG] 2018-03-19 23:28:52,793: ----------------------------------------------
  1561. [rosout][DEBUG] 2018-03-19 23:28:52,794:
  1562.  
  1563. J . J_p:
  1564. [[ 0.000 0.000 -0.000 -0.000 0.000 0.000]
  1565. [ 0.000 1.000 -0.000 0.000 0.000 -0.000]
  1566. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  1567. [-0.000 -0.000 -0.000 1.000 -0.000 -0.000]
  1568. [ 0.000 -0.000 -0.000 -0.000 1.000 0.000]
  1569. [ 0.000 -0.000 0.000 -0.000 0.000 1.000]]
  1570. [rosout][DEBUG] 2018-03-19 23:28:52,795: ----------------------------------------------
  1571. [rosout][DEBUG] 2018-03-19 23:28:52,796:
  1572.  
  1573. dq:
  1574. [-0.000 0.096 -0.000 0.199 0.000 -0.358 0.000]
  1575. [rosout][DEBUG] 2018-03-19 23:28:52,800: ----------------------------------------------
  1576. [rosout][DEBUG] 2018-03-19 23:28:52,803:
  1577.  
  1578. dq:
  1579. [-0.000 0.096 -0.000 0.199 0.000 -0.358 0.000]
  1580. [rosout][DEBUG] 2018-03-19 23:28:52,806: ----------------------------------------------
  1581.  
  1582.  
  1583.  
  1584.  
  1585.  
  1586.  
  1587.  
  1588.  
  1589.  
  1590.  
  1591.  
  1592.  
  1593.  
  1594.  
  1595.  
  1596.  
  1597.  
  1598.  
  1599. [rosout][INFO] 2018-03-19 23:28:53,307: =============== cartesian_control() ===============
  1600. [rosout][DEBUG] 2018-03-19 23:28:53,309:
  1601.  
  1602. joint_transforms:
  1603. [rosout][DEBUG] 2018-03-19 23:28:53,309:
  1604.  
  1605. joint_transforms[0]:
  1606. [[ 1.000 0.000 0.000 0.000]
  1607. [ 0.000 1.000 0.000 0.000]
  1608. [ 0.000 0.000 1.000 0.310]
  1609. [ 0.000 0.000 0.000 1.000]]
  1610. [rosout][DEBUG] 2018-03-19 23:28:53,310:
  1611.  
  1612. joint_transforms[1]:
  1613. [[ 1.000 0.000 0.000 0.000]
  1614. [-0.000 0.000 1.000 0.000]
  1615. [ 0.000 -1.000 0.000 0.510]
  1616. [ 0.000 0.000 0.000 1.000]]
  1617. [rosout][DEBUG] 2018-03-19 23:28:53,310:
  1618.  
  1619. joint_transforms[2]:
  1620. [[ 1.000 0.000 -0.022 -0.004]
  1621. [-0.000 1.000 0.000 0.000]
  1622. [ 0.022 0.000 1.000 0.710]
  1623. [ 0.000 0.000 0.000 1.000]]
  1624. [rosout][DEBUG] 2018-03-19 23:28:53,311:
  1625.  
  1626. joint_transforms[3]:
  1627. [[ 1.000 -0.022 -0.000 -0.009]
  1628. [-0.000 0.000 -1.000 0.000]
  1629. [ 0.022 1.000 -0.000 0.910]
  1630. [ 0.000 0.000 0.000 1.000]]
  1631. [rosout][DEBUG] 2018-03-19 23:28:53,312:
  1632.  
  1633. joint_transforms[4]:
  1634. [[ 0.993 0.000 -0.119 -0.033]
  1635. [-0.000 1.000 0.000 0.000]
  1636. [ 0.119 0.000 0.993 1.109]
  1637. [ 0.000 0.000 0.000 1.000]]
  1638. [rosout][DEBUG] 2018-03-19 23:28:53,312:
  1639.  
  1640. joint_transforms[5]:
  1641. [[ 0.993 0.119 0.000 -0.055]
  1642. [-0.000 -0.000 1.000 0.000]
  1643. [ 0.119 -0.993 -0.000 1.298]
  1644. [ 0.000 0.000 0.000 1.000]]
  1645. [rosout][DEBUG] 2018-03-19 23:28:53,313:
  1646.  
  1647. joint_transforms[6]:
  1648. [[ 0.967 0.000 -0.255 -0.075]
  1649. [-0.000 1.000 0.000 0.000]
  1650. [ 0.255 -0.000 0.967 1.373]
  1651. [ 0.000 0.000 0.000 1.000]]
  1652. [rosout][DEBUG] 2018-03-19 23:28:53,313: ----------------------------------------------
  1653. [rosout][DEBUG] 2018-03-19 23:28:53,314:
  1654.  
  1655. b_T_ee_current:
  1656. [[ 0.967 0.000 -0.255 -0.075]
  1657. [ 0.000 1.000 0.000 0.000]
  1658. [ 0.255 -0.000 0.967 1.373]
  1659. [ 0.000 0.000 0.000 1.000]]
  1660. [rosout][DEBUG] 2018-03-19 23:28:53,314: ----------------------------------------------
  1661. [rosout][DEBUG] 2018-03-19 23:28:53,316:
  1662.  
  1663. b_T_ee_desired:
  1664. [[ 0.866 0.000 -0.500 0.000]
  1665. [ 0.000 1.000 0.000 0.000]
  1666. [ 0.500 -0.000 0.866 1.378]
  1667. [ 0.000 0.000 0.000 1.000]]
  1668. [rosout][DEBUG] 2018-03-19 23:28:53,317: ----------------------------------------------
  1669. [rosout][DEBUG] 2018-03-19 23:28:53,318:
  1670.  
  1671. red_control:
  1672. False
  1673. [rosout][DEBUG] 2018-03-19 23:28:53,318: ----------------------------------------------
  1674. [rosout][DEBUG] 2018-03-19 23:28:53,319:
  1675.  
  1676. q_current:
  1677. (-2.1248224469441799e-07, -0.021798680042333853, -7.084187827909742e-08, 0.09774790323418787, 7.080791103921825e-08, -0.13870158210822248, 2.1244829577527386e-07)
  1678. [rosout][DEBUG] 2018-03-19 23:28:53,320: ----------------------------------------------
  1679. [rosout][DEBUG] 2018-03-19 23:28:53,320:
  1680.  
  1681. q0_desired:
  1682. 0
  1683. [rosout][DEBUG] 2018-03-19 23:28:53,320: ----------------------------------------------
  1684. [rosout][DEBUG] 2018-03-19 23:28:53,321:
  1685.  
  1686. b_p_current:
  1687. [-0.075 0.000 1.373 1.000]
  1688. [rosout][DEBUG] 2018-03-19 23:28:53,321: ----------------------------------------------
  1689. [rosout][DEBUG] 2018-03-19 23:28:53,322:
  1690.  
  1691. b_p_desired:
  1692. [ 0.000 0.000 1.378 1.000]
  1693. [rosout][DEBUG] 2018-03-19 23:28:53,322: ----------------------------------------------
  1694. [rosout][DEBUG] 2018-03-19 23:28:53,323:
  1695.  
  1696. x_current:
  1697. [-0.075 0.000 1.373 -0.000 -0.258 0.000]
  1698. [rosout][DEBUG] 2018-03-19 23:28:53,323: ----------------------------------------------
  1699. [rosout][DEBUG] 2018-03-19 23:28:53,325:
  1700.  
  1701. x_desired:
  1702. [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
  1703. [rosout][DEBUG] 2018-03-19 23:28:53,325: ----------------------------------------------
  1704. [rosout][DEBUG] 2018-03-19 23:28:53,326:
  1705.  
  1706. b_dX:
  1707. [ 0.075 -0.000 0.005 0.000 -0.265 0.000]
  1708. [rosout][DEBUG] 2018-03-19 23:28:53,328: ----------------------------------------------
  1709. [rosout][DEBUG] 2018-03-19 23:28:53,328:
  1710.  
  1711. b_X_dot:
  1712. [ 0.038 -0.000 0.003 0.000 -0.132 0.000]
  1713. [rosout][DEBUG] 2018-03-19 23:28:53,329: ----------------------------------------------
  1714. [rosout][DEBUG] 2018-03-19 23:28:53,330:
  1715.  
  1716. V:
  1717. [[ 0.967 0.000 0.255 0.000 0.000 0.000]
  1718. [ 0.000 1.000 -0.000 0.000 0.000 0.000]
  1719. [-0.255 0.000 0.967 0.000 0.000 0.000]
  1720. [ 0.000 0.000 0.000 0.967 0.000 0.255]
  1721. [ 0.000 0.000 0.000 0.000 1.000 -0.000]
  1722. [ 0.000 0.000 0.000 -0.255 0.000 0.967]]
  1723. [rosout][DEBUG] 2018-03-19 23:28:53,331: ----------------------------------------------
  1724. [rosout][DEBUG] 2018-03-19 23:28:53,333:
  1725.  
  1726. V_ee:
  1727. [ 0.037 -0.000 -0.007 0.000 -0.132 -0.000]
  1728. [rosout][DEBUG] 2018-03-19 23:28:53,334: ----------------------------------------------
  1729. [rosout][DEBUG] 2018-03-19 23:28:53,334: ++++++++++++++++++++++++++++++++++++++++++++++
  1730. [rosout][DEBUG] 2018-03-19 23:28:53,337:
  1731.  
  1732. ee_T_ee_desired:
  1733. [[ 0.965 0.000 -0.262 0.074]
  1734. [-0.000 1.000 -0.000 -0.000]
  1735. [ 0.262 0.000 0.965 -0.014]
  1736. [ 0.000 0.000 0.000 1.000]]
  1737. [rosout][DEBUG] 2018-03-19 23:28:53,337: ----------------------------------------------
  1738. [rosout][DEBUG] 2018-03-19 23:28:53,338:
  1739.  
  1740. delta_X:
  1741. [ 0.037 -0.000 -0.007]
  1742. [rosout][DEBUG] 2018-03-19 23:28:53,338: ----------------------------------------------
  1743. [rosout][DEBUG] 2018-03-19 23:28:53,339:
  1744.  
  1745. delta_X:
  1746. [ 0.037 -0.000 -0.007]
  1747. [rosout][DEBUG] 2018-03-19 23:28:53,339: ----------------------------------------------
  1748. [rosout][DEBUG] 2018-03-19 23:28:53,340:
  1749.  
  1750. delta_W:
  1751. [ 0.000 -0.265 -0.000]
  1752. [rosout][DEBUG] 2018-03-19 23:28:53,340: ----------------------------------------------
  1753. [rosout][DEBUG] 2018-03-19 23:28:53,341:
  1754.  
  1755. delta_W:
  1756. [ 0.000 -0.265 -0.000]
  1757. [rosout][DEBUG] 2018-03-19 23:28:53,342: ----------------------------------------------
  1758. [rosout][DEBUG] 2018-03-19 23:28:53,343:
  1759.  
  1760. V_ee:
  1761. [ 0.037 -0.000 -0.007 0.000 -0.265 -0.000]
  1762. [rosout][DEBUG] 2018-03-19 23:28:53,343: ----------------------------------------------
  1763. [rosout][DEBUG] 2018-03-19 23:28:53,344: ----------------------------------------------
  1764. [rosout][DEBUG] 2018-03-19 23:28:53,344: ----------------------------------------------
  1765. [rosout][DEBUG] 2018-03-19 23:28:53,353:
  1766.  
  1767. j_T_ee:
  1768. [rosout][DEBUG] 2018-03-19 23:28:53,354:
  1769.  
  1770. j_T_ee[0]:
  1771. [[ 0.967 0.000 -0.255 -0.075]
  1772. [ 0.000 1.000 0.000 0.000]
  1773. [ 0.255 -0.000 0.967 1.063]
  1774. [ 0.000 0.000 0.000 1.000]]
  1775. [rosout][DEBUG] 2018-03-19 23:28:53,355:
  1776.  
  1777. j_T_ee[1]:
  1778. [[ 0.967 -0.000 -0.255 -0.075]
  1779. [-0.255 0.000 -0.967 -0.863]
  1780. [ 0.000 1.000 0.000 0.000]
  1781. [ 0.000 0.000 0.000 1.000]]
  1782. [rosout][DEBUG] 2018-03-19 23:28:53,355:
  1783.  
  1784. j_T_ee[2]:
  1785. [[ 0.972 -0.000 -0.234 -0.056]
  1786. [ 0.000 1.000 0.000 0.000]
  1787. [ 0.234 -0.000 0.972 0.664]
  1788. [ 0.000 0.000 0.000 1.000]]
  1789. [rosout][DEBUG] 2018-03-19 23:28:53,356:
  1790.  
  1791. j_T_ee[3]:
  1792. [[ 0.972 -0.000 -0.234 -0.056]
  1793. [ 0.234 -0.000 0.972 0.464]
  1794. [-0.000 -1.000 0.000 0.000]
  1795. [ 0.000 0.000 0.000 1.000]]
  1796. [rosout][DEBUG] 2018-03-19 23:28:53,357:
  1797.  
  1798. j_T_ee[4]:
  1799. [[ 0.990 -0.000 -0.138 -0.011]
  1800. [ 0.000 1.000 -0.000 -0.000]
  1801. [ 0.138 -0.000 0.990 0.267]
  1802. [ 0.000 0.000 0.000 1.000]]
  1803. [rosout][DEBUG] 2018-03-19 23:28:53,358:
  1804.  
  1805. j_T_ee[5]:
  1806. [[ 0.990 -0.000 -0.138 -0.011]
  1807. [-0.138 0.000 -0.990 -0.077]
  1808. [ 0.000 1.000 0.000 0.000]
  1809. [ 0.000 0.000 0.000 1.000]]
  1810. [rosout][DEBUG] 2018-03-19 23:28:53,359:
  1811.  
  1812. j_T_ee[6]:
  1813. [[ 1.000 -0.000 0.000 0.000]
  1814. [ 0.000 1.000 0.000 0.000]
  1815. [-0.000 -0.000 1.000 0.000]
  1816. [ 0.000 0.000 0.000 1.000]]
  1817. [rosout][DEBUG] 2018-03-19 23:28:53,359: ----------------------------------------------
  1818. [rosout][DEBUG] 2018-03-19 23:28:53,367:
  1819.  
  1820. ee_T_j:
  1821. [rosout][DEBUG] 2018-03-19 23:28:53,368:
  1822.  
  1823. ee_T_j[0]:
  1824. [[ 0.967 0.000 0.255 -0.199]
  1825. [ 0.000 1.000 -0.000 0.000]
  1826. [-0.255 0.000 0.967 -1.047]
  1827. [ 0.000 0.000 0.000 1.000]]
  1828. [rosout][DEBUG] 2018-03-19 23:28:53,368:
  1829.  
  1830. ee_T_j[1]:
  1831. [[ 0.967 -0.255 0.000 -0.148]
  1832. [-0.000 0.000 1.000 0.000]
  1833. [-0.255 -0.967 0.000 -0.853]
  1834. [ 0.000 0.000 0.000 1.000]]
  1835. [rosout][DEBUG] 2018-03-19 23:28:53,369:
  1836.  
  1837. ee_T_j[2]:
  1838. [[ 0.972 0.000 0.234 -0.101]
  1839. [-0.000 1.000 -0.000 0.000]
  1840. [-0.234 0.000 0.972 -0.659]
  1841. [ 0.000 0.000 0.000 1.000]]
  1842. [rosout][DEBUG] 2018-03-19 23:28:53,370:
  1843.  
  1844. ee_T_j[3]:
  1845. [[ 0.972 0.234 -0.000 -0.054]
  1846. [-0.000 -0.000 -1.000 0.000]
  1847. [-0.234 0.972 0.000 -0.464]
  1848. [ 0.000 0.000 0.000 1.000]]
  1849. [rosout][DEBUG] 2018-03-19 23:28:53,370:
  1850.  
  1851. ee_T_j[4]:
  1852. [[ 0.990 0.000 0.138 -0.026]
  1853. [-0.000 1.000 -0.000 0.000]
  1854. [-0.138 -0.000 0.990 -0.266]
  1855. [ 0.000 0.000 0.000 1.000]]
  1856. [rosout][DEBUG] 2018-03-19 23:28:53,371:
  1857.  
  1858. ee_T_j[5]:
  1859. [[ 0.990 -0.138 0.000 -0.000]
  1860. [-0.000 0.000 1.000 0.000]
  1861. [-0.138 -0.990 0.000 -0.078]
  1862. [ 0.000 0.000 0.000 1.000]]
  1863. [rosout][DEBUG] 2018-03-19 23:28:53,371:
  1864.  
  1865. ee_T_j[6]:
  1866. [[ 1.000 0.000 -0.000 -0.000]
  1867. [-0.000 1.000 -0.000 0.000]
  1868. [ 0.000 0.000 1.000 -0.000]
  1869. [ 0.000 0.000 0.000 1.000]]
  1870. [rosout][DEBUG] 2018-03-19 23:28:53,372: ----------------------------------------------
  1871. [rosout][DEBUG] 2018-03-19 23:28:53,372:
  1872.  
  1873. V_j:
  1874. [rosout][DEBUG] 2018-03-19 23:28:53,373:
  1875.  
  1876. V_j[0]:
  1877. [[ 0.967 0.000 0.255 -0.000 0.000 -0.000]
  1878. [ 0.000 1.000 -0.000 -0.000 -0.000 0.000]
  1879. [-0.255 0.000 0.967 -0.000 0.000 -0.000]
  1880. [ 0.000 0.000 0.000 0.967 0.000 0.255]
  1881. [ 0.000 0.000 0.000 0.000 1.000 -0.000]
  1882. [ 0.000 0.000 0.000 -0.255 0.000 0.967]]
  1883. [rosout][DEBUG] 2018-03-19 23:28:53,374:
  1884.  
  1885. V_j[1]:
  1886. [[ 0.967 -0.255 0.000 -0.000 -0.000 0.000]
  1887. [-0.000 0.000 1.000 0.000 -0.000 -0.075]
  1888. [-0.255 -0.967 0.000 0.220 -0.073 -0.000]
  1889. [ 0.000 0.000 0.000 0.967 -0.255 0.000]
  1890. [ 0.000 0.000 0.000 -0.000 0.000 1.000]
  1891. [ 0.000 0.000 0.000 -0.255 -0.967 0.000]]
  1892. [rosout][DEBUG] 2018-03-19 23:28:53,375:
  1893.  
  1894. V_j[2]:
  1895. [[ 0.972 0.000 0.234 -0.000 0.000 -0.000]
  1896. [-0.000 1.000 -0.000 0.000 -0.000 0.000]
  1897. [-0.234 0.000 0.972 -0.000 0.000 -0.000]
  1898. [ 0.000 0.000 0.000 0.972 0.000 0.234]
  1899. [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
  1900. [ 0.000 0.000 0.000 -0.234 0.000 0.972]]
  1901. [rosout][DEBUG] 2018-03-19 23:28:53,376:
  1902.  
  1903. V_j[3]:
  1904. [[ 0.972 0.234 -0.000 -0.000 0.000 0.000]
  1905. [-0.000 -0.000 -1.000 0.000 0.000 0.056]
  1906. [-0.234 0.972 0.000 -0.109 0.055 -0.000]
  1907. [ 0.000 0.000 0.000 0.972 0.234 -0.000]
  1908. [ 0.000 0.000 0.000 -0.000 -0.000 -1.000]
  1909. [ 0.000 0.000 0.000 -0.234 0.972 0.000]]
  1910. [rosout][DEBUG] 2018-03-19 23:28:53,377:
  1911.  
  1912. V_j[4]:
  1913. [[ 0.990 0.000 0.138 -0.000 0.000 0.000]
  1914. [-0.000 1.000 -0.000 0.000 -0.000 0.000]
  1915. [-0.138 -0.000 0.990 0.000 -0.000 -0.000]
  1916. [ 0.000 0.000 0.000 0.990 0.000 0.138]
  1917. [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
  1918. [ 0.000 0.000 0.000 -0.138 -0.000 0.990]]
  1919. [rosout][DEBUG] 2018-03-19 23:28:53,378:
  1920.  
  1921. V_j[5]:
  1922. [[ 0.990 -0.138 0.000 -0.000 -0.000 0.000]
  1923. [-0.000 0.000 1.000 0.000 -0.000 -0.011]
  1924. [-0.138 -0.990 0.000 0.011 -0.011 -0.000]
  1925. [ 0.000 0.000 0.000 0.990 -0.138 0.000]
  1926. [ 0.000 0.000 0.000 -0.000 0.000 1.000]
  1927. [ 0.000 0.000 0.000 -0.138 -0.990 0.000]]
  1928. [rosout][DEBUG] 2018-03-19 23:28:53,378:
  1929.  
  1930. V_j[6]:
  1931. [[ 1.000 0.000 -0.000 -0.000 0.000 0.000]
  1932. [-0.000 1.000 -0.000 0.000 -0.000 -0.000]
  1933. [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
  1934. [ 0.000 0.000 0.000 1.000 0.000 -0.000]
  1935. [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
  1936. [ 0.000 0.000 0.000 0.000 0.000 1.000]]
  1937. [rosout][DEBUG] 2018-03-19 23:28:53,380: ----------------------------------------------
  1938. [rosout][DEBUG] 2018-03-19 23:28:53,381: ----------------------------------------------
  1939. [rosout][DEBUG] 2018-03-19 23:28:53,382: ----------------------------------------------
  1940. [rosout][DEBUG] 2018-03-19 23:28:53,382:
  1941.  
  1942. J:
  1943. [[-0.000 0.000 -0.000 0.000 0.000 0.000 0.000]
  1944. [ 0.000 -0.075 0.000 0.056 0.000 -0.011 -0.000]
  1945. [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
  1946. [ 0.255 0.000 0.234 -0.000 0.138 0.000 -0.000]
  1947. [-0.000 1.000 -0.000 -1.000 -0.000 1.000 -0.000]
  1948. [ 0.967 0.000 0.972 0.000 0.990 0.000 1.000]]
  1949. [rosout][DEBUG] 2018-03-19 23:28:53,384: ----------------------------------------------
  1950. [rosout][DEBUG] 2018-03-19 23:28:53,384:
  1951.  
  1952. J_p:
  1953. [[-0.000 0.000 -0.000 2.415 -0.000 -0.132]
  1954. [ 0.000 -12.658 0.000 -0.000 -0.267 0.000]
  1955. [-0.000 0.000 -0.000 1.896 -0.000 -0.048]
  1956. [-0.000 4.067 -0.000 -0.000 -0.140 0.000]
  1957. [ 0.000 -0.000 0.000 -0.440 0.000 0.326]
  1958. [-0.000 16.725 -0.000 -0.000 1.126 0.000]
  1959. [ 0.000 -0.000 0.000 -3.742 0.000 0.852]]
  1960. [rosout][DEBUG] 2018-03-19 23:28:53,385: ----------------------------------------------
  1961. [rosout][DEBUG] 2018-03-19 23:28:53,386:
  1962.  
  1963. J . J_p:
  1964. [[ 0.000 -0.000 0.000 -0.000 -0.000 0.000]
  1965. [-0.000 1.000 -0.000 -0.000 -0.000 0.000]
  1966. [ 0.000 0.000 0.000 0.000 0.000 0.000]
  1967. [-0.000 -0.000 -0.000 1.000 -0.000 -0.000]
  1968. [-0.000 -0.000 -0.000 -0.000 1.000 0.000]
  1969. [ 0.000 0.000 0.000 0.000 -0.000 1.000]]
  1970. [rosout][DEBUG] 2018-03-19 23:28:53,386: ----------------------------------------------
  1971. [rosout][DEBUG] 2018-03-19 23:28:53,387:
  1972.  
  1973. dq:
  1974. [ 0.000 0.071 0.000 0.037 -0.000 -0.298 -0.000]
  1975. [rosout][DEBUG] 2018-03-19 23:28:53,388: ----------------------------------------------
  1976. [rosout][DEBUG] 2018-03-19 23:28:53,389:
  1977.  
  1978. dq:
  1979. [ 0.000 0.071 0.000 0.037 -0.000 -0.298 -0.000]
  1980. [rosout][DEBUG] 2018-03-19 23:28:53,389: ----------------------------------------------
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement