tulip_controller_functions.cpp

Go to the documentation of this file.

/************************************************************************
 *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
 *      All rights reserved.                                                                                            *
 ************************************************************************
 *      Redistribution and use in source and binary forms, with or without      *
 *      modification, are permitted provided that the following conditions      *
 *      are met:                                                                                                                        *
 *                                                                                                                                              *
 *              1.      Redistributions of source code must retain the above            *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer.                                                                                                     *
 *                                                                                                                                              *
 *              2.      Redistributions in binary form must reproduce the above         *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer in the documentation and/or other materials          *
 *                      provided with the distribution.                                                         *
 *                                                                                                                                              *
 *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
 *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
 *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
 *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
 *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
 *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
 *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
 *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
 *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
 *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
 *      DAMAGE.                                                                                                                         *
 *                                                                                                                                              *
 *      The views and conclusions contained in the software and                         *
 *      documentation are those of the authors and should not be                        *
 *      interpreted as representing official policies, either expressed or      *
 *      implied, of TU/e.                                                                                                       *
 ************************************************************************/

#include <Eigen/Geometry> 

namespace tulip_controller_namespace {

bool tulip_controller_class::getXyzPar(std::string xyz_name, xyz& xyz_var, ros::NodeHandle &n) {
  if (n.getParam(xyz_name + "/x", xyz_var.x) &&
      n.getParam(xyz_name + "/y", xyz_var.y) &&
      n.getParam(xyz_name + "/z", xyz_var.z)  )
  { return true;} else { return false;}
}

bool tulip_controller_class::getExpPar(std::string exp_name, exp_properties& exp_var, ros::NodeHandle &n) {
  
  if (tulip_controller_class::getXyzPar(exp_name + "/init_model_state/pose/position",          exp_var.init_model_state.pose.position,       n)     &&
                             n.getParam(exp_name + "/init_model_state/pose/orientation/roll",  exp_var.init_model_state.pose.orientation.rpy.roll)  &&
                             n.getParam(exp_name + "/init_model_state/pose/orientation/pitch", exp_var.init_model_state.pose.orientation.rpy.pitch) &&
                             n.getParam(exp_name + "/init_model_state/pose/orientation/yaw",   exp_var.init_model_state.pose.orientation.rpy.yaw)   &&
      tulip_controller_class::getXyzPar(exp_name + "/init_model_state/twist/linear",           exp_var.init_model_state.twist.linear,        n)     &&
      tulip_controller_class::getXyzPar(exp_name + "/init_model_state/twist/angular",          exp_var.init_model_state.twist.angular,       n)     &&
      tulip_controller_class::getXyzPar(exp_name + "/push/force",                              exp_var.push.force,                           n)     &&
      n.getParam(exp_name + "/push/time",     exp_var.push.time)     && 
      n.getParam(exp_name + "/push/duration", exp_var.push.duration) )
  { 
   
   tf::Quaternion q;
   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);   
   
   exp_var.init_model_state.pose.orientation.quaternion.x = q.x();
   exp_var.init_model_state.pose.orientation.quaternion.y = q.y();
   exp_var.init_model_state.pose.orientation.quaternion.z = q.z();
   exp_var.init_model_state.pose.orientation.quaternion.w = q.w();
   
   /* 
   // transform orientation: from angleaxis to quaternion 
   Eigen::Quaternion<double> tmp_quaternion;
   Eigen::Vector3d axis;
   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;
   tmp_quaternion = Eigen::AngleAxis<double>(exp_var.init_model_state.pose.orientation.angleaxis.angle,axis);
   //std::cout << "quaternion" << tmp_quaternion.x() << tmp_quaternion.y() << tmp_quaternion.z() << tmp_quaternion.w() << std:: endl;
   

   
   //tf::quaternionMsgToTF requires gazebo_msgs::GetModelState message as input...
   gazebo_msgs::GetModelState tmp_state;
   tmp_state.response.pose.orientation.x=tmp_quaternion.x();
   tmp_state.response.pose.orientation.y=tmp_quaternion.y();
   tmp_state.response.pose.orientation.z=tmp_quaternion.z();
   tmp_state.response.pose.orientation.w=tmp_quaternion.w();
   
   //transform orientation: from quaternion to roll pitch yaw
   tf::Quaternion q;
   tf::quaternionMsgToTF(tmp_state.response.pose.orientation, q);
   tf::Matrix3x3(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);
   
   */
   
   return true;
    
  } else {
      return false;
  }                          
}

