00940           // convert to relative pose
    00941           gazebo::Pose3d frame_pose = frame->GetWorldPose();
    00942           model_pos = model_pos - frame_pose.pos;
    00943           model_pos = frame_pose.rot.RotateVectorReverse(model_pos);
    00944           model_rot *= frame_pose.rot.GetInverse();
    00945 
    00946           // convert to relative rates
    00947           gazebo::Vector3 frame_vpos = frame->GetWorldLinearVel(); // get velocity in gazebo frame
    00948           gazebo::Vector3 frame_veul = frame->GetWorldAngularVel(); // get velocity in gazebo frame
    00949           model_linear_vel = frame_pose.rot.RotateVector(model_linear_vel - frame_vpos);
    00950           model_angular_vel = frame_pose.rot.RotateVector(model_angular_vel - frame_veul);
    00951         }
    00953         else if (req.[relative_entity_name](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateRequest__.html#a4b15abcdd6f57dd69eaa959f25642509) == "" || req.[relative_entity_name](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateRequest__.html#a4b15abcdd6f57dd69eaa959f25642509) == "world" || req.[relative_entity_name](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateRequest__.html#a4b15abcdd6f57dd69eaa959f25642509) == "map" || req.[relative_entity_name](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateRequest__.html#a4b15abcdd6f57dd69eaa959f25642509) == "/map")
    00954         {
    00955             [ROS_DEBUG](../../../api/rosconsole/html/macros__generated_8h.html#a83228c0b42c80605abdd0f42af64b1f7)("GetModelState: relative_entity_name is empty/world/map, using inertial frame");
    00956         }
    00957         else
    00958         {
    00959           res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#aeb58e60c2f46b69ad7ff582169ae3b6c) = false;
    00960           res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a2ac60b79617ca44e69200efbafe6d3ee) = "GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?";
    00961           return false;
    00962         }
    00963 
    00964         // fill in response
    00965         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#acd9420e6916084b2a9c09019bfda1708) = model_pos.x;
    00966         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a46a84ff5a3e5817ab5693abda66fccb0) = model_pos.y;
    00967         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a061f0093e927a95998f35ceceb5cebda) = model_pos.z;
    00968         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[w](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a440791d268a65607ecbac21e0ed153cf) = model_rot.u;
    00969         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a11f3b4e2df587ce516fd2a8140c1c7c5) = model_rot.x;
    00970         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a601c978178311ce3a618afb38c0b6b98) = model_rot.y;
    00971         res.[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a0256f03b0e119f8d35d0b080bdb1a86e).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#ae2caceb9fc4b2fd01acd8419aa7ebc2e) = model_rot.z;
    00972 
    00973         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be) = model_linear_vel.x;
    00974         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404) = model_linear_vel.y;
    00975         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1) = model_linear_vel.z;
    00976         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be) = model_angular_vel.x;
    00977         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404) = model_angular_vel.y;
    00978         res.[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#a36b1535a3a2572860b77a9f719cfb11a).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1) = model_angular_vel.z;
    00979 
    00980         res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1GetModelStateResponse__.html#aeb58e60c2f46b69ad7ff582169ae3b6c) = true;