01367         gazebo::Entity* relative_entity = gazebo::World::Instance()->GetEntityByName(req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14));
    01368         if (relative_entity)
    01369         {
    01370           gazebo::Pose3d  frame_pose = relative_entity->GetWorldPose(); // - this->myBody->GetCoMPose();
    01371           gazebo::Vector3 frame_pos = frame_pose.pos;
    01372           gazebo::Quatern frame_rot = frame_pose.rot;
    01373 
    01374           //std::cout << " debug : " << relative_entity->GetName() << " : " << frame_pose << " : " << target_pose << std::endl;
    01375           //target_pose = frame_pose + target_pose; // seems buggy, use my own
    01376           target_pose.pos = frame_pos + frame_rot.RotateVector(target_pos);
    01377           target_pose.rot = frame_rot * target_pose.rot;
    01378         }
    01380         else if (req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14) == "" || req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14) == "world" || req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14) == "map" || req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14) == "/map" )
    01381         {
    01382           [ROS_DEBUG](../../../api/rosconsole/html/macros__generated_8h.html#a83228c0b42c80605abdd0f42af64b1f7)("SetModelState: reference frame is empty/world/map, usig inertial frame");
    01383         }
    01384         else
    01385         {
    01386           [ROS_ERROR](../../../api/rosconsole/html/macros__generated_8h.html#a7fb635b03cc608284eadecd2f8becd9a)("SetModelState: for model[%s], specified reference frame entity [%s] does not exist",
    01387                     req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[model_name](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#acddb4cc1ba3d29b97db1141f78c454f0).c_str(),req.[model_state](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateRequest__.html#a6a20bccbfd561c02ae793bd41a6742e5).[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ModelState__.html#aba2a167473d2b4ed3275952deb71cf14).c_str());
    01388           res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateResponse__.html#acca9802bb129cd425a0eeb458fac040e) = false;
    01389           res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateResponse__.html#abe59335014d06188f09f6ef2f91447cb) = "SetModelState: specified reference frame entity does not exist";
    01390           return false;
    01391         }
    01392 
    01393         //ROS_ERROR("target state: %f %f %f",target_pose.pos.x,target_pose.pos.y,target_pose.pos.z);
    01394         bool is_paused = gazebo::Simulator::Instance()->IsPaused();
    01395         gazebo::Simulator::Instance()->SetPaused(true);
    01396         model->SetWorldPose(target_pose);
    01397         gazebo::Simulator::Instance()->SetPaused(is_paused);
    01398         //gazebo::Pose3d p3d = model->GetWorldPose();
    01399         //ROS_ERROR("model updated state: %f %f %f",p3d.pos.x,p3d.pos.y,p3d.pos.z);
    01400 
    01401         // set model velocity
    01402         model->SetLinearVel(target_pos_dot);
    01403         model->SetAngularVel(target_rot_dot);
    01404 
    01405         res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateResponse__.html#acca9802bb129cd425a0eeb458fac040e) = true;
    01406         res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1SetModelStateResponse__.html#abe59335014d06188f09f6ef2f91447cb) = "SetModelState: set model state done";
    01407         return true;