01153       // Get inertial rates
    01154       gazebo::Vector3 body_vpos = body->GetWorldLinearVel(); // get velocity in gazebo frame
    01155       gazebo::Vector3 body_veul = body->GetWorldAngularVel(); // get velocity in gazebo frame
    01156 
    01157       if (frame)
    01158       {
    01159         // convert to relative pose
    01160         gazebo::Pose3d frame_pose = frame->GetWorldPose();
    01161         body_pose.pos = body_pose.pos - frame_pose.pos;
    01162         body_pose.pos = frame_pose.rot.RotateVectorReverse(body_pose.pos);
    01163         body_pose.rot *= frame_pose.rot.GetInverse();
    01164 
    01165         // convert to relative rates
    01166         gazebo::Vector3 frame_vpos = frame->GetWorldLinearVel(); // get velocity in gazebo frame
    01167         gazebo::Vector3 frame_veul = frame->GetWorldAngularVel(); // get velocity in gazebo frame
    01168         body_vpos = frame_pose.rot.RotateVector(body_vpos - frame_vpos);
    01169         body_veul = frame_pose.rot.RotateVector(body_veul - frame_veul);
    01170       }
    01172       else if (req.[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateRequest__.html#a83af84cb417fc8e9297c12f514a2be53) == "" || req.[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateRequest__.html#a83af84cb417fc8e9297c12f514a2be53) == "world" || req.[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateRequest__.html#a83af84cb417fc8e9297c12f514a2be53) == "map" || req.[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateRequest__.html#a83af84cb417fc8e9297c12f514a2be53) == "/map")
    01173       {
    01174           [ROS_DEBUG](../../../api/rosconsole/html/macros__generated_8h.html#a83228c0b42c80605abdd0f42af64b1f7)("GetLinkState: reference_frame is empty/world/map, using inertial frame");
    01175       }
    01176       else
    01177       {
    01178         res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#ab078790cbd3b85968631041914f263eb) = false;
    01179         res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#ae6630d81e448f045256efffe68e5b7bd) = "GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?";
    01180         return false;
    01181       }
    01182 
    01183       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[link_name](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#afc22e7a217cbc73817640680884df2bd) = req.[link_name](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateRequest__.html#a2c698b3773b198139f234c5c0a3d9dbb);
    01184       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#acd9420e6916084b2a9c09019bfda1708) = body_pose.pos.x;
    01185       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a46a84ff5a3e5817ab5693abda66fccb0) = body_pose.pos.y;
    01186       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[position](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#a27067d1772d85a856b0e4ed4690a80b4).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a061f0093e927a95998f35ceceb5cebda) = body_pose.pos.z;
    01187       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a11f3b4e2df587ce516fd2a8140c1c7c5) = body_pose.rot.x;
    01188       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a601c978178311ce3a618afb38c0b6b98) = body_pose.rot.y;
    01189       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#ae2caceb9fc4b2fd01acd8419aa7ebc2e) = body_pose.rot.z;
    01190       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[pose](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#ae83d49b974df6e08f92c3e1c8870507b).[orientation](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Pose__.html#af8c1d34a44a7628ea9c3e571339f5a28).[w](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Quaternion__.html#a440791d268a65607ecbac21e0ed153cf) = body_pose.rot.u;
    01191       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be) = body_vpos.x;
    01192       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404) = body_vpos.y;
    01193       res.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1GetLinkStateResponse__.html#aa83952af428b1a3732e0e19cc2a7353e).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[linear](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#a394500332ac20028528c37103e1bf8e5).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1) = body_vpos.z;