Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- [rosout][INFO] 2018-03-19 23:27:18,491: =============== cartesian_control() ===============
- [rosout][DEBUG] 2018-03-19 23:27:18,492:
- joint_transforms:
- [rosout][DEBUG] 2018-03-19 23:27:18,492:
- joint_transforms[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.310]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,493:
- joint_transforms[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 0.510]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,493:
- joint_transforms[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.710]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,502:
- joint_transforms[3]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 0.000]
- [ 0.000 1.000 0.000 0.910]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,505:
- joint_transforms[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.110]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,511:
- joint_transforms[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 1.300]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,513:
- joint_transforms[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,513: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,518:
- b_T_ee_current:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,518: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,519:
- b_T_ee_desired:
- [[ 1.000 0.000 0.000 -0.076]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,519: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,520:
- red_control:
- False
- [rosout][DEBUG] 2018-03-19 23:27:18,520: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,520:
- q_current:
- (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
- [rosout][DEBUG] 2018-03-19 23:27:18,521: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,521:
- q0_desired:
- 0
- [rosout][DEBUG] 2018-03-19 23:27:18,521: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,522:
- b_p_current:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,522: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,522:
- b_p_desired:
- [-0.076 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,523: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,523:
- x_current:
- [ 0.000 0.000 1.378 0.000 -0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,524: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,524:
- x_desired:
- [-0.076 0.000 1.378 0.000 -0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,525: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,525:
- b_dX:
- [-0.076 0.000 -0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,526: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,526:
- b_X_dot:
- [-0.038 0.000 -0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,527: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,532:
- V:
- [[ 1.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,533: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,533:
- V_ee:
- [-0.038 0.000 -0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,534: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,534: ++++++++++++++++++++++++++++++++++++++++++++++
- [rosout][DEBUG] 2018-03-19 23:27:18,536:
- ee_T_ee_desired:
- [[ 1.000 0.000 0.000 -0.076]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,536: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,537:
- delta_X:
- [-0.038 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,537: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,538:
- delta_X:
- [-0.038 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,538: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,539:
- delta_W:
- [ 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,539: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,540:
- delta_W:
- [ 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,540: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,541:
- V_ee:
- [-0.038 0.000 -0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,541: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,542: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,542: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,544:
- j_T_ee:
- [rosout][DEBUG] 2018-03-19 23:27:18,545:
- j_T_ee[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,546:
- j_T_ee[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 -0.868]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,550:
- j_T_ee[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,551:
- j_T_ee[3]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.468]
- [ 0.000 -1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,551:
- j_T_ee[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,552:
- j_T_ee[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 -0.078]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,553:
- j_T_ee[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,553: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,554:
- ee_T_j:
- [rosout][DEBUG] 2018-03-19 23:27:18,554:
- ee_T_j[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,554:
- ee_T_j[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [-0.000 -1.000 0.000 -0.868]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,555:
- ee_T_j[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,556:
- ee_T_j[3]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 0.000 -1.000 -0.000]
- [ 0.000 1.000 0.000 -0.468]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,556:
- ee_T_j[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,557:
- ee_T_j[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [-0.000 -1.000 0.000 -0.078]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,557:
- ee_T_j[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,558: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,558:
- V_j:
- [rosout][DEBUG] 2018-03-19 23:27:18,559:
- V_j[0]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,559:
- V_j[1]:
- [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
- [-0.000 -1.000 0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,560:
- V_j[2]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,565:
- V_j[3]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [-0.000 0.000 -1.000 0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 -1.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,566:
- V_j[4]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,569:
- V_j[5]:
- [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
- [-0.000 -1.000 0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,569:
- V_j[6]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,570: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,570: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,571: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,571:
- J:
- [[-0.000 0.000 -0.000 -0.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 0.000 0.000]
- [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
- [ 1.000 0.000 1.000 0.000 1.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,572: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,573:
- J_p:
- [[ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 -0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]]
- [rosout][DEBUG] 2018-03-19 23:27:18,573: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,574:
- J . J_p:
- [[ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:27:18,574: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,577:
- dq:
- [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,579: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:27:18,579:
- dq:
- [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:27:18,582: ----------------------------------------------
- [rosout][INFO] 2018-03-19 23:28:52,212: =============== cartesian_control() ===============
- [rosout][DEBUG] 2018-03-19 23:28:52,213:
- joint_transforms:
- [rosout][DEBUG] 2018-03-19 23:28:52,214:
- joint_transforms[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.310]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,215:
- joint_transforms[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 0.510]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,215:
- joint_transforms[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.710]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,216:
- joint_transforms[3]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 0.000]
- [ 0.000 1.000 0.000 0.910]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,219:
- joint_transforms[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.110]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,219:
- joint_transforms[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 1.300]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,220:
- joint_transforms[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,221: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,221:
- b_T_ee_current:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,222: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,222:
- b_T_ee_desired:
- [[ 0.894 0.000 -0.447 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.447 -0.000 0.894 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,223: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,223:
- red_control:
- False
- [rosout][DEBUG] 2018-03-19 23:28:52,224: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,224:
- q_current:
- (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
- [rosout][DEBUG] 2018-03-19 23:28:52,225: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,225:
- q0_desired:
- 0
- [rosout][DEBUG] 2018-03-19 23:28:52,227: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,229:
- b_p_current:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,229: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,230:
- b_p_desired:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,230: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,231:
- x_current:
- [ 0.000 0.000 1.378 0.000 -0.000 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,231: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,232:
- x_desired:
- [ 0.000 0.000 1.378 -0.000 -0.464 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,232: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,233:
- b_dX:
- [ 0.000 0.000 -0.000 -0.000 -0.464 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,234: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,234:
- b_X_dot:
- [ 0.000 0.000 -0.000 -0.000 -0.232 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,234: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,241:
- V:
- [[ 1.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,244: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,247:
- V_ee:
- [ 0.000 0.000 -0.000 -0.000 -0.232 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,249: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,250: ++++++++++++++++++++++++++++++++++++++++++++++
- [rosout][DEBUG] 2018-03-19 23:28:52,252:
- ee_T_ee_desired:
- [[ 0.894 0.000 -0.447 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.447 -0.000 0.894 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,253: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,254:
- delta_X:
- [ 0.000 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,256: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,257:
- delta_X:
- [ 0.000 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,258: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,262:
- delta_W:
- [-0.000 -0.464 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,264: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,264:
- delta_W:
- [-0.000 -0.464 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,265: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,265:
- V_ee:
- [ 0.000 0.000 -0.000 -0.000 -0.464 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,267: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,268: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,269: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,269:
- j_T_ee:
- [rosout][DEBUG] 2018-03-19 23:28:52,270:
- j_T_ee[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,271:
- j_T_ee[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 -0.868]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,273:
- j_T_ee[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,274:
- j_T_ee[3]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.468]
- [ 0.000 -1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,278:
- j_T_ee[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,281:
- j_T_ee[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 -1.000 -0.078]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,282:
- j_T_ee[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,284: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,285:
- ee_T_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,285:
- ee_T_j[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,287:
- ee_T_j[1]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [-0.000 -1.000 0.000 -0.868]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,288:
- ee_T_j[2]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,289:
- ee_T_j[3]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 0.000 -1.000 -0.000]
- [ 0.000 1.000 0.000 -0.468]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,291:
- ee_T_j[4]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,294:
- ee_T_j[5]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [-0.000 -1.000 0.000 -0.078]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,295:
- ee_T_j[6]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,296: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,297:
- V_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,298:
- V_j[0]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,301:
- V_j[1]:
- [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
- [-0.000 -1.000 0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,302:
- V_j[2]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,304:
- V_j[3]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [-0.000 0.000 -1.000 0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 -1.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,305:
- V_j[4]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,308:
- V_j[5]:
- [[ 1.000 0.000 0.000 -0.000 0.000 0.000]
- [ 0.000 0.000 1.000 -0.000 -0.000 0.000]
- [-0.000 -1.000 0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.000 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,313:
- V_j[6]:
- [[ 1.000 0.000 0.000 -0.000 0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,315: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,316: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,317: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,318:
- J:
- [[-0.000 0.000 -0.000 -0.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 0.000 0.000]
- [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
- [ 1.000 0.000 1.000 0.000 1.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,320: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,321:
- J_p:
- [[ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 -0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]
- [ 0.000 0.000 0.000 0.000 0.333 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.250]]
- [rosout][DEBUG] 2018-03-19 23:28:52,323: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,325:
- J . J_p:
- [[ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,327: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,328:
- dq:
- [-0.000 -0.155 -0.000 0.155 -0.000 -0.155 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,331: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,331:
- dq:
- [-0.000 -0.155 -0.000 0.155 -0.000 -0.155 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,332: ----------------------------------------------
- [rosout][INFO] 2018-03-19 23:28:52,499: =============== cartesian_control() ===============
- [rosout][DEBUG] 2018-03-19 23:28:52,500:
- joint_transforms:
- [rosout][DEBUG] 2018-03-19 23:28:52,502:
- joint_transforms[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.310]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,504:
- joint_transforms[1]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 0.510]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,505:
- joint_transforms[2]:
- [[ 1.000 0.000 -0.009 -0.002]
- [-0.000 1.000 0.000 0.000]
- [ 0.009 0.000 1.000 0.710]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,506:
- joint_transforms[3]:
- [[ 1.000 -0.009 -0.000 -0.004]
- [-0.000 0.000 -1.000 0.000]
- [ 0.009 1.000 -0.000 0.910]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,507:
- joint_transforms[4]:
- [[ 1.000 0.000 -0.018 -0.007]
- [-0.000 1.000 0.000 0.000]
- [ 0.018 0.000 1.000 1.110]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,509:
- joint_transforms[5]:
- [[ 1.000 0.018 0.000 -0.010]
- [-0.000 -0.000 1.000 0.000]
- [ 0.018 -1.000 0.000 1.300]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,511:
- joint_transforms[6]:
- [[ 1.000 0.000 -0.026 -0.012]
- [-0.000 1.000 0.000 0.000]
- [ 0.026 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,512: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,513:
- b_T_ee_current:
- [[ 1.000 0.000 -0.026 -0.012]
- [-0.000 1.000 0.000 0.000]
- [ 0.026 0.000 1.000 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,515: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,516:
- b_T_ee_desired:
- [[ 0.866 0.000 -0.500 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.500 -0.000 0.866 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,518: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,518:
- red_control:
- False
- [rosout][DEBUG] 2018-03-19 23:28:52,519: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,520:
- q_current:
- (-1.8967685315544776e-12, -0.008808642438297074, -1.8967693784890137e-12, 0.008808642438297074, -1.8967693784890137e-12, -0.008808642438297074, -1.8967693784890137e-12)
- [rosout][DEBUG] 2018-03-19 23:28:52,522: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,523:
- q0_desired:
- 0
- [rosout][DEBUG] 2018-03-19 23:28:52,524: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,525:
- b_p_current:
- [-0.012 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,526: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,526:
- b_p_desired:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,527: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,528:
- x_current:
- [-0.012 0.000 1.378 0.000 -0.026 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,529: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,530:
- x_desired:
- [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,531: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,532:
- b_dX:
- [ 0.012 -0.000 0.000 -0.000 -0.497 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,533: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,536:
- b_X_dot:
- [ 0.006 -0.000 0.000 -0.000 -0.248 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,536: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,537:
- V:
- [[ 1.000 -0.000 0.026 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000 0.000 0.000]
- [-0.026 0.000 1.000 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 0.026]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.026 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,539: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,541:
- V_ee:
- [ 0.006 0.000 -0.000 -0.000 -0.248 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,544: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,544: ++++++++++++++++++++++++++++++++++++++++++++++
- [rosout][DEBUG] 2018-03-19 23:28:52,552:
- ee_T_ee_desired:
- [[ 0.879 0.000 -0.476 0.012]
- [ 0.000 1.000 0.000 0.000]
- [ 0.476 -0.000 0.879 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,553: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,554:
- delta_X:
- [ 0.006 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,555: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,556:
- delta_X:
- [ 0.006 0.000 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,556: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,558:
- delta_W:
- [-0.000 -0.497 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,558: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,559:
- delta_W:
- [-0.000 -0.497 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,560: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,561:
- V_ee:
- [ 0.006 0.000 -0.000 -0.000 -0.497 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,561: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,562: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,564: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,572:
- j_T_ee:
- [rosout][DEBUG] 2018-03-19 23:28:52,575:
- j_T_ee[0]:
- [[ 1.000 0.000 -0.026 -0.012]
- [-0.000 1.000 0.000 0.000]
- [ 0.026 0.000 1.000 1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,577:
- j_T_ee[1]:
- [[ 1.000 0.000 -0.026 -0.012]
- [-0.026 -0.000 -1.000 -0.868]
- [-0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,579:
- j_T_ee[2]:
- [[ 1.000 0.000 -0.018 -0.005]
- [-0.000 1.000 0.000 0.000]
- [ 0.018 0.000 1.000 0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,582:
- j_T_ee[3]:
- [[ 1.000 0.000 -0.018 -0.005]
- [ 0.018 0.000 1.000 0.468]
- [ 0.000 -1.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,584:
- j_T_ee[4]:
- [[ 1.000 0.000 -0.009 -0.001]
- [-0.000 1.000 0.000 0.000]
- [ 0.009 0.000 1.000 0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,585:
- j_T_ee[5]:
- [[ 1.000 0.000 -0.009 -0.001]
- [-0.009 -0.000 -1.000 -0.078]
- [-0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,586:
- j_T_ee[6]:
- [[ 1.000 0.000 0.000 -0.000]
- [-0.000 1.000 0.000 0.000]
- [-0.000 -0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,588: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,588:
- ee_T_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,589:
- ee_T_j[0]:
- [[ 1.000 -0.000 0.026 -0.016]
- [ 0.000 1.000 0.000 -0.000]
- [-0.026 0.000 1.000 -1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,590:
- ee_T_j[1]:
- [[ 1.000 -0.026 -0.000 -0.010]
- [ 0.000 -0.000 1.000 -0.000]
- [-0.026 -1.000 0.000 -0.868]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,592:
- ee_T_j[2]:
- [[ 1.000 -0.000 0.018 -0.007]
- [ 0.000 1.000 0.000 -0.000]
- [-0.018 0.000 1.000 -0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,593:
- ee_T_j[3]:
- [[ 1.000 0.018 0.000 -0.003]
- [ 0.000 0.000 -1.000 -0.000]
- [-0.018 1.000 -0.000 -0.468]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,594:
- ee_T_j[4]:
- [[ 1.000 -0.000 0.009 -0.002]
- [ 0.000 1.000 0.000 -0.000]
- [-0.009 0.000 1.000 -0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,597:
- ee_T_j[5]:
- [[ 1.000 -0.009 -0.000 -0.000]
- [ 0.000 -0.000 1.000 0.000]
- [-0.009 -1.000 0.000 -0.078]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,598:
- ee_T_j[6]:
- [[ 1.000 -0.000 -0.000 0.000]
- [ 0.000 1.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,600: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,601:
- V_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,603:
- V_j[0]:
- [[ 1.000 -0.000 0.026 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.026 0.000 1.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 0.026]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.026 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,604:
- V_j[1]:
- [[ 1.000 -0.026 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 -0.000 1.000 -0.000 0.000 -0.012]
- [-0.026 -1.000 0.000 0.023 -0.012 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.026 -0.000]
- [ 0.000 0.000 0.000 0.000 -0.000 1.000]
- [ 0.000 0.000 0.000 -0.026 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,608:
- V_j[2]:
- [[ 1.000 -0.000 0.018 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.018 0.000 1.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 0.018]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.018 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,611:
- V_j[3]:
- [[ 1.000 0.018 0.000 -0.000 -0.000 -0.000]
- [ 0.000 0.000 -1.000 0.000 -0.000 0.005]
- [-0.018 1.000 -0.000 -0.008 0.005 0.000]
- [ 0.000 0.000 0.000 1.000 0.018 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 -1.000]
- [ 0.000 0.000 0.000 -0.018 1.000 -0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,614:
- V_j[4]:
- [[ 1.000 -0.000 0.009 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.009 0.000 1.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 0.009]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.009 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,615:
- V_j[5]:
- [[ 1.000 -0.009 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 -0.000 1.000 -0.000 0.000 -0.001]
- [-0.009 -1.000 0.000 0.001 -0.001 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.009 -0.000]
- [ 0.000 0.000 0.000 0.000 -0.000 1.000]
- [ 0.000 0.000 0.000 -0.009 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,616:
- V_j[6]:
- [[ 1.000 -0.000 -0.000 -0.000 -0.000 0.000]
- [ 0.000 1.000 -0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 1.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,617: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,618: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,619: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,619:
- J:
- [[-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 0.000]
- [-0.000 -0.012 -0.000 0.005 -0.000 -0.001 0.000]
- [-0.000 -0.000 -0.000 0.000 -0.000 -0.000 -0.000]
- [ 0.026 -0.000 0.018 0.000 0.009 -0.000 -0.000]
- [ 0.000 1.000 0.000 -1.000 0.000 1.000 -0.000]
- [ 1.000 0.000 1.000 -0.000 1.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,621: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,622:
- J_p:
- [[-0.000 -0.000 -0.000 34.059 0.000 -0.200]
- [-0.000 -90.747 -0.000 0.000 -0.210 -0.000]
- [-0.000 -0.000 -0.000 11.355 0.000 0.100]
- [-0.000 -16.469 -0.000 0.000 -0.432 -0.000]
- [ 0.000 0.000 0.000 -11.349 -0.000 0.400]
- [ 0.000 74.279 0.000 0.000 0.778 -0.000]
- [ 0.000 0.000 0.000 -34.052 -0.000 0.700]]
- [rosout][DEBUG] 2018-03-19 23:28:52,624: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,625:
- J . J_p:
- [[ 0.000 0.000 0.000 -0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [-0.000 0.000 -0.000 1.000 0.000 0.000]
- [ 0.000 -0.000 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,627: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,629:
- dq:
- [-0.000 0.104 -0.000 0.214 0.000 -0.386 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,633: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,634:
- dq:
- [-0.000 0.104 -0.000 0.214 0.000 -0.386 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,635: ----------------------------------------------
- [rosout][INFO] 2018-03-19 23:28:52,652: =============== cartesian_control() ===============
- [rosout][DEBUG] 2018-03-19 23:28:52,652:
- joint_transforms:
- [rosout][DEBUG] 2018-03-19 23:28:52,653:
- joint_transforms[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.310]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,655:
- joint_transforms[1]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 0.510]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,656:
- joint_transforms[2]:
- [[ 1.000 0.000 -0.021 -0.004]
- [-0.000 1.000 0.000 0.000]
- [ 0.021 0.000 1.000 0.710]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,658:
- joint_transforms[3]:
- [[ 1.000 -0.021 -0.000 -0.008]
- [-0.000 0.000 -1.000 0.000]
- [ 0.021 1.000 -0.000 0.910]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,660:
- joint_transforms[4]:
- [[ 0.999 0.000 -0.042 -0.017]
- [-0.000 1.000 0.000 0.000]
- [ 0.042 0.000 0.999 1.110]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,662:
- joint_transforms[5]:
- [[ 0.999 0.042 0.000 -0.025]
- [-0.000 -0.000 1.000 0.000]
- [ 0.042 -0.999 0.000 1.300]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,664:
- joint_transforms[6]:
- [[ 0.998 0.000 -0.063 -0.030]
- [-0.000 1.000 0.000 0.000]
- [ 0.063 0.000 0.998 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,665: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,666:
- b_T_ee_current:
- [[ 0.998 0.000 -0.063 -0.030]
- [-0.000 1.000 0.000 0.000]
- [ 0.063 0.000 0.998 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,668: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,669:
- b_T_ee_desired:
- [[ 0.866 0.000 -0.500 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.500 -0.000 0.866 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,672: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,673:
- red_control:
- False
- [rosout][DEBUG] 2018-03-19 23:28:52,675: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,676:
- q_current:
- (-4.541408397491353e-12, -0.02109041882247827, -4.541410425295772e-12, 0.02109041882247827, -4.541410425295772e-12, -0.021090418822478262, -4.541410425295772e-12)
- [rosout][DEBUG] 2018-03-19 23:28:52,678: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,679:
- q0_desired:
- 0
- [rosout][DEBUG] 2018-03-19 23:28:52,680: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,681:
- b_p_current:
- [-0.030 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,682: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,683:
- b_p_desired:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,684: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,685:
- x_current:
- [-0.030 0.000 1.378 0.000 -0.063 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,686: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,687:
- x_desired:
- [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,688: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,689:
- b_dX:
- [ 0.030 -0.000 0.001 -0.000 -0.460 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,690: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,691:
- b_X_dot:
- [ 0.015 -0.000 0.000 -0.000 -0.230 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,693: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,695:
- V:
- [[ 0.998 -0.000 0.063 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000 0.000 0.000]
- [-0.063 0.000 0.998 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.998 -0.000 0.063]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.063 0.000 0.998]]
- [rosout][DEBUG] 2018-03-19 23:28:52,696: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,697:
- V_ee:
- [ 0.015 0.000 -0.001 -0.000 -0.230 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,698: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,700: ++++++++++++++++++++++++++++++++++++++++++++++
- [rosout][DEBUG] 2018-03-19 23:28:52,701:
- ee_T_ee_desired:
- [[ 0.896 0.000 -0.444 0.030]
- [ 0.000 1.000 0.000 0.000]
- [ 0.444 -0.000 0.896 -0.001]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,704: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,707:
- delta_X:
- [ 0.015 0.000 -0.001]
- [rosout][DEBUG] 2018-03-19 23:28:52,708: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,709:
- delta_X:
- [ 0.015 0.000 -0.001]
- [rosout][DEBUG] 2018-03-19 23:28:52,712: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,713:
- delta_W:
- [-0.000 -0.460 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,714: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,715:
- delta_W:
- [-0.000 -0.460 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,716: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,718:
- V_ee:
- [ 0.015 0.000 -0.001 -0.000 -0.460 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,719: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,720: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,721: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,727:
- j_T_ee:
- [rosout][DEBUG] 2018-03-19 23:28:52,728:
- j_T_ee[0]:
- [[ 0.998 0.000 -0.063 -0.030]
- [-0.000 1.000 0.000 0.000]
- [ 0.063 0.000 0.998 1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,730:
- j_T_ee[1]:
- [[ 0.998 0.000 -0.063 -0.030]
- [-0.063 -0.000 -0.998 -0.867]
- [-0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,731:
- j_T_ee[2]:
- [[ 0.999 0.000 -0.042 -0.012]
- [-0.000 1.000 0.000 0.000]
- [ 0.042 0.000 0.999 0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,732:
- j_T_ee[3]:
- [[ 0.999 0.000 -0.042 -0.012]
- [ 0.042 0.000 0.999 0.468]
- [ 0.000 -1.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,735:
- j_T_ee[4]:
- [[ 1.000 0.000 -0.021 -0.002]
- [-0.000 1.000 0.000 0.000]
- [ 0.021 0.000 1.000 0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,738:
- j_T_ee[5]:
- [[ 1.000 0.000 -0.021 -0.002]
- [-0.021 -0.000 -1.000 -0.078]
- [-0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,742:
- j_T_ee[6]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 1.000 -0.000 0.000]
- [ 0.000 -0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,743: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,746:
- ee_T_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,747:
- ee_T_j[0]:
- [[ 0.998 -0.000 0.063 -0.038]
- [ 0.000 1.000 0.000 -0.000]
- [-0.063 0.000 0.998 -1.068]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,749:
- ee_T_j[1]:
- [[ 0.998 -0.063 -0.000 -0.025]
- [ 0.000 -0.000 1.000 -0.000]
- [-0.063 -0.998 0.000 -0.868]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,750:
- ee_T_j[2]:
- [[ 0.999 -0.000 0.042 -0.017]
- [ 0.000 1.000 0.000 -0.000]
- [-0.042 0.000 0.999 -0.668]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,752:
- ee_T_j[3]:
- [[ 0.999 0.042 0.000 -0.008]
- [ 0.000 0.000 -1.000 -0.000]
- [-0.042 0.999 -0.000 -0.468]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,753:
- ee_T_j[4]:
- [[ 1.000 -0.000 0.021 -0.004]
- [ 0.000 1.000 0.000 -0.000]
- [-0.021 0.000 1.000 -0.268]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,756:
- ee_T_j[5]:
- [[ 1.000 -0.021 -0.000 0.000]
- [ 0.000 -0.000 1.000 0.000]
- [-0.021 -1.000 0.000 -0.078]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,757:
- ee_T_j[6]:
- [[ 1.000 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000]
- [-0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,759: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,761:
- V_j:
- [rosout][DEBUG] 2018-03-19 23:28:52,762:
- V_j[0]:
- [[ 0.998 -0.000 0.063 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.063 0.000 0.998 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.998 -0.000 0.063]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.063 0.000 0.998]]
- [rosout][DEBUG] 2018-03-19 23:28:52,764:
- V_j[1]:
- [[ 0.998 -0.063 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 -0.000 1.000 -0.000 0.000 -0.030]
- [-0.063 -0.998 0.000 0.055 -0.030 -0.000]
- [ 0.000 0.000 0.000 0.998 -0.063 -0.000]
- [ 0.000 0.000 0.000 0.000 -0.000 1.000]
- [ 0.000 0.000 0.000 -0.063 -0.998 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,766:
- V_j[2]:
- [[ 0.999 -0.000 0.042 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.042 0.000 0.999 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.999 -0.000 0.042]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.042 0.000 0.999]]
- [rosout][DEBUG] 2018-03-19 23:28:52,773:
- V_j[3]:
- [[ 0.999 0.042 0.000 -0.000 -0.000 -0.000]
- [ 0.000 0.000 -1.000 0.000 -0.000 0.012]
- [-0.042 0.999 -0.000 -0.020 0.012 0.000]
- [ 0.000 0.000 0.000 0.999 0.042 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 -1.000]
- [ 0.000 0.000 0.000 -0.042 0.999 -0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,776:
- V_j[4]:
- [[ 1.000 -0.000 0.021 -0.000 -0.000 -0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 -0.000]
- [-0.021 0.000 1.000 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 0.021]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.021 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,778:
- V_j[5]:
- [[ 1.000 -0.021 -0.000 -0.000 -0.000 -0.000]
- [ 0.000 -0.000 1.000 -0.000 0.000 -0.002]
- [-0.021 -1.000 0.000 0.002 -0.002 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.021 -0.000]
- [ 0.000 0.000 0.000 0.000 -0.000 1.000]
- [ 0.000 0.000 0.000 -0.021 -1.000 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,781:
- V_j[6]:
- [[ 1.000 -0.000 -0.000 -0.000 -0.000 0.000]
- [ 0.000 1.000 0.000 -0.000 -0.000 0.000]
- [-0.000 0.000 1.000 -0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 1.000 0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,783: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,784: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,785: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,787:
- J:
- [[-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 0.000]
- [-0.000 -0.030 -0.000 0.012 -0.000 -0.002 0.000]
- [-0.000 -0.000 -0.000 0.000 -0.000 -0.000 -0.000]
- [ 0.063 -0.000 0.042 0.000 0.021 -0.000 -0.000]
- [ 0.000 1.000 0.000 -1.000 0.000 1.000 0.000]
- [ 0.998 0.000 0.999 -0.000 1.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,790: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,791:
- J_p:
- [[-0.000 -0.000 -0.000 14.227 0.000 -0.200]
- [-0.000 -37.911 0.000 0.000 -0.210 -0.000]
- [-0.000 -0.000 -0.000 4.748 0.000 0.100]
- [-0.000 -6.878 0.000 0.000 -0.432 -0.000]
- [ 0.000 0.000 0.000 -4.733 -0.000 0.400]
- [ 0.000 31.033 -0.000 0.000 0.778 -0.000]
- [ 0.000 0.000 0.000 -14.211 -0.000 0.700]]
- [rosout][DEBUG] 2018-03-19 23:28:52,793: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,794:
- J . J_p:
- [[ 0.000 0.000 -0.000 -0.000 0.000 0.000]
- [ 0.000 1.000 -0.000 0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [-0.000 -0.000 -0.000 1.000 -0.000 -0.000]
- [ 0.000 -0.000 -0.000 -0.000 1.000 0.000]
- [ 0.000 -0.000 0.000 -0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:52,795: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,796:
- dq:
- [-0.000 0.096 -0.000 0.199 0.000 -0.358 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,800: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:52,803:
- dq:
- [-0.000 0.096 -0.000 0.199 0.000 -0.358 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:52,806: ----------------------------------------------
- [rosout][INFO] 2018-03-19 23:28:53,307: =============== cartesian_control() ===============
- [rosout][DEBUG] 2018-03-19 23:28:53,309:
- joint_transforms:
- [rosout][DEBUG] 2018-03-19 23:28:53,309:
- joint_transforms[0]:
- [[ 1.000 0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 1.000 0.310]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,310:
- joint_transforms[1]:
- [[ 1.000 0.000 0.000 0.000]
- [-0.000 0.000 1.000 0.000]
- [ 0.000 -1.000 0.000 0.510]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,310:
- joint_transforms[2]:
- [[ 1.000 0.000 -0.022 -0.004]
- [-0.000 1.000 0.000 0.000]
- [ 0.022 0.000 1.000 0.710]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,311:
- joint_transforms[3]:
- [[ 1.000 -0.022 -0.000 -0.009]
- [-0.000 0.000 -1.000 0.000]
- [ 0.022 1.000 -0.000 0.910]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,312:
- joint_transforms[4]:
- [[ 0.993 0.000 -0.119 -0.033]
- [-0.000 1.000 0.000 0.000]
- [ 0.119 0.000 0.993 1.109]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,312:
- joint_transforms[5]:
- [[ 0.993 0.119 0.000 -0.055]
- [-0.000 -0.000 1.000 0.000]
- [ 0.119 -0.993 -0.000 1.298]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,313:
- joint_transforms[6]:
- [[ 0.967 0.000 -0.255 -0.075]
- [-0.000 1.000 0.000 0.000]
- [ 0.255 -0.000 0.967 1.373]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,313: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,314:
- b_T_ee_current:
- [[ 0.967 0.000 -0.255 -0.075]
- [ 0.000 1.000 0.000 0.000]
- [ 0.255 -0.000 0.967 1.373]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,314: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,316:
- b_T_ee_desired:
- [[ 0.866 0.000 -0.500 0.000]
- [ 0.000 1.000 0.000 0.000]
- [ 0.500 -0.000 0.866 1.378]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,317: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,318:
- red_control:
- False
- [rosout][DEBUG] 2018-03-19 23:28:53,318: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,319:
- q_current:
- (-2.1248224469441799e-07, -0.021798680042333853, -7.084187827909742e-08, 0.09774790323418787, 7.080791103921825e-08, -0.13870158210822248, 2.1244829577527386e-07)
- [rosout][DEBUG] 2018-03-19 23:28:53,320: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,320:
- q0_desired:
- 0
- [rosout][DEBUG] 2018-03-19 23:28:53,320: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,321:
- b_p_current:
- [-0.075 0.000 1.373 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,321: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,322:
- b_p_desired:
- [ 0.000 0.000 1.378 1.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,322: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,323:
- x_current:
- [-0.075 0.000 1.373 -0.000 -0.258 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,323: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,325:
- x_desired:
- [ 0.000 0.000 1.378 -0.000 -0.523 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,325: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,326:
- b_dX:
- [ 0.075 -0.000 0.005 0.000 -0.265 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,328: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,328:
- b_X_dot:
- [ 0.038 -0.000 0.003 0.000 -0.132 0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,329: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,330:
- V:
- [[ 0.967 0.000 0.255 0.000 0.000 0.000]
- [ 0.000 1.000 -0.000 0.000 0.000 0.000]
- [-0.255 0.000 0.967 0.000 0.000 0.000]
- [ 0.000 0.000 0.000 0.967 0.000 0.255]
- [ 0.000 0.000 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 -0.255 0.000 0.967]]
- [rosout][DEBUG] 2018-03-19 23:28:53,331: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,333:
- V_ee:
- [ 0.037 -0.000 -0.007 0.000 -0.132 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,334: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,334: ++++++++++++++++++++++++++++++++++++++++++++++
- [rosout][DEBUG] 2018-03-19 23:28:53,337:
- ee_T_ee_desired:
- [[ 0.965 0.000 -0.262 0.074]
- [-0.000 1.000 -0.000 -0.000]
- [ 0.262 0.000 0.965 -0.014]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,337: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,338:
- delta_X:
- [ 0.037 -0.000 -0.007]
- [rosout][DEBUG] 2018-03-19 23:28:53,338: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,339:
- delta_X:
- [ 0.037 -0.000 -0.007]
- [rosout][DEBUG] 2018-03-19 23:28:53,339: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,340:
- delta_W:
- [ 0.000 -0.265 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,340: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,341:
- delta_W:
- [ 0.000 -0.265 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,342: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,343:
- V_ee:
- [ 0.037 -0.000 -0.007 0.000 -0.265 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,343: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,344: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,344: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,353:
- j_T_ee:
- [rosout][DEBUG] 2018-03-19 23:28:53,354:
- j_T_ee[0]:
- [[ 0.967 0.000 -0.255 -0.075]
- [ 0.000 1.000 0.000 0.000]
- [ 0.255 -0.000 0.967 1.063]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,355:
- j_T_ee[1]:
- [[ 0.967 -0.000 -0.255 -0.075]
- [-0.255 0.000 -0.967 -0.863]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,355:
- j_T_ee[2]:
- [[ 0.972 -0.000 -0.234 -0.056]
- [ 0.000 1.000 0.000 0.000]
- [ 0.234 -0.000 0.972 0.664]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,356:
- j_T_ee[3]:
- [[ 0.972 -0.000 -0.234 -0.056]
- [ 0.234 -0.000 0.972 0.464]
- [-0.000 -1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,357:
- j_T_ee[4]:
- [[ 0.990 -0.000 -0.138 -0.011]
- [ 0.000 1.000 -0.000 -0.000]
- [ 0.138 -0.000 0.990 0.267]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,358:
- j_T_ee[5]:
- [[ 0.990 -0.000 -0.138 -0.011]
- [-0.138 0.000 -0.990 -0.077]
- [ 0.000 1.000 0.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,359:
- j_T_ee[6]:
- [[ 1.000 -0.000 0.000 0.000]
- [ 0.000 1.000 0.000 0.000]
- [-0.000 -0.000 1.000 0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,359: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,367:
- ee_T_j:
- [rosout][DEBUG] 2018-03-19 23:28:53,368:
- ee_T_j[0]:
- [[ 0.967 0.000 0.255 -0.199]
- [ 0.000 1.000 -0.000 0.000]
- [-0.255 0.000 0.967 -1.047]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,368:
- ee_T_j[1]:
- [[ 0.967 -0.255 0.000 -0.148]
- [-0.000 0.000 1.000 0.000]
- [-0.255 -0.967 0.000 -0.853]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,369:
- ee_T_j[2]:
- [[ 0.972 0.000 0.234 -0.101]
- [-0.000 1.000 -0.000 0.000]
- [-0.234 0.000 0.972 -0.659]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,370:
- ee_T_j[3]:
- [[ 0.972 0.234 -0.000 -0.054]
- [-0.000 -0.000 -1.000 0.000]
- [-0.234 0.972 0.000 -0.464]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,370:
- ee_T_j[4]:
- [[ 0.990 0.000 0.138 -0.026]
- [-0.000 1.000 -0.000 0.000]
- [-0.138 -0.000 0.990 -0.266]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,371:
- ee_T_j[5]:
- [[ 0.990 -0.138 0.000 -0.000]
- [-0.000 0.000 1.000 0.000]
- [-0.138 -0.990 0.000 -0.078]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,371:
- ee_T_j[6]:
- [[ 1.000 0.000 -0.000 -0.000]
- [-0.000 1.000 -0.000 0.000]
- [ 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,372: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,372:
- V_j:
- [rosout][DEBUG] 2018-03-19 23:28:53,373:
- V_j[0]:
- [[ 0.967 0.000 0.255 -0.000 0.000 -0.000]
- [ 0.000 1.000 -0.000 -0.000 -0.000 0.000]
- [-0.255 0.000 0.967 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.967 0.000 0.255]
- [ 0.000 0.000 0.000 0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 -0.255 0.000 0.967]]
- [rosout][DEBUG] 2018-03-19 23:28:53,374:
- V_j[1]:
- [[ 0.967 -0.255 0.000 -0.000 -0.000 0.000]
- [-0.000 0.000 1.000 0.000 -0.000 -0.075]
- [-0.255 -0.967 0.000 0.220 -0.073 -0.000]
- [ 0.000 0.000 0.000 0.967 -0.255 0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.255 -0.967 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,375:
- V_j[2]:
- [[ 0.972 0.000 0.234 -0.000 0.000 -0.000]
- [-0.000 1.000 -0.000 0.000 -0.000 0.000]
- [-0.234 0.000 0.972 -0.000 0.000 -0.000]
- [ 0.000 0.000 0.000 0.972 0.000 0.234]
- [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 -0.234 0.000 0.972]]
- [rosout][DEBUG] 2018-03-19 23:28:53,376:
- V_j[3]:
- [[ 0.972 0.234 -0.000 -0.000 0.000 0.000]
- [-0.000 -0.000 -1.000 0.000 0.000 0.056]
- [-0.234 0.972 0.000 -0.109 0.055 -0.000]
- [ 0.000 0.000 0.000 0.972 0.234 -0.000]
- [ 0.000 0.000 0.000 -0.000 -0.000 -1.000]
- [ 0.000 0.000 0.000 -0.234 0.972 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,377:
- V_j[4]:
- [[ 0.990 0.000 0.138 -0.000 0.000 0.000]
- [-0.000 1.000 -0.000 0.000 -0.000 0.000]
- [-0.138 -0.000 0.990 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 0.990 0.000 0.138]
- [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 -0.138 -0.000 0.990]]
- [rosout][DEBUG] 2018-03-19 23:28:53,378:
- V_j[5]:
- [[ 0.990 -0.138 0.000 -0.000 -0.000 0.000]
- [-0.000 0.000 1.000 0.000 -0.000 -0.011]
- [-0.138 -0.990 0.000 0.011 -0.011 -0.000]
- [ 0.000 0.000 0.000 0.990 -0.138 0.000]
- [ 0.000 0.000 0.000 -0.000 0.000 1.000]
- [ 0.000 0.000 0.000 -0.138 -0.990 0.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,378:
- V_j[6]:
- [[ 1.000 0.000 -0.000 -0.000 0.000 0.000]
- [-0.000 1.000 -0.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 1.000 0.000 -0.000 -0.000]
- [ 0.000 0.000 0.000 1.000 0.000 -0.000]
- [ 0.000 0.000 0.000 -0.000 1.000 -0.000]
- [ 0.000 0.000 0.000 0.000 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,380: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,381: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,382: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,382:
- J:
- [[-0.000 0.000 -0.000 0.000 0.000 0.000 0.000]
- [ 0.000 -0.075 0.000 0.056 0.000 -0.011 -0.000]
- [-0.000 -0.000 -0.000 -0.000 -0.000 -0.000 -0.000]
- [ 0.255 0.000 0.234 -0.000 0.138 0.000 -0.000]
- [-0.000 1.000 -0.000 -1.000 -0.000 1.000 -0.000]
- [ 0.967 0.000 0.972 0.000 0.990 0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,384: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,384:
- J_p:
- [[-0.000 0.000 -0.000 2.415 -0.000 -0.132]
- [ 0.000 -12.658 0.000 -0.000 -0.267 0.000]
- [-0.000 0.000 -0.000 1.896 -0.000 -0.048]
- [-0.000 4.067 -0.000 -0.000 -0.140 0.000]
- [ 0.000 -0.000 0.000 -0.440 0.000 0.326]
- [-0.000 16.725 -0.000 -0.000 1.126 0.000]
- [ 0.000 -0.000 0.000 -3.742 0.000 0.852]]
- [rosout][DEBUG] 2018-03-19 23:28:53,385: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,386:
- J . J_p:
- [[ 0.000 -0.000 0.000 -0.000 -0.000 0.000]
- [-0.000 1.000 -0.000 -0.000 -0.000 0.000]
- [ 0.000 0.000 0.000 0.000 0.000 0.000]
- [-0.000 -0.000 -0.000 1.000 -0.000 -0.000]
- [-0.000 -0.000 -0.000 -0.000 1.000 0.000]
- [ 0.000 0.000 0.000 0.000 -0.000 1.000]]
- [rosout][DEBUG] 2018-03-19 23:28:53,386: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,387:
- dq:
- [ 0.000 0.071 0.000 0.037 -0.000 -0.298 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,388: ----------------------------------------------
- [rosout][DEBUG] 2018-03-19 23:28:53,389:
- dq:
- [ 0.000 0.071 0.000 0.037 -0.000 -0.298 -0.000]
- [rosout][DEBUG] 2018-03-19 23:28:53,389: ----------------------------------------------
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement