01952         // set body velocity to desired twist
    01953         body->SetLinearVel(target_linear_vel);
    01954         body->SetAngularVel(target_angular_vel);
    01955 
    01956       }
    01957 
    01958     }
    01959 
    01960 
    [01964](classGazeboROSNode.html#ab198eaed8eab1ba85cdfdfb72c268150)     void [transformWrench](classGazeboROSNode.html#ab198eaed8eab1ba85cdfdfb72c268150 "shift wrench from reference frame to target frame assume wrench is defined in")( gazebo::Vector3 &target_force, gazebo::Vector3 &target_torque,
    01965                                gazebo::Vector3 reference_force, gazebo::Vector3 reference_torque,
    01966                                gazebo::Pose3d target_to_reference )
    01967     {
    01968       // rotate force into target frame
    01969       target_force = target_to_reference.rot.RotateVector(reference_force);
    01970       // rotate torque into target frame
    01971       target_torque = target_to_reference.rot.RotateVector(reference_torque);
    01972 
    01973       // target force is the refence force rotated by the target->reference transform
    01974       target_force = target_force;
    01975       target_torque = target_torque + target_to_reference.pos.GetCrossProd(target_force);
    01976     }
    01977 
    [01980](classGazeboROSNode.html#affc8ea4611f0cd8c4b4a497056359398)     bool [applyBodyWrench](classGazeboROSNode.html#affc8ea4611f0cd8c4b4a497056359398)([gazebo_msgs::ApplyBodyWrench::Request](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html) &req,[gazebo_msgs::ApplyBodyWrench::Response](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchResponse__.html) &res)
    01981     {
    01982       gazebo::Body* body = dynamic_cast<gazebo::Body*>(gazebo::World::Instance()->GetEntityByName(req.[body_name](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a5ac16bd4c0d1004ab321e7f93b603e95)));
    01983       gazebo::Body* frame = dynamic_cast<gazebo::Body*>(gazebo::World::Instance()->GetEntityByName(req.[reference_frame](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a478aa71d79dc3b29ba87496996406980)));
    01984 
    01985       if (!body)
    01986       {
    01987         [ROS_ERROR](../../../api/rosconsole/html/macros__generated_8h.html#a7fb635b03cc608284eadecd2f8becd9a)("ApplyBodyWrench: body [%s] does not exist",req.[body_name](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a5ac16bd4c0d1004ab321e7f93b603e95).c_str());
    01988         res.[success](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchResponse__.html#a614b4dd49b7b7f87fb360bc0f420f8ed) = false;
    01989         res.[status_message](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchResponse__.html#aeb381da235043f2b7e2875d78beacade) = "ApplyBodyWrench: body does not exist";
    01990         return false;
    01991       }
    01992 
    01993       // target wrench
    01994       gazebo::Vector3 reference_force(req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[force](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a2922f100d999404b660018eb809c6a20).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be),req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[force](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a2922f100d999404b660018eb809c6a20).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404),req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[force](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a2922f100d999404b660018eb809c6a20).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1));
    01995       gazebo::Vector3 reference_torque(req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[torque](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a9fa00191ad94ce694feaf9bbc510043f).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#abf9dabaf651196aee50fd14bf3b643be),req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[torque](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a9fa00191ad94ce694feaf9bbc510043f).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#aaea58ad929c43960a06f728d97132404),req.[wrench](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a9a96ff63f003a8df06b91cdccd26f4c6).[torque](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Wrench__.html#a9fa00191ad94ce694feaf9bbc510043f).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Vector3__.html#a4cde719326e6c3688ab50b51ab4708b1));
    01996       gazebo::Vector3 reference_point(req.[reference_point](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a4efb2891cc9a840bb1791fe0bd2bcf2d).[x](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#acd9420e6916084b2a9c09019bfda1708),req.[reference_point](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a4efb2891cc9a840bb1791fe0bd2bcf2d).[y](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a46a84ff5a3e5817ab5693abda66fccb0),req.[reference_point](../../gazebo_msgs/html/structgazebo__msgs_1_1ApplyBodyWrenchRequest__.html#a4efb2891cc9a840bb1791fe0bd2bcf2d).[z](../../../api/geometry_msgs/html/structgeometry__msgs_1_1Point__.html#a061f0093e927a95998f35ceceb5cebda));