0
$\begingroup$

Rosanswers logo

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 publisher which is used below to publish
  ## trajectories for RVIZ to visualize.
  display_trajectory_publisher = rospy.Publisher(
                                    '/move_group/display_planned_path',
                                    moveit_msgs.msg.DisplayTrajectory)
  
  ## Create a publisher to gazebo arm controller
  gazebo_command_publisher = rospy.Publisher(
                                      '/arm_1/arm_controller/command',
                                      JointTrajectory,
                                      queue_size=10 ) 
  
  # setup the loop rate for the node
  r = rospy.Rate(10) # 10hz
  
  ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
  print "============ Waiting for RVIZ..."
  #rospy.sleep(10)
  print "============ Starting tutorial "

  ## Getting Basic Information
  ## ^^^^^^^^^^^^^^^^^^^^^^^^^
  ##
  ## We can get the name of the reference frame for this robot
  print "============ Reference frame: %s" % group.get_planning_frame()
  
  ## We can also print the name of the end-effector link for this group
  print "============ Reference frame: %s" % group.get_end_effector_link()

  ## We can get a list of all the groups in the robot
  print "============ Robot Groups:"
  print robot.get_group_names()

  ## Sometimes for debugging it is useful to print the entire state of the
  ## robot.
  print "============ Printing robot state"
  print robot.get_current_state()
  print "============"
 
  print "+++++++++++++++++++ selecting waypoints/pose"
  end_effector_link = "gripper_eef"
  group.set_named_target("gun")
  plan = group.plan()

  ## We want the cartesian path to be interpolated at a resolution of 1 cm
  ## which is why we will specify 0.01 as the eef_step in cartesian
  ## translation. We will specify the jump threshold as 0.0, effectively
  ## disabling it.
    
  jt = make_trajectory_msg(joint_trajectory_plan=plan, dt=0.2, frame_id='base_link' )
  print "========= Here be the joint trajectory\n"
  print "JTP length is " + str(len(jt.points)) + "\n"
  print jt 
    
  print "My group object is\n"
  print group
  print "\n"

  sent = 3
  while not rospy.is_shutdown():
      if sent > 0:
          sys.stdout.write('.')
          sys.stdout.flush()
          gazebo_command_publisher.publish(jt)
          sent -= 1
      else:
          sys.stdout.write('x')
          sys.stdout.flush()
      r.sleep()


  ## When finished shut down moveit_commander.
  moveit_commander.roscpp_shutdown()


if __name__=='__main__':
  try:
    move_robot()
    
  except rospy.ROSInterruptException:
    pass

We are obviously doing something wrong, but cannot explain it. Please provide us guidance on how to make the robot move smoothly with a single JT message instead of three. Any other helpful hints would be great. Thanks!


Originally posted by Rick C NIST on ROS Answers with karma: 28 on 2014-06-25

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

  1. You have to publish 3 times : Increase the delay in your MoveIt kinematic parameters -> kinematics_solver_timeout

  2. As the robot moves, it appears to jerk to and from the starting position : In Rviz parameters,

  • add RobotModel
  • MotionPlanning -> Scene Robot : uncheck "Show Robot Visual"
  • MotionPlanning -> Planning Request : uncheck "Query Start State"

Originally posted by est_CEAR with karma: 170 on 2014-11-24

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.