Robotics StackExchange | Archived questions

Dual arm UR5 Real time interconnection

Ubuntu 16.04.

When I run command "roslaunch urXmoveitconfig urXmoveitplanning_execution.launch limited:=true", it's an initializing OMPL error and Action client not connected error:

[ INFO] [1541579345.106159949]: Initializing OMPL interface using ROS parameters
[ERROR] [1541579345.108937973]: Could not find the planner configuration 'kConfigDefault' on the param server
[ERROR] [1541579345.117633459]: Could not find the planner configuration 'FMTkConfigDefault' on the param server
[ERROR] [1541579345.118218054]: Could not find the planner configuration 'BFMTkConfigDefault' on the param server
[ERROR] [1541579345.118660389]: Could not find the planner configuration 'PDSTkConfigDefault' on the param server
    ...

Thank you !

ompl_planning.yaml:

planner_configs:
  SBL:
    type: geometric::SBL
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  EST:
    type: geometric::EST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  LBKPIECE:
    type: geometric::LBKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  BKPIECE:
    type: geometric::BKPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  KPIECE:
    type: geometric::KPIECE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
  RRT:
    type: geometric::RRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
  RRTConnect:
    type: geometric::RRTConnect
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  RRTstar:
    type: geometric::RRTstar
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
  TRRT:
    type: geometric::TRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    max_states_failed: 10  # when to start increasing temp. default: 10
    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
    init_temperature: 10e-6  # initial temperature. default: 10e-6
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
  PRM:
    type: geometric::PRM
    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
  PRMstar:
    type: geometric::PRMstar
  FMT:
    type: geometric::FMT
    num_samples: 1000  # number of states that the planner should sample. default: 1000
    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
    nearest_k: 1  # use Knearest strategy. default: 1
    cache_cc: 1  # use collision checking cache. default: 1
    heuristics: 0  # activate cost to go heuristics. default: 0
    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
  BFMT:
    type: geometric::BFMT
    num_samples: 1000  # number of states that the planner should sample. default: 1000
    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
    nearest_k: 1  # use the Knearest strategy. default: 1
    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
    heuristics: 1  # activates cost to go heuristics. default: 1
    cache_cc: 1  # use the collision checking cache. default: 1
    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
  PDST:
    type: geometric::PDST
  STRIDE:
    type: geometric::STRIDE
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 
    max_degree: 18  # max degree of a node in the GNAT. default: 12
    min_degree: 12  # min degree of a node in the GNAT. default: 12
    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
  BiTRRT:
    type: geometric::BiTRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
    init_temperature: 100  # initial temperature. default: 100
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
  LBTRRT:
    type: geometric::LBTRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
    epsilon: 0.4  # optimality approximation factor. default: 0.4
  BiEST:
    type: geometric::BiEST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  ProjEST:
    type: geometric::ProjEST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
  LazyPRM:
    type: geometric::LazyPRM
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  LazyPRMstar:
    type: geometric::LazyPRMstar
  SPARS:
    type: geometric::SPARS
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 1000  # maximum consecutive failure limit. default: 1000
  SPARStwo:
    type: geometric::SPARStwo
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 5000  # maximum consecutive failure limit. default: 5000
ur5_left:
  default_planner_config: NkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - ESTkConfigDefault
    - LBKPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - KPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - FMTkConfigDefault
    - BFMTkConfigDefault
    - PDSTkConfigDefault
    - STRIDEkConfigDefault
    - BiTRRTkConfigDefault
    - LBTRRTkConfigDefault
    - BiESTkConfigDefault
    - ProjESTkConfigDefault
    - LazyPRMkConfigDefault
    - LazyPRMstarkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault
  projection_evaluator: joints(leftshoulder_pan_joint,leftshoulder_lift_joint)
  longest_valid_segment_fraction: 0.005
ur5_right:
  default_planner_config: NkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - ESTkConfigDefault
    - LBKPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - KPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - FMTkConfigDefault
    - BFMTkConfigDefault
    - PDSTkConfigDefault
    - STRIDEkConfigDefault
    - BiTRRTkConfigDefault
    - LBTRRTkConfigDefault
    - BiESTkConfigDefault
    - ProjESTkConfigDefault
    - LazyPRMkConfigDefault
    - LazyPRMstarkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault
  projection_evaluator: joints(rightshoulder_pan_joint,rightshoulder_lift_joint)
  longest_valid_segment_fraction: 0.005
left_end:
  default_planner_config: kConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - ESTkConfigDefault
    - LBKPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - KPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - FMTkConfigDefault
    - BFMTkConfigDefault
    - PDSTkConfigDefault
    - STRIDEkConfigDefault
    - BiTRRTkConfigDefault
    - LBTRRTkConfigDefault
    - BiESTkConfigDefault
    - ProjESTkConfigDefault
    - LazyPRMkConfigDefault
    - LazyPRMstarkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault
