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!
↧