Gazebo teleport robot via set_model_state in 0-gravity only moves part of robot
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:
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 beingnan
.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.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 ...
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.