Execute Trajectory using MoveIt!

[Ulysse]

Hi all,

I am facing an issue trying to use the MoveIt! package to control the Sawyer.

I followed all tutorials on how to set up ROS Kinetic, the Sawyer SDK packages and the MoveIt! package.

I am know trying to work my way through the MoveIt tutorial.
I am able to successfully load the robot inside Rviz, move the end effector in the interface and plan a trajectory.
However when I try to execute it, Rviz shows a "failed" message and in the terminal output I can read :

Failed to validate trajectory: couldn't receive full current joint state within 1s

I also tried to use MoveIt! through the Python interface but when I execute the following :

import sys                                                                                                      
import copy                                                            
import rospy                                            
import moveit_commander      
import moveit_msgs.msg                   
import geometry_msgs.msg       
from std_msgs.msg import String

moveit_commander.roscpp_initialize(sys.argv)                                                                    
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

robot = moveit_commander.RobotCommander()                                                                       
scene = moveit_commander.PlanningSceneInterface()                      
group = moveit_commander.MoveGroupCommander("right_arm")

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)

print "============ Reference frame: %s" % group.get_planning_frame()                                           
print "============ End effector: %s" % group.get_end_effector_link()  
print "============ Robot Groups:"                      
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"

... I get something like :

============ Reference frame: /base
============ End effector: right_hand
============ Robot Groups:
['right_arm']
============ Printing robot state
[ WARN] [1526377733.002742750]: Joint values for monitored state are requested but the full state is not known
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/base"
  name: [right_j0, head_pan, right_j1, right_j2, right_j3, right_j4, right_j5, right_j6]
  position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/base"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False
============

This is basically the same problem as in this ROS Q&A question : MoveIt: troubles with joint values
Unfortunately this question did not get any useful answer.

The weird thing is that the joint state is actually published, when I echo the /robot/joint_states topic I can see the values being updated :

header: 
  seq: 600867
  stamp: 
    secs: 1526364559
    nsecs: 890697828
  frame_id: ''
name: [head_pan, right_j0, right_j1, right_j2, right_j3, right_j4, right_j5, right_j6, torso_t0]
position: [-4.0742021484375, -0.2243544921875, -1.1203125, 0.4582724609375, 2.05134375, -0.29490625, 0.69954296875, -4.114162109375, 0.0]
velocity: [-0.001, -0.001, -0.001, -0.001, -0.001, -0.001, -0.001, -0.001, 0.0]
effort: [-0.012, 0.052, -20.068, -1.06, -5.996, 0.572, 0.104, -0.284, 0.0]

And when I query informations about the /robot/joint_states topic, I can see that both the /realtime_loop and /move_group nodes are publishing/subscribing to it :

Type: sensor_msgs/JointState

Publishers: 
 * /realtime_loop (http://192.168.212.60:42358/)

Subscribers: 
 * /robot_state_publisher (http://192.168.212.60:38760/)
 * /robot/environment_server (http://192.168.212.60:59368/)
 * /calibrate_arm (http://192.168.212.60:57953/)
 * /PositionKinematicsNode_right (http://192.168.212.60:56399/)
 * /JointMotionPathNode_right (http://192.168.212.60:58612/)
 * /motion_controller (http://192.168.212.60:37187/)
 * /sdk_position_w_id_joint_trajectory_action_server_right (http://192.168.212.160:37229/)
 * /move_group (http://192.168.212.160:43623/)

Any idea of what is going on or how I can further investigate the error ?

Thanks for your help.

[Ian McMahon]

Hi Ulysse,

I have a couple pointers that you may find helpful:
 

Quote

 

However when I try to execute it, Rviz shows a "failed" message and in the terminal output I can read :

Failed to validate trajectory: couldn't receive full current joint state within 1s

 

This could have two possible causes. The first can be caused by attempting to control your Sawyer from a laptop connected over Wifi. You can remedy this by using an ethernet cable and a router for communication between robot and computer. The other is that you have the electric gripper plugged into the robot, but have not told MoveIt to be expecting an extra joint. You can do this by supplying electric_gripper:=true when launching sawyer in MoveIt:

roslaunch sawyer_moveit_config sawyer_moveit.launch electric_gripper:=true

 

The zero joint states issue is likely caused by not being able to find the joint_states topic. When launching your own MoveIt node, you must ensure the joint_states topic is properly remapped for Sawyer's /robot namespace:

rosrun my_custom_moveit_pkg my_custom_moveit_node joint_states:=/robot/joint_states

 

Hope this helps!

~ Ian

[Ulysse]

Thanks for your help Ian.

Unfortunately I am not in any of the cases you spotted :

• My laptop and the robot are connected to the same network via ethernet.
• I am not using any tool on the robot (only the camera on the hand).
• I can see using rqt_graph that both the realtime_loop and moveit nodes are connected to the same /robot/joint_states topic.

However in the meantime I kept investigating and I think I found where the problem comes from : my laptop clock and the robot clock are completely out of sync (by about 5 hours).
The logs are not very explicit but I don't think MoveIt! is complaining about not receiving data, I think it is complaining about not receiving RECENT data (because  obviously 5 hours > 1 second).
The solution might then be to set a list of valid NTP servers in the FSM (which I am not able to test because my sawyer is not connected to the Internet).

[Ian McMahon]

You're definitely on the right track. Without connecting the robot and your computer to the Internet, you'll need to configure your computer to act as the robot's NTP absolute time source, with a stratum less than 16. We have a tutorial to help out with this on the wiki:

http://sdk.rethinkrobotics.com/intera/Time_and_NTP#Configuration

Then add your workstation to Sawyer's FSM NTP timeserver field: http://sdk.rethinkrobotics.com/intera/Field_Service_Menu_(FSM)

Then add your computer Feel free to ask if anything in the tutorial is unclear. Hope this helps!

~ Ian

[Ulysse]

It works! At least some part of it does.

For some reason I wasn't able to set up a local NTP server but I found a workaround using the faketime library.

So now I am able to plan a trajectory using the Rviz GUI and have it executed by the robot.
However I am still out of luck for the MoveIt! python interface tutorial. Everything goes fine until I reach the line:

print robot.get_current_state()

Then I get the same result as before:

[ WARN] [1527149047.023091673]: Joint values for monitored state are requested but the full state is not known
joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/base"
  name: [right_j0, head_pan, right_j1, right_j2, right_j3, right_j4, right_j5, right_j6]
  position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "/base"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False

I don't know if this might be an issue but in the above there is an "head_pan" joint that doesn't get fed by the "/robot/joint_states" topic.