[pr2eus_moveit/robot-moveit.l] support angle-vector-sequence with MoveIt!

[knorth55]

What is changed

• remove unnecessary trajectory-constraints methods (see TrajectoryConstraints message needs better documentation moveit/moveit_msgs#2)
• support angle-vector-sequence with moveit!

angle-vector-sequence with moveit!

• solve trajctory to each midpoint with moveit! separately
• concatenate result paths and get final trajctory
• send the trajectory to robot

Experiment

(setq avs (list (send *baxter* :untuck-pose :rarm) (send *baxter* :reset-pose :rarm)))
(send *ri* :angle-vector-sequence avs 6000 :rarm-controller 0 :move-arm :rarm)

rostopic echo /robot/limb/right/follow_trajectory_goal

header:
  seq: 1
  stamp:
    secs: 1487139899
    nsecs: 754134156
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs:         0
  id: 1487139899749636012_/default_robot_interface_1487139745943315325_10751_/robot/limb/right/follow_joint_trajectory_1
goal:
  trajectory:
    header:
      seq: 1
      stamp:
        secs: 1487139899
        nsecs: 754134156
      frame_id: ''
    joint_names: ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
    points:
      -
        positions: [-0.3499658482152863, -0.43537770915332774, 0.6985418400962629, 1.0473464671309722, 0.3489160947095502, 1.3958109239424168, 0.0009764803708530948]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs:   9999999
      -
        positions: [-0.3062708755702852, -0.4914105427420492, 0.7472709422324804, 1.1362656775521396, 0.2477995867471735, 1.3591726998909373, 0.0008378659528680129]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs: 459192480
      -
        positions: [-0.2625759029252841, -0.5474433763307709, 0.7960000443686979, 1.225184887973307, 0.1466830787847968, 1.3225344758394586, 0.0006992515348829315]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs: 711505408
      -
        positions: [-0.218880930280283, -0.6034762099194921, 0.8447291465049154, 1.3141040983944752, 0.04556657082242016, 1.285896251787979, 0.00056063711689785]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs: 944512192
      -
        positions: [-0.17518595763528177, -0.6595090435082138, 0.8934582486411329, 1.4030233088156425, -0.05554993713995654, 1.2492580277365004, 0.00042202269891276857]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 1
          nsecs: 177519008
      -
        positions: [-0.13149098499028067, -0.7155418770969355, 0.9421873507773504, 1.4919425192368099, -0.15666644510233318, 1.2126198036850209, 0.0002834082809276871]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 1
          nsecs: 410525792
      -
        positions: [-0.08779601234527967, -0.7715747106856572, 0.9909164529135679, 1.580861729657978, -0.2577829530647098, 1.1759815796335422, 0.00014479386294260555]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 1
          nsecs: 643532608
      -
        positions: [-0.044101039700278566, -0.8276075442743784, 1.039645555049785, 1.6697809400791455, -0.35889946102708636, 1.1393433555820627, 6.1794449575240966e-06]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 1
          nsecs: 876539392
      -
        positions: [-0.0004060670552774037, -0.8836403778631001, 1.0883746571860033, 1.7587001505003128, -0.4600159689894632, 1.102705131530584, -0.00013243497302755747]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 2
          nsecs: 109546152
      -
        positions: [0.0432889055897237, -0.9396732114518218, 1.1371037593222209, 1.847619360921481, -0.5611324769518395, 1.0660669074791045, -0.00027104939101263914]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 2
          nsecs: 372104032
      -
        positions: [0.08698387823472475, -0.995706045040543, 1.1858328614584384, 1.9365385713426484, -0.6622489849142164, 1.0294286834276258, -0.0004096638089977204]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 2
          nsecs: 998175680
      -
        positions: [0.08726646259971643, -0.9948376736367672, 1.186823891356143, 1.9373154697137043, -0.6632251157578448, 1.0297442586766534, 0.0]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 3
          nsecs:  17828940
      -
        positions: [0.043681375924426724, -0.9390447885919517, 1.138050804506939, 1.8482469206923575, -0.5620436280316268, 1.066335100906997, -4.3204124784097065e-05]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 3
          nsecs: 467412704
      -
        positions: [9.628924913704373e-05, -0.8832519035471367, 1.089277717657735, 1.7591783716710108, -0.46086214030540895, 1.1029259431373415, -8.640824956819413e-05]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 3
          nsecs: 720149376
      -
        positions: [-0.04348879742615264, -0.8274590185023212, 1.040504630808532, 1.670109822649664, -0.3596806525791909, 1.139516785367685, -0.00012961237435229122]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 3
          nsecs: 953547584
      -
        positions: [-0.08707388410144234, -0.7716661334575061, 0.9917315439593279, 1.5810412736283173, -0.2584991648529731, 1.1761076275980296, -0.00017281649913638826]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 4
          nsecs: 186945696
      -
        positions: [-0.13065897077673194, -0.7158732484126906, 0.9429584571101244, 1.4919727246069705, -0.15731767712675515, 1.212698469828374, -0.0002160206239204854]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 4
          nsecs: 420343808
      -
        positions: [-0.17424405745202165, -0.6600803633678756, 0.8941853702609204, 1.4029041755856237, -0.05613618940053722, 1.2492893120587176, -0.00025922474870458245]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 4
          nsecs: 653741952
      -
        positions: [-0.21782914412731136, -0.6042874783230601, 0.8454122834117168, 1.3138356265642779, 0.045045298325680716, 1.285880154289062, -0.0003024288734886794]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 4
          nsecs: 887140096
      -
        positions: [-0.26141423080260107, -0.548494593278245, 0.7966391965625128, 1.2247670775429311, 0.14622678605189865, 1.3224709965194057, -0.0003456329982727765]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 5
          nsecs: 120538152
      -
        positions: [-0.30499931747789066, -0.49270170823342974, 0.7478661097133092, 1.1356985285215844, 0.24740827377811658, 1.3590618387497502, -0.00038883712305687367]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 5
          nsecs: 383537056
      -
        positions: [-0.3485844041531805, -0.43690882318861446, 0.6990930228641052, 1.0466299795002376, 0.3485897615043345, 1.3956526809800947, -0.0004320412478409708]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 6
          nsecs:   9999999
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 0
    nsecs:         0
---