Ask Your Question
0

MoveIt! commander with youbot

asked 2014-06-25 09:49:55 -0500

Rick C NIST gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-11-24 08:55:43 -0500

est_CEAR gravatar image

updated 2014-11-24 08:56:22 -0500

  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"
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2014-06-25 09:49:55 -0500

Seen: 1,318 times

Last updated: Nov 24 '14