01826 
    [01829](classGazeboROSNode.html#a885a6f43b990036c7625cf50a06113fb)     bool [setLinkState](classGazeboROSNode.html#a885a6f43b990036c7625cf50a06113fb)([gazebo_msgs::SetLinkState::Request](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html) &req,[gazebo_msgs::SetLinkState::Response](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateResponse__.html) &res)
    01830     {
    01831       gazebo::Body* body = dynamic_cast<gazebo::Body*>(gazebo::World::Instance()->GetEntityByName(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[link_name](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#afc22e7a217cbc73817640680884df2bd)));
    01832       gazebo::Body* frame = dynamic_cast<gazebo::Body*>(gazebo::World::Instance()->GetEntityByName(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#af3c75584cb85e20e4bac931829d8a80e)));
    01833       if (!body)
    01834       {
    01835         [ROS_ERROR](../../../api/rosconsole/html/macros__generated_8h.html#a7fb635b03cc608284eadecd2f8becd9a)("SetLinkState: link [%s] does not exist",req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[link_name](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#afc22e7a217cbc73817640680884df2bd).c_str());
    01836         res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateResponse__.html#a9ddd44a499fd3f5e117637729474e969) = false;
    01837         res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateResponse__.html#a599d42bb7063ff3f9139767cd62f3c10) = "SetLinkState: link does not exist";
    01838         return false;
    01839       }
    01840 
    01842       // get reference frame (body/model(link)) pose and
    01843       // transform target pose to absolute world frame
    01844       gazebo::Vector3 target_pos(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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));
    01845       gazebo::Quatern target_rot(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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));
    01846       gazebo::Pose3d target_pose(target_pos,target_rot);
    01847       gazebo::Vector3 target_linear_vel(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[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));
    01848       gazebo::Vector3 target_angular_vel(req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404),req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[twist](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#aacdf4d3b28003c05951526d13a1c57f8).[angular](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Twist__.html#aa0cb518e509179290aa793228ef01f6b).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1));
    01849 
    01850       if (frame)
    01851       {
    01852         gazebo::Pose3d  frame_pose = frame->GetWorldPose(); // - this->myBody->GetCoMPose();
    01853         gazebo::Vector3 frame_pos = frame_pose.pos;
    01854         gazebo::Quatern frame_rot = frame_pose.rot;
    01855 
    01856         //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl;
    01857         //target_pose = frame_pose + target_pose; // seems buggy, use my own
    01858         target_pose.pos = frame_pos + frame_rot.RotateVector(target_pos);
    01859         target_pose.rot = frame_rot * target_pose.rot;
    01860 
    01861         gazebo::Vector3 frame_linear_vel = frame->GetWorldLinearVel();
    01862         gazebo::Vector3 frame_angular_vel = frame->GetWorldAngularVel();
    01863         target_linear_vel -= frame_linear_vel;
    01864         target_angular_vel -= frame_angular_vel;
    01865       }
    01866       else if (req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#af3c75584cb85e20e4bac931829d8a80e) == "" || req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#af3c75584cb85e20e4bac931829d8a80e) == "world" || req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#af3c75584cb85e20e4bac931829d8a80e) == "map" || req.[link_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetLinkStateRequest__.html#ae59381a6d7c1fd2dea5f4189a4f118fe).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1LinkState__.html#af3c75584cb85e20e4bac931829d8a80e) == "/map")
    01867       {
    01868         [ROS_INFO](../../../api/rosconsole/html/macros__generated_8h.html#a20d5da22f16df09d52a7577227636a83)("SetLinkState: reference_frame is empty/world/map, using inertial frame");