Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged

Conversation

knorth55
Copy link
Member

What is changed

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)

angle_vector_sequence

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
---

@knorth55 knorth55 force-pushed the angle-vector-sequence-motion-plan branch from 6900276 to 0135eac Compare February 15, 2017 07:17
@knorth55 knorth55 changed the title [pr2eus_moveit] support angle-vector-sequence with MoveIt! [pr2eus_moveit/robot-moveit.l] support angle-vector-sequence with MoveIt! Feb 15, 2017
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants