MoveIt! commander with youbot

edit

Hello MoveIt! Commander enthusiasts. We are having problems getting our youbot to move properly in simulation with gazebo. Our architecture is as follows:

MoveIt Commander ==> python_script ==> gazebo (headless) ==> rviz

We use Moveit Commander to select a named pose stored in the SRDF and then compute a plan. We then publish that plan to the youbot topic called "/arm_1/arm_controller/command". This topic works great with sending messages using the rostopic pub command with all joint positions set to valid values and velocity, acceleration, and effort set to zero. We can publish a JointTrajectory (JT) message with a single JointTrajectoryPoint (JTP) at 10 Hz and the robot moves just fine in gazebo.

When publishing from our Python script, the velocity, acceleration, and efforts are all populated. We publish a valid JT with more than one JTP computed by the MoveIt Commander plan() function at the same rate, and the robot does not move 1) unless we publish at least 3 times, and 2) as the robot moves, it appears to jerk to and from the starting position. Presumably this is because I have published the JT command three times and the LIFO design of ROS subscriber/publisher queuing causes the robot to get confused until the final JT command is received by gazebo.

Our python code is as follows. I apologize ahead of time for the look of our code, but we are just prototyping as this stage.

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 

from std_msgs.msg import String

def get_random_waypoints(move_group=[], n=2):
    ''' Get a series of waypoints with the current pose first'''
    geom_points = []
    pose = move_group.get_current_pose().pose
    geom_points.append(copy.deepcopy(pose))
    try:
        for ii in range(0,n):
            pose = move_group.get_random_pose().pose
            geom_points.append(copy.deepcopy(pose))
        return copy.deepcopy(geom_points)    
    except MoveItCommanderException:
        print "get_random_waypoints failed"
        return False


def make_trajectory_msg(joint_trajectory_plan=[], seq=0, secs=0, nsecs=0, dt=2, frame_id='/base_link'):
    jt = JointTrajectory()
    jt.header.seq = seq
    jt.header.stamp.secs = 0 #secs
    jt.header.stamp.nsecs = 0 #nsecs
    jt.header.frame_id = frame_id
    jt.joint_names = joint_trajectory_plan.joint_trajectory.joint_names
    njtp = len(joint_trajectory_plan.joint_trajectory.points)
    for ii in range(0, njtp):
        jtp = JointTrajectoryPoint()
        jtp = copy.deepcopy(joint_trajectory_plan.joint_trajectory.points[ii])
        jtp.time_from_start.secs = secs + dt*(ii+1)
        jtp.time_from_start.nsecs = nsecs
        jt.points.append(jtp)
    return jt

def move_robot():

  print "============ Starting tutorial setup"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('my_moveit_client',
                  anonymous=False)

  ## Instantiate a RobotCommander object. This object is an interface to
  ## the robot as a whole. 
  robot = moveit_commander.RobotCommander()

  ## Instantiate a PlanningSceneInterface object. This object is an interface
  ## to the world surrounding the robot.
  scene = moveit_commander.PlanningSceneInterface()

  ## Instantiate a MoveGroupCommander object. This object is an interface
  ## to one group of joints. In this case the group is the joints in the left
  ## arm. This interface can be used to plan and execute motions on the left
  ## arm.
  print "Creating move group commander object"
  group = moveit_commander.MoveGroupCommander("manipulator")

  ## We create this DisplayTrajectory ...