Ask Your Question
0

Gazebo and PR2 simulation setup publishing nan values on base_odometry/odom

asked 2012-07-26 05:06:59 -0500

EmbeddedSystem gravatar image

updated 2014-01-28 17:13:09 -0500

ngrennan gravatar image

Some background: Essentially I have set up pr2 in simulation in an empty world on gazebo.

I would like to set the initial pose of the robot, so I have done the following:

model_state = gazebo.msg.ModelState()
model_state.model_name = 'pr2'
model_state.pose.position.x = map2lab[0]
model_state.pose.position.y = map2lab[1]

rospy.wait_for_service('/gazebo/set_model_state')
try:
    sms = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
    print 'Model State: ' + str(model_state)
    sms(model_state)
except rospy.ServiceException, e:
    print "Service call failed: %s"%e

At this point, I'm not sure if it has broken yet, but I then would also like to move the robot around, so I set up the following:

self.pub = rospy.Publisher('/base_controller/command', Twist)
rospy.init_node('locomotionCommand')

Which later I use to publish something like the following (in which v and w are always real numbers):

twist = Twist()
twist.linear.x = v
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = w
self.pub.publish(twist)

So sometime in here, the pr2 jumps and disappears and I get the following error repeated at some high frequency:

[ERROR] [1343312019.823059305, 40.582000000]: TF_NAN_INPUT: Ignoring transform 
for child_frame_id "/wheelodom" from authority "default_authority" because of a 
nan value in the transform (nan nan nan) (-nan -nan -nan -nan)

I'm fairly certain it is only on /base_odometry/odom, here's a sample:

header:
  seq: 5320
  stamp:
    secs: 58
    nsecs: 971000000
  frame_id: odom
child_frame_id: ''
pose:
  pose:
    position:
      x: nan
      y: nan
      z: 0.0
    orientation:
      x: nan
      y: nan
      z: nan
      w: nan
  covariance: [4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-06, 0.0, 0.0, 0.0, 0.0,     0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,         1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00028900000000000003]
twist:
  twist:
    linear:
      x: 2.2798345089
      y: 2.02649641037
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.20633020997
  covariance: [4e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4e-06, 0.0, 0.0, 0.0, 0.0,     0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00028900000000000003]

I'm at a loss to figure out what is going on here.

edit retag flag offensive close merge delete

Comments

Other important info I forgot to mention: Ubuntu 12.04, Ros Fuerte, all up to date.

EmbeddedSystem gravatar image EmbeddedSystem  ( 2012-07-26 05:21:24 -0500 )edit

I'm also getting this error: Could not transform imu message from torso_lift_link to base_footprint But I'm not sure that is as relevant to my problem? or maybe it is?

EmbeddedSystem gravatar image EmbeddedSystem  ( 2012-07-26 05:41:59 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
2

answered 2012-07-26 09:46:21 -0500

rtoris288 gravatar image

What's happening is your robot model is "exploding": see here, here, and here for what I mean by exploding.

Essentially, what's happening is the model (the PR2 in your case) is colliding with something (either itself or the plane since its and empty world) and the physics engine is not updating fast enough to catch the collision. The models wind up slightly inside of each other and, in an effort to fix the collision, the physics engine pushes the two models away from each other (sometimes at VERY high speeds making the mode "explode). That explains why the PR2 is disappearing. When this happens, the physics engine essentially stop working properly and causes the error message you are seeing. Try modifying your world file with the parameters defined here. In particular, contact_max_correcting_vel defaults at 100 and can be lowered to a smaller value (e.g., 1) to slow down this violent collision correction. You can also update the update rate of the physics engine (iters).

edit flag offensive delete link more
0

answered 2012-07-27 12:48:44 -0500

hsu gravatar image

The lack of orientation in your service request causes SetModelState to put all links at the same location (overlapping each other), this is definitely a bug, but you can add the following definition to avoid this behavior:

model_state = gazebo.msg.ModelState()
model_state.model_name = 'pr2'
model_state.pose.position.x = 1
model_state.pose.position.y = 2
model_state.pose.position.z = 0
model_state.pose.orientation.x = 0
model_state.pose.orientation.y = 0
model_state.pose.orientation.z = 0
model_state.pose.orientation.w = 1

rospy.wait_for_service('/gazebo/set_model_state')
try:
    sms = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
    print 'Model State: ' + str(model_state)
    sms(model_state)
except rospy.ServiceException, e:
    print "Service call failed: %s"%e

By the way, the recommended message to use is in gazebo_msgs package as I've reference above.

edit flag offensive delete link more
0

answered 2012-07-27 05:32:43 -0500

EmbeddedSystem gravatar image

So, given this solution, it works better, but the robot still doesn't like the whole 'teleporting' thing. It seems as if it rotates and reconfigures during the reassignment of the pose.

This is not optimal, is there a better way to set the initial pose in relation to the plane?

edit flag offensive delete link more

Comments

The initial pose can be set prior to launching the simulator with the ROBOT_INITIAL_POSE environment variable. For example: export ROBOT_INITIAL_POSE="-x 1 -y 0.5 -Y 1.57"

rtoris288 gravatar image rtoris288  ( 2012-07-27 11:21:44 -0500 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2012-07-26 05:06:59 -0500

Seen: 1,582 times

Last updated: Jul 27 '12