right_end:
  default_planner_config: kConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - ESTkConfigDefault
    - LBKPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - KPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - FMTkConfigDefault
    - BFMTkConfigDefault
    - PDSTkConfigDefault
    - STRIDEkConfigDefault
    - BiTRRTkConfigDefault
    - LBTRRTkConfigDefault
    - BiESTkConfigDefault
    - ProjESTkConfigDefault
    - LazyPRMkConfigDefault
    - LazyPRMstarkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault

cotroller.yaml:

controller_list:
  - name: "right_arm_controller"
    action_ns: ur5/follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rightshoulder_pan_joint
      - rightshoulder_lift_joint
      - rightelbow_joint
      - rightwrist_1_joint
      - rightwrist_2_joint
      - rightwrist_3_joint

      - name: "left_arm_controller"
        action_ns: ur5/follow_joint_trajectory
        type: FollowJointTrajectory
        default: true
        joints:
          - leftshoulder_pan_joint
          - leftshoulder_lift_joint
          - leftelbow_joint
          - leftwrist_1_joint
          - leftwrist_2_joint
          - leftwrist_3_joint

Asked by zzz on 2018-11-06 20:55:58 UTC

Comments

Answers

@zzz hello, I guess you didn't change anything in the ompl_planning.yaml, so no real need to paste it. But in your controller.yaml you have some mistakes. You have a namespace problem I think. If in your bringup.launch file you named your namespace group like this:

  • List item

      group ns="left" and group ns="right"

Then you ur_driver will generate topics like "/left/folow_joint_trajectory" and "/right/follow_joint_trajectory" and in your controller_list you have different names (ur5/follow_joint_trajectory), so the ur_driver can never find the controllers because it's excepted another names. So i sugegest you to modify the name and the action_key of your controller.yaml like below, to make them communicate in your namespaces:

  • List item

       -name: ""  
      action_ns: right/follow_joint_trajectory 
      (No change for the rest)
   -name: ""
      action_ns: left/follow_joint_trajectory
       (no change for the rest)

Asked by Soul Goumey on 2018-11-07 12:12:50 UTC

Comments

Thank you very much! The problem is solved by modified ompl_planning.yaml, and your reply about the controller helps me so much!

Asked by zzz on 2018-11-07 20:27:24 UTC

By the way ,need I to start two groups for each arm when using move_group.launch? My robot model is in one xacro, but how can I control the two arms on it?

Asked by zzz on 2018-11-08 03:48:01 UTC

Obviously you need to start two groups otherwise the topics and node run by the move_group.launch couldn't communicate with the ur_driver run in the namespaces. Be careful to use the same group names that you used in the bringup.launch

Asked by Soul Goumey on 2018-11-08 04:08:11 UTC

I didn't understand your question about how can you control the two arms? do you mean by using motion planning on Rviz? or by move_group_interface? Did you already check if the test_move.py works with your arms?

Asked by Soul Goumey on 2018-11-08 08:46:55 UTC

@Soul Goumey I did as you said. But the trouble is I couldn't recieve the right arm' joint state, when I use Rviz to do MotionPlanning wih the left arm. In my dual arm UR5s model(xacro) ,there are two arms. Do you know how to solve it?

Asked by zzz on 2018-11-12 10:26:35 UTC

@zzz you need to build your moveit package for the two arms to be able to use motionplanning with them, using moveit setup assistant and your dual arm file( urdf ). How it is about the test_move.py? Does it work with your two arms?

Asked by Soul Goumey on 2018-11-12 13:22:23 UTC

I have try many times, but test_move.py couldnot move on the demo.launch. There must be many mistakes. I modified the simpleactionclient to /right/follow_joint_trajectory, joint_states to /right/joint_states .There is "on goal" in terminal but rviz doesn't work.

Asked by zzz on 2018-11-13 21:43:12 UTC

@zzz i thought you were working on real robots. Because using demo.launch means that you’are on simulation environment, so even don’t have or don’t need ur_driver. Anyway now, i need to have a look to your package or catkin_ws/src. So try to compress it, put it on a website of download.

Asked by Soul Goumey on 2018-11-14 02:10:47 UTC

@zzz And be sure to write, on a readme file, what you want to do exactly. Because it’s still not clear. And then you can give us the link, we can use to download your package or catkin_ws/src.

Asked by Soul Goumey on 2018-11-14 02:14:53 UTC

Can I get your e-mail addrress?

Asked by zzz on 2018-11-14 03:11:33 UTC

@zzz I don't think it's possible by email. But you can use this website: http://www.sendbox.fr/index.html?upload=1 You can also leave me, your adress email .

Asked by Soul Goumey on 2018-11-14 03:22:31 UTC

zhujing6847@gmail.com I upload the src in sendbox ,then you can email me so that I can send it to you. Thank you very much!

Asked by zzz on 2018-11-14 03:29:40 UTC