bool tulip_controller_class::setExpState(exp_properties exp_var, ros::NodeHandle &n) {
  
   

  //set initial state
   ros::service::waitForService("/gazeborver/set_model_state");
   ros::ServiceClient client_initstate = n.serviceClient<gazebo_msgs::SetModelState>("/gazeborver/set_model_state");
   gazebo_msgs::SetModelState srv_initstate;
 
   srv_initstate.request.model_state.model_name      = "tulip";                 // model to set state
   //srv_initpose.request.urdf_param_name = "robot_description";        // parameter name that contains the urdf XML.
   srv_initstate.request.model_state.pose.position.x           = exp_var.init_model_state.pose.position.x;
   srv_initstate.request.model_state.pose.position.y           = exp_var.init_model_state.pose.position.y;
   srv_initstate.request.model_state.pose.position.z           = exp_var.init_model_state.pose.position.z;
   srv_initstate.request.model_state.pose.orientation.x        = exp_var.init_model_state.pose.orientation.quaternion.x;
   srv_initstate.request.model_state.pose.orientation.y        = exp_var.init_model_state.pose.orientation.quaternion.y;
   srv_initstate.request.model_state.pose.orientation.z        = exp_var.init_model_state.pose.orientation.quaternion.z;
   srv_initstate.request.model_state.pose.orientation.w        = exp_var.init_model_state.pose.orientation.quaternion.w;
   srv_initstate.request.model_state.twist.linear.x            = exp_var.init_model_state.twist.linear.x;
   srv_initstate.request.model_state.twist.linear.y            = exp_var.init_model_state.twist.linear.y;
   srv_initstate.request.model_state.twist.linear.z            = exp_var.init_model_state.twist.linear.z;
   srv_initstate.request.model_state.twist.angular.x           = exp_var.init_model_state.twist.angular.x;
   srv_initstate.request.model_state.twist.angular.y           = exp_var.init_model_state.twist.angular.y;
   srv_initstate.request.model_state.twist.angular.z           = exp_var.init_model_state.twist.angular.z;
   srv_initstate.request.model_state.reference_frame = "world"; //gives error, unclear why? "tulip::base_link";
      
   if (client_initstate.call(srv_initstate))
      {
        ROS_INFO("Succesfully set initial state");
        return true;
      }
   else
      {
        ROS_ERROR("Failed to set initial state: %s",srv_initstate.response.status_message.c_str());
        return false;
      }
}

bool tulip_controller_class::getRobotState(gazebo_msgs::GetModelState &srv_getstate, rpy_prop &cur_orient_rpy, ros::NodeHandle &n) {
  
  //set initial state
   ros::service::waitForService("/gazeborver/get_model_state");
   ros::ServiceClient client_getstate = n.serviceClient<gazebo_msgs::GetModelState>("/gazeborver/get_model_state");
   srv_getstate.request.model_name      = "tulip";
   
   if (client_getstate.call(srv_getstate))
      {
        //ROS_INFO("Succesfully received current robot state");
        
        tf::Quaternion q;
        tf::quaternionMsgToTF(srv_getstate.response.pose.orientation, q);
        tf::Matrix3x3(q).getRPY(cur_orient_rpy.roll, cur_orient_rpy.pitch, cur_orient_rpy.yaw);
        
        return true;
      }
   else
      {
        ROS_ERROR("Failed to get robot state: %s",srv_getstate.response.status_message.c_str());
        return false;
      }
}

bool tulip_controller_class::applyForcetoRobot(gazebo_msgs::ApplyBodyWrench &srv_wrench, ros::NodeHandle &n) {

   ros::service::waitForService("/gazeborver/apply_body_wrench");
   ros::ServiceClient client_wrench = n.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazeborver/apply_body_wrench");
   srv_wrench.request.body_name       = "tulip::base_link";
   //srv_wrench.request.reference_frame = "world"; //gives error, unclear why? "tulip::base_link";
   
   if (client_wrench.call(srv_wrench))
     {
     //  ROS_INFO("Succesfully applied push force");
        return true;
      }
   else
      {
        ROS_ERROR("Failed to apply push force: %s",srv_wrench.response.status_message.c_str());
        return false;
      } 

}

}