Gazebo teleport robot via set_model_state in 0-gravity only moves part of robot

asked 2015-11-05 17:31:18 -0600

mz gravatar image

updated 2015-11-06 00:24:25 -0600

Gazebo 2.2.6 with ROS Indigo.

My goal is to teleport a robot hand (without a robot body attached) to arbitrary places in the world, including in mid-air, without the hand falling to the ground. Therefore I need simulation in 0 gravity.

The problem I’m having is, as soon as I set gravity to 0, I can no longer teleport the hand correctly by set_model_state. Part of the hand (all the fingers) moves to the commanded position, and the rest of the hand (the palm) remains in original place.

The condition to produce this:

  1. Set gravity to 0 (rosservice call set_physics_properties). Right after I set gravity to 0, rosservice call get_model_state on the hand shows all 6 twist values being nan.

  2. Now rosservice call set_model_state, then the fingers move to the right place, but the palm doesn’t. So I’d see a hand that’s broken apart, with fingers in one place, palm in original place.

  3. Then any subsequent calls to set_model_state would produce very odd behavior - the fingers move in relative position, rather than absolute, and the palm remains unmoved. e.g. issuing 0.5 0 0 position again and again would move fingers farther and farther from the palm. (Expected behavior is that the fingers move to absolute world coordinates 0.5 0 0, not keep going farther).

Things I've tried:

If I don’t turn off gravity, the full hand would move correctly. Twist values are never nan. Issuing subsequent commands like 0.5 0 0 many times just makes the hand remain at absolute coordinates 0.5 0 0, as expected. The obvious problem is, if I move the hand to mid-air, it falls to the ground. So I need 0 gravity simulation.

If I pause physics during the set_model_state call, the palm and the fingers would appear to have moved together, but once I unpause, it shows they are apart.

This only happens with specific poses, the one that always fails with 0 gravity is position 0.5 0 0, orientation 0 0 0 1. Position 0 0 0.5 moves the whole hand correctly, 1 0 0 too. I need to be able to set the hand to arbitrary poses, so some working and others don’t is not enough.

Resetting simulation (rosservice call /gazebo/reset_simulation) doesn’t make the twist values normal, they remain nan from the moment I turn off gravity.

I also checked my URDF to make sure the joint types are correct. The connection between palm and fingers is revolute, which is needed to move the fingers. The root link, I first had it as a floating type joint, attached to world link. Then I removed the world link and the joint altogether. In either case, the same problem above happens.

It appears to be a problem with gravity being turned off?

Is there anyway to instantly move the hand to arbitrary poses, with the poses specified as 7-tuples (tx ty tz qx qy qz qw ... (more)

edit retag flag offensive close merge delete


This combination seems to be working: Set gravity to 0 using the Gazebo WorldPlugin, _world->GetPhysicsEngine()->SetGravity(). Then move the robot model using set_model_state. This seems tricky. Doing both using one method doesn't work, must use this two-method combination.

mz gravatar image mz  ( 2015-11-06 00:49:12 -0600 )edit