Teleporting robot via 'rosservice call /gazebo/set_model_state' produces weird behaviour

edit

Trying with the pioneer2dx_camera.world confirms that there is a bug with setting pose of nested models in diamondback and electric. The problem is due to a bug in the recursive updates in Entity.cc. Ticketed.

Current work around is to combine nested models into a single model. For example, pioneer2dx_camera.world becomes:

<?xml version="1.0"?>

<gazebo:world 
  xmlns:xi="http://www.w3.org/2001/XInclude"
  xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" 
  xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" 
  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
  xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" 
  xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" 
  xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" 
  xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" 
  xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" 
  xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" 
  xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
  xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" 
  xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
  xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >

  <verbosity>5</verbosity>

  <physics:ode>
    <stepTime>0.001</stepTime>
    <gravity>0 0 0</gravity>
    <cfm>0.0000000001</cfm>
    <erp>0.2</erp>

    <stepType>quick</stepType>
    <stepIters>100</stepIters>
    <stepW>1.3</stepW>
    <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
    <contactSurfaceLayer>0.00</contactSurfaceLayer>
  </physics:ode>

  <rendering:gui>
    <type>fltk</type>
    <size>800 600</size>
    <updateRate>20</updateRate>
    <pos>0 0</pos>

    <frames>
      <row height="100%">
        <camera width="100%">
          <xyz>0.35 -0.5 0.43</xyz>
          <rpy>0 14 111.5</rpy>
        </camera>
      </row>
    </frames>
  </rendering:gui>

  <rendering:ogre>
    <ambient>0.5 0.5 0.5 1.0</ambient>
  </rendering:ogre>

   <!-- Ground Plane -->
  <model:physical name="plane1_model">
    <xyz>0 0 0</xyz>
    <rpy>0 0 0</rpy>
    <static>true</static>

    <body:plane name="plane1_body">
      <geom:plane name="plane1_geom">
        <normal>0 0 1</normal>
        <size>2000 2000</size>
        <segments>10 10</segments>
        <uvTile>100 100</uvTile>
        <material>Gazebo/Grey</material>
      </geom:plane>
    </body:plane>
  </model:physical>

  <model:physical name="cylinder1_model">
    <xyz>1 -1.5 0.5</xyz>
    <rpy>0.0 0.0 0.0</rpy>

    <body:cylinder name="cylinder1_body">
      <massMatrix>true</massMatrix> 
      <mass>1.0</mass>
      <ixx>0.01</ixx>
      <ixy>0.00</ixy>
      <ixz>0.00</ixz>
      <iyy>0.01</iyy>
      <iyz>0.00</iyz>
      <izz>0.01</izz>
      <cx>0.0</cx>
      <cy>0.0</cy>
      <cz>0.0</cz>
      <geom:cylinder name="cylinder1_geom">
        <size>0.5 1</size>
        <visual>
          <mesh>unit_cylinder</mesh>
          <material>Gazebo/RustyBarrel</material>
        </visual>
      </geom:cylinder>
    </body:cylinder>
  </model:physical>

  <model:physical name="pioneer2dx_model1">
    <xyz>0 0 0.148</xyz>
    <rpy>0.0 0.0 0.0</rpy>

  <body:box name="sonyvid30_body">
    <xyz>0.15 0.0 0.09</xyz>
    <rpy>0.0 0.0 0.0</rpy>

    <massMatrix>true</massMatrix> 
    <mass>0.1</mass>
    <ixx>0.01</ixx>
    <ixy>0.00</ixy>
    <ixz>0.00</ixz>
    <iyy>0.01</iyy>
    <iyz>0.00</iyz>
    <izz>0.01</izz>
    <cx>0.0</cx>
    <cy ...