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 setmodelstate. 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)?
I tried using Gazebo WorldPlugin to get access to a link and doing SetWorldPose() upon receiving a rosmsg, but setting like that just made the hand fly out. I read the pages on Gazebo plugins many times, and the Gazebo Plugins API, but I cannot find one that lets me do this by a 7-tuple. I do not know how to get the hand to an exact pose by changing the velocity or force. If there’s a way to translate directly from a 7-tuple to a velocity / force on the root link, that could help too. But I do need it to move instantly, otherwise I could just attach the hand to a robot body, which defeats the purpose of this simulation, because the robot IK movements just take forever. (maybe a remedy is to speed up physics steps? But a real solution with hand alone would be much preferred).
I saw a related post here, but it is not the same and doesn't solve my problem http://answers.ros.org/question/11134/teleporting-robot-via-rosservice-call-gazeboset_model_state-produces-weird-behaviour/
Thank you so much.
Asked by mz on 2015-11-05 18:31:18 UTC
Comments
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.
Asked by mz on 2015-11-06 01:49:12 UTC