tulip_gazebo: tulip_gazebo: tulip_controller

00001 
00015 /************************************************************************
00016  *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
00017  *      All rights reserved.                                                                                            *
00018  ************************************************************************
00019  *      Redistribution and use in source and binary forms, with or without      *
00020  *      modification, are permitted provided that the following conditions      *
00021  *      are met:                                                                                                                        *
00022  *                                                                                                                                              *
00023  *              1.      Redistributions of source code must retain the above            *
00024  *                      copyright notice, this list of conditions and the following *
00025  *                      disclaimer.                                                                                                     *
00026  *                                                                                                                                              *
00027  *              2.      Redistributions in binary form must reproduce the above         *
00028  *                      copyright notice, this list of conditions and the following *
00029  *                      disclaimer in the documentation and/or other materials          *
00030  *                      provided with the distribution.                                                         *
00031  *                                                                                                                                              *
00032  *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
00033  *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
00034  *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
00035  *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
00036  *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
00037  *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
00038  *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
00039  *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
00040  *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
00041  *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
00042  *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
00043  *      DAMAGE.                                                                                                                         *
00044  *                                                                                                                                              *
00045  *      The views and conclusions contained in the software and                         *
00046  *      documentation are those of the authors and should not be                        *
00047  *      interpreted as representing official policies, either expressed or      *
00048  *      implied, of TU/e.                                                                                                       *
00049  ************************************************************************/
00050 
00051 #include <Eigen/Geometry> 
00052 
00053 namespace tulip_controller_namespace {
00054 
00055 bool tulip_controller_class::getXyzPar(std::string xyz_name, xyz& xyz_var, ros::NodeHandle &n) {
00056   if (n.getParam(xyz_name + "/x", xyz_var.x) &&
00057       n.getParam(xyz_name + "/y", xyz_var.y) &&
00058       n.getParam(xyz_name + "/z", xyz_var.z)  )
00059   { return true;} else { return false;}
00060 }
00061 
00062 bool tulip_controller_class::getExpPar(std::string exp_name, exp_properties& exp_var, ros::NodeHandle &n) {
00063   
00064   if (tulip_controller_class::getXyzPar(exp_name + "/init_model_state/pose/position",          exp_var.init_model_state.pose.position,       n)     &&
00065                              n.getParam(exp_name + "/init_model_state/pose/orientation/roll",  exp_var.init_model_state.pose.orientation.rpy.roll)  &&
00066                              n.getParam(exp_name + "/init_model_state/pose/orientation/pitch", exp_var.init_model_state.pose.orientation.rpy.pitch) &&
00067                              n.getParam(exp_name + "/init_model_state/pose/orientation/yaw",   exp_var.init_model_state.pose.orientation.rpy.yaw)   &&
00068       tulip_controller_class::getXyzPar(exp_name + "/init_model_state/twist/linear",           exp_var.init_model_state.twist.linear,        n)     &&
00069       tulip_controller_class::getXyzPar(exp_name + "/init_model_state/twist/angular",          exp_var.init_model_state.twist.angular,       n)     &&
00070       tulip_controller_class::getXyzPar(exp_name + "/push/force",                              exp_var.push.force,                           n)     &&
00071       n.getParam(exp_name + "/push/time",     exp_var.push.time)     && 
00072       n.getParam(exp_name + "/push/duration", exp_var.push.duration) )
00073   { 
00074    
00075    btQuaternion q;
00076    q = tf::createQuaternionFromRPY(exp_var.init_model_state.pose.orientation.rpy.roll,exp_var.init_model_state.pose.orientation.rpy.pitch,exp_var.init_model_state.pose.orientation.rpy.yaw);   
00077    
00078    exp_var.init_model_state.pose.orientation.quaternion.x = q.x();
00079    exp_var.init_model_state.pose.orientation.quaternion.y = q.y();
00080    exp_var.init_model_state.pose.orientation.quaternion.z = q.z();
00081    exp_var.init_model_state.pose.orientation.quaternion.w = q.w();
00082    
00083    /* 
00084    // transform orientation: from angleaxis to quaternion 
00085    Eigen::Quaternion<double> tmp_quaternion;
00086    Eigen::Vector3d axis;
00087    axis << exp_var.init_model_state.pose.orientation.angleaxis.x, exp_var.init_model_state.pose.orientation.angleaxis.y, exp_var.init_model_state.pose.orientation.angleaxis.z;
00088    tmp_quaternion = Eigen::AngleAxis<double>(exp_var.init_model_state.pose.orientation.angleaxis.angle,axis);
00089    //std::cout << "quaternion" << tmp_quaternion.x() << tmp_quaternion.y() << tmp_quaternion.z() << tmp_quaternion.w() << std:: endl;
00090    
00091 
00092    
00093    //tf::quaternionMsgToTF requires gazebo_msgs::GetModelState message as input...
00094    gazebo_msgs::GetModelState tmp_state;
00095    tmp_state.response.pose.orientation.x=tmp_quaternion.x();
00096    tmp_state.response.pose.orientation.y=tmp_quaternion.y();
00097    tmp_state.response.pose.orientation.z=tmp_quaternion.z();
00098    tmp_state.response.pose.orientation.w=tmp_quaternion.w();
00099    
00100    //transform orientation: from quaternion to roll pitch yaw
00101    btQuaternion q;
00102    tf::quaternionMsgToTF(tmp_state.response.pose.orientation, q);
00103    btMatrix3x3(q).getRPY(exp_var.init_model_state.pose.orientation.rpy.roll, exp_var.init_model_state.pose.orientation.rpy.pitch, exp_var.init_model_state.pose.orientation.rpy.yaw);
00104    
00105    */
00106    
00107    return true;
00108     
00109   } else {
00110       return false;
00111   }                          
00112 }
00113 
00114 bool tulip_controller_class::setExpState(exp_properties exp_var, ros::NodeHandle &n) {
00115   
00116    
00117 
00118   //set initial state
00119    ros::service::waitForService("/gazebo/set_model_state");
00120    ros::ServiceClient client_initstate = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
00121    gazebo_msgs::SetModelState srv_initstate;
00122  
00123    srv_initstate.request.model_state.model_name      = "tulip";                 // model to set state
00124    //srv_initpose.request.urdf_param_name = "robot_description";        // parameter name that contains the urdf XML.
00125    srv_initstate.request.model_state.pose.position.x           = exp_var.init_model_state.pose.position.x;
00126    srv_initstate.request.model_state.pose.position.y           = exp_var.init_model_state.pose.position.y;
00127    srv_initstate.request.model_state.pose.position.z           = exp_var.init_model_state.pose.position.z;
00128    srv_initstate.request.model_state.pose.orientation.x        = exp_var.init_model_state.pose.orientation.quaternion.x;
00129    srv_initstate.request.model_state.pose.orientation.y        = exp_var.init_model_state.pose.orientation.quaternion.y;
00130    srv_initstate.request.model_state.pose.orientation.z        = exp_var.init_model_state.pose.orientation.quaternion.z;
00131    srv_initstate.request.model_state.pose.orientation.w        = exp_var.init_model_state.pose.orientation.quaternion.w;
00132    srv_initstate.request.model_state.twist.linear.x            = exp_var.init_model_state.twist.linear.x;
00133    srv_initstate.request.model_state.twist.linear.y            = exp_var.init_model_state.twist.linear.y;
00134    srv_initstate.request.model_state.twist.linear.z            = exp_var.init_model_state.twist.linear.z;
00135    srv_initstate.request.model_state.twist.angular.x           = exp_var.init_model_state.twist.angular.x;
00136    srv_initstate.request.model_state.twist.angular.y           = exp_var.init_model_state.twist.angular.y;
00137    srv_initstate.request.model_state.twist.angular.z           = exp_var.init_model_state.twist.angular.z;
00138    srv_initstate.request.model_state.reference_frame = "world"; //gives error, unclear why? "tulip::base_link";
00139       
00140    if (client_initstate.call(srv_initstate))
00141       {
00142         ROS_INFO("Succesfully set initial state");
00143         return true;
00144       }
00145    else
00146       {
00147         ROS_ERROR("Failed to set initial state: %s",srv_initstate.response.status_message.c_str());
00148         return false;
00149       }
00150 }
00151 
00152 bool tulip_controller_class::getRobotState(gazebo_msgs::GetModelState &srv_getstate, rpy_prop &cur_orient_rpy, ros::NodeHandle &n) {
00153   
00154   //set initial state
00155    ros::service::waitForService("/gazebo/get_model_state");
00156    ros::ServiceClient client_getstate = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
00157    srv_getstate.request.model_name      = "tulip";
00158    
00159    if (client_getstate.call(srv_getstate))
00160       {
00161         //ROS_INFO("Succesfully received current robot state");
00162         
00163         btQuaternion q;
00164         tf::quaternionMsgToTF(srv_getstate.response.pose.orientation, q);
00165         btMatrix3x3(q).getRPY(cur_orient_rpy.roll, cur_orient_rpy.pitch, cur_orient_rpy.yaw);
00166         
00167         return true;
00168       }
00169    else
00170       {
00171         ROS_ERROR("Failed to get robot state: %s",srv_getstate.response.status_message.c_str());
00172         return false;
00173       }
00174 }
00175 
00176 bool tulip_controller_class::applyForcetoRobot(gazebo_msgs::ApplyBodyWrench &srv_wrench, ros::NodeHandle &n) {
00177 
00178    ros::service::waitForService("/gazebo/apply_body_wrench");
00179    ros::ServiceClient client_wrench = n.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench");
00180    srv_wrench.request.body_name       = "tulip::base_link";
00181    //srv_wrench.request.reference_frame = "world"; //gives error, unclear why? "tulip::base_link";
00182    
00183    if (client_wrench.call(srv_wrench))
00184      {
00185      //  ROS_INFO("Succesfully applied push force");
00186         return true;
00187       }
00188    else
00189       {
00190         ROS_ERROR("Failed to apply push force: %s",srv_wrench.response.status_message.c_str());
00191         return false;
00192       } 
00193 
00194 }
00195 
00196 double tulip_controller_class::getBacklashPosition(std::vector<pr2_mechanism_model::JointState*>& backlash_state, int i) {
00197     if (backlash_state[i])
00198       { return backlash_state[i]->position_;}
00199     else
00200       { return 0.0; }  
00201 }
00202 
00203 double tulip_controller_class::getBacklashVelocity(std::vector<pr2_mechanism_model::JointState*>& backlash_state, int i) {
00204     if (backlash_state[i])
00205       { return backlash_state[i]->velocity_;}
00206     else
00207       { return 0.0; }  
00208 }
00209 
00210 }
tulip_controller_functions.cpp
