ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

cannot use moveit! with dual_ur5 platform

asked 2017-10-18 21:36:59 -0500

yyf gravatar image

updated 2017-10-19 02:56:50 -0500

Hello everyone! I'm using moveit! package to do motion planning on real dual_ur5 platform.I can ping both the IP adresses of the two arms.All config plugins may work well (like the .yaml files).But the warning occured when I run a Python file with moveit commander:[ WARN] [1508121443.319273668]: Failed to validate trajectory: couldn't receive full current joint state within 1s.

My bringup.launch:

 <launch>

 <group ns="left">
   <arg name="limited" default="false"/>
   <arg name="robot_ip" default="192.168.0.108"/>
   <arg name="reverse_port" default="50001"/>
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="left_" />
  <include file="$(find ur_bringup)/launch/ur5_bringup.launch">
   <arg name="limited" value="$(arg limited)"/>
   <arg name="robot_ip" value="$(arg robot_ip)"/>
   <arg name="reverse_port" value="$(arg reverse_port)"/>
   <arg name="min_payload"  value="$(arg min_payload)"/>
   <arg name="max_payload"  value="$(arg max_payload)"/>
   <arg name="prefix" value="$(arg prefix)"/>
  </include>
 </group>

 <group ns="right">
   <arg name="limited" default="false"/>
   <arg name="robot_ip" default="192.168.0.109"/>    
   <arg name="reverse_port" default="50002"/>    
   <arg name="min_payload"  default="0.0"/>
   <arg name="max_payload"  default="5.0"/>
   <arg name="prefix" default="right_" />
  <include file="$(find ur_bringup)/launch/ur5_bringup.launch">
   <arg name="limited" value="$(arg limited)"/>
   <arg name="robot_ip" value="$(arg robot_ip)"/>
   <arg name="reverse_port" value="$(arg reverse_port)"/>
   <arg name="min_payload"  value="$(arg min_payload)"/>
   <arg name="max_payload"  value="$(arg max_payload)"/>
   <arg name="prefix" value="$(arg prefix)"/>
  </include>
 </group>

</launch>

controllers.yaml in moveit_config package:

controller_list:

  - name: "left"
    action_ns: left_follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - left_shoulder_pan_joint
      - left_shoulder_lift_joint
      - left_elbow_joint
      - left_wrist_1_joint
      - left_wrist_2_joint
      - left_wrist_3_joint

  - name: "right"
    action_ns: right_follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - right_shoulder_pan_joint
      - right_shoulder_lift_joint
      - right_elbow_joint
      - right_wrist_1_joint
      - right_wrist_2_joint
      - right_wrist_3_joint

The warning in the terminal after runnning the python file:

[ INFO] [1508121442.292992289]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1508121442.293298977]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1508121442.293327316]: No planner specified. Using default.
[ INFO] [1508121442.293420818]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1508121442.314419323]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1508121442.314442216]: Solution found in 0.021074 seconds
[ INFO] [1508121442.314505058]: SimpleSetup: Path simplification took 0.000045 seconds and changed from 3 to 2 states
[ INFO] [1508121442.317369505]: Received new trajectory execution service request...
***[ WARN] [1508121443.319273668]: Failed to validate trajectory: couldn't receive full current joint state within 1s
[ INFO] [1508121443.319330948]: Execution `enter code here`completed: ABORTED***

ur5_bringup.launch:

<launch>

  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
  <arg name="robot_ip"/>
  <arg name="port" default="50001"/>
  <arg name="limited" default="false"/>
  <arg name="min_payload"  default="0.0"/>
  <arg name="max_payload"  default="5.0"/>
  <arg name="prefix" default="" />
  <!-- robot model 
  <include file="$(find ur_description)/launch/ur5_upload.launch">
    <arg name="limited" value="$(arg limited)"/>
  </include>-->

  <!-- ur common -->
  <include file="$(find ur_modern_driver)/launch/ur_common.launch">
    <arg name="prefix"  value="$(arg prefix)" />
    <arg name="robot_ip" value="$(arg robot_ip)"/>
    <arg name="port" value="$(arg port)"/>
    <arg name="min_payload"  value="$(arg min_payload)"/>
    <arg name="max_payload"  value="$(arg max_payload)"/>
  </include>

</launch>

ur_common.launch:

<launch>
  <!-- robot_ip: IP-address of the robot's socket-messaging server ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2017-10-19 02:40:22 -0500

gvdhoorn gravatar image

updated 2017-10-19 02:42:17 -0500

[ WARN] [1508121443.319273668]: Failed to validate trajectory: couldn't receive full current joint state within 1s.

You don't show any launch files - so it's hard to know for certain, but it sounds like the JointState msgs from your driver instances are not reaching MoveIt / the MoveIt commander.

MoveIt will try to make sure that the trajectory that you want to execte actually starts at the current state of the robot (to avoid large sudden movements). For that it needs JointState messages, or it can't determine current state. Your two ur_driver instances should be publishing in the left and right namespaces, so there is no global /joint_states topic for MoveIt to listen to.

Easiest is probably to use a joint_state_publisher instance with the source_list parameter to publish an aggregate message on /joint_states. See moveit_resources/fanuc_moveit_config/launch/demo.launch for an example, and wiki/joint_state_publisher - Node API - parameters for info on source_list.

edit flag offensive delete link more

Comments

I have added two launch files.The /joint_states message is established in ur_common.launch:

<remap from="joint_states" to="$(arg prefix)joint_states"/>
yyf gravatar image yyf  ( 2017-10-19 02:58:32 -0500 )edit

The value of prefix is set to "left_" and "right_". So I would expect two topics: left_joint_states and right_joint_states. That is not /joint_states that MoveIt will be looking for.

gvdhoorn gravatar image gvdhoorn  ( 2017-10-19 03:00:30 -0500 )edit

Also: I don't think a remap is necessary. The topics should be published under /left/joint_states and /right/joint_states automatically. The remap confuses me.

gvdhoorn gravatar image gvdhoorn  ( 2017-10-19 03:01:29 -0500 )edit

Thank you very much!The problem is /left/joint_states was not connected with moveit!

yyf gravatar image yyf  ( 2017-10-30 06:35:12 -0500 )edit

hello! I actually have the same problem. I am trying to control an UR5 and UR10 with move_group_python_interface. Can you please add your ur_moveit_planning_trajectory_execution.launch? or did you use the one from the ur5_moveit_config package? did you also use move_group.launch in a group ns?

Soul Goumey gravatar image Soul Goumey  ( 2018-09-04 18:46:04 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2017-10-18 21:36:59 -0500

Seen: 426 times

Last updated: Oct 19 '17