tulip_controller.cpp
Go to the documentation of this file.
00001 
00013 /************************************************************************
00014  *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
00015  *      All rights reserved.                                                                                            *
00016  ************************************************************************
00017  *      Redistribution and use in source and binary forms, with or without      *
00018  *      modification, are permitted provided that the following conditions      *
00019  *      are met:                                                                                                                        *
00020  *                                                                                                                                              *
00021  *              1.      Redistributions of source code must retain the above            *
00022  *                      copyright notice, this list of conditions and the following *
00023  *                      disclaimer.                                                                                                     *
00024  *                                                                                                                                              *
00025  *              2.      Redistributions in binary form must reproduce the above         *
00026  *                      copyright notice, this list of conditions and the following *
00027  *                      disclaimer in the documentation and/or other materials          *
00028  *                      provided with the distribution.                                                         *
00029  *                                                                                                                                              *
00030  *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
00031  *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
00032  *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
00033  *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
00034  *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
00035  *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
00036  *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
00037  *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
00038  *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
00039  *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
00040  *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
00041  *      DAMAGE.                                                                                                                         *
00042  *                                                                                                                                              *
00043  *      The views and conclusions contained in the software and                         *
00044  *      documentation are those of the authors and should not be                        *
00045  *      interpreted as representing official policies, either expressed or      *
00046  *      implied, of TU/e.                                                                                                       *
00047  ************************************************************************/
00048 
00049 
00050 #include "tulip_controller.h"
00051 #include <pluginlib/class_list_macros.h>
00052 #include "tulip_controller_functions.cpp"
00053 
00054 // http://www.ros.org/wiki/pr2_mechanism
00055 // http://www.ros.org/wiki/pr2_mechanism/Tutorials
00056 
00057 
00058 namespace tulip_controller_namespace {
00059 
00060  
00062 bool tulip_controller_class::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00063 {
00064   ROS_INFO("tulip_controller::init()");
00065   joint_pos_current.resize(17);
00066   joint_pos_desired.resize(17);
00067   joint_pos_feedback.resize(17);
00068   joint_pos_init.resize(17);
00069   joint_state.resize(17);
00070   joint_name.resize(17);
00071   pid_controller.resize(17);
00072   published_jointdata = false;
00073 
00074   exp.resize(1);  
00075 
00076   //NOTE: the zeroth element of all joint vectors is not used
00077   //      -to keep compatible with matlab, where there is no zeroth element in a vector
00078   //      -to use the conventional joint numbering of TUlip, 
00079   
00080   joint_name[0]  = "";
00081   joint_name[1]  = "right_hip_yaw";
00082   joint_name[2]  = "right_hip_roll";
00083   joint_name[3]  = "right_hip_pitch";
00084   joint_name[4]  = "right_knee_pitch";
00085   joint_name[5]  = "right_ankle_pitch";
00086   joint_name[6]  = "right_ankle_roll";
00087   joint_name[7]  = "left_hip_yaw";
00088   joint_name[8]  = "left_hip_roll";
00089   joint_name[9]  = "left_hip_pitch";
00090   joint_name[10] = "left_knee_pitch";
00091   joint_name[11] = "left_ankle_pitch";
00092   joint_name[12] = "left_ankle_roll";
00093   joint_name[13] = "neck_pitch";
00094   joint_name[14] = "neck_yaw";
00095   joint_name[15] = "right_shoulder_pitch";
00096   joint_name[16] = "left_shoulder_pitch";
00097   
00098   //get joints and controller from parameter server
00099   for (int i = 1; i < 17 ; i++){ // for loop starts at i=1, not i=0
00100           
00101     joint_state[i] = robot->getJointState(joint_name[i]);
00102     if (!joint_state[i] ||
00103         !pid_controller[i].init(ros::NodeHandle(n, joint_name[i] + "_pid")) ||
00104         !n.getParam(joint_name[i] + "_pid/r", joint_pos_init[i]))
00105     {
00106       ROS_ERROR("Error reading joint parameters, check for typos in yaml file");
00107       while(1){};
00108     }
00109   }
00110   
00111  
00112   //get initial model state from parameter server
00113 
00114   if (!getExpPar("home_posture", exp[0], n))
00115   {
00116     ROS_ERROR("Error reading experiment parameters, check for typos in yaml file");
00117     while(1){};
00118   }
00119 
00120   
00121   std::cout << "Succesfully read parameters" << std::endl;
00122 
00123   joint_state_pub_ = n.advertise<tulip_gazebo::joint_state_message>("matlab_joints", StoreLen);
00124   storage_index_ = 0; // start recording immediately, set to StoreLen to start recording when triggered by capture
00125   cycle_index_ = 0;
00126   
00127   // copy robot pointer so we can access time
00128   robot_ = robot;
00129 
00130   //set initial posture
00131   ros::service::waitForService("/gazeborver/set_model_configuration");
00132   ros::ServiceClient client_initpose = n.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazeborver/set_model_configuration");
00133   gazebo_msgs::SetModelConfiguration srv_initpose;
00134    
00135   srv_initpose.request.model_name      = "tulip";               // model to set state
00136   srv_initpose.request.urdf_param_name = "robot_description";   // parameter name that contains the urdf XML.
00137   srv_initpose.request.joint_names     = joint_name;
00138   srv_initpose.request.joint_positions = joint_pos_init; 
00139 
00140   if (client_initpose.call(srv_initpose))
00141      { ROS_INFO("Succesfully set initial posture to ");
00142         for (int i=1; i<17; i++) {std::cout << joint_pos_init[i] << " ";}
00143         std::cout << std::endl;
00144       }
00145    else
00146       { ROS_ERROR("Failed to set initial posture: %s",srv_initpose.response.status_message.c_str()); } 
00147   
00148   //unpause physics
00149   ros::service::waitForService("/gazeborver/unpause_physics");
00150   ros::ServiceClient client_unpause = n.serviceClient<std_srvs::Empty>("/gazeborver/unpause_physics");
00151   std_srvs::Empty srv_unpause;
00152         
00153   if (client_unpause.call(srv_unpause)) {
00154     ROS_INFO("Succesfully unpaused physics");
00155   } else {
00156     ROS_ERROR("Failed to unpause physics");
00157   } 
00158    
00159   return true;
00160 }
00161 
00163 void tulip_controller_class::starting()
00164 {
00165    ROS_INFO("tulip_controller::starting()");
00166    for (int i = 1; i < 17; i++){
00167      joint_pos_init[i] = joint_state[i]->position_;
00168      pid_controller[i].reset();
00169    }
00170    time_of_first_cycle_    = robot_->getTime();
00171    time_of_previous_cycle_ = robot_->getTime();
00172    
00173    //http://answers.ros.org/question/2009/applying-a-force-to-a-rigid-body
00174    //https://code.ros.org/svn/wg-ros-pkg/stacks/wg_robots_gazebo/trunk/pr2_arm_gazebo/src/move_arm.cpp
00175    //http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c%2B%2B)
00176    ROS_INFO("time is push time!");
00177    
00178    // give push prescribed in yaml parameter file
00179    
00180    
00181       ros::NodeHandle n;
00182       gazebo_msgs::ApplyBodyWrench srv_wrench;
00183       srv_wrench.request.wrench.force.x  = exp[0].push.force.x;
00184       srv_wrench.request.wrench.force.y  = exp[0].push.force.y;
00185       srv_wrench.request.wrench.force.z  = exp[0].push.force.z;
00186       srv_wrench.request.start_time      = robot_->getTime() + ros::Duration(exp[0].push.time);
00187       srv_wrench.request.duration        = ros::Duration(exp[0].push.duration);
00188    
00189       tulip_controller_class::applyForcetoRobot(srv_wrench, n);
00190 }
00191 
00192 
00194 void tulip_controller_class::update()
00195 {
00196   ros::NodeHandle n;
00197   //ROS_INFO("tulip_controller::update()");
00198  
00199   ros::Duration dt = robot_->getTime() - time_of_previous_cycle_;
00200   time_of_previous_cycle_ = robot_->getTime();
00201   
00202   for (int i = 1; i < 17; i++){
00203     joint_pos_desired[i] = joint_pos_init[i];
00204     joint_pos_current[i] = joint_state[i]->position_;
00205     joint_pos_feedback[i] = pid_controller[i].updatePid(joint_pos_current[i]-joint_pos_desired[i], dt);
00206     joint_state[i]->commanded_effort_ = joint_pos_feedback[i];
00207   
00208     //ROS_INFO("joint[%2d] %-20s desired %f \tcurrent %f \teffort %f",i,joint_name[i].c_str(),joint_pos_desired[i],joint_pos_current[i],joint_pos_feedback[i]);
00209   }
00210 
00211 
00212   cycle_index_ = cycle_index_ + 1;
00213   if (cycle_index_ % 10 == 1) { //only save every tenth cycle
00214     for (int i = 1; i < 17; i++){
00215       int index = storage_index_;
00216       if ((index >= 0) && (index < StoreLen))
00217       {
00218         storage_[index].cycle_nr         = (cycle_index_-1)/10 +1; //trick to count 1,2,3,4 instead of 1,11,21,31 because we only save every tenth cycle
00219         storage_[index].time             = robot_->getTime().toSec();
00220         storage_[index].joint_nr         = i;
00221         storage_[index].desired_position = joint_pos_desired[i];
00222         storage_[index].position         = joint_state[i]->position_;
00223         storage_[index].velocity         = joint_state[i]->velocity_;
00224         storage_[index].commanded_effort = joint_state[i]->commanded_effort_;
00225         storage_[index].measured_effort  = joint_state[i]->measured_effort_;
00226 
00227         // Increment for the next cycle.
00228         storage_index_ = index+1;
00229       }
00230 
00231     }
00232     if (storage_index_ >= StoreLen && !published_jointdata) {
00233       tulip_controller_class::capture();
00234       published_jointdata = true;
00235     }
00236   }
00237 }
00238 
00239 
00241 void tulip_controller_class::stopping()
00242 {}
00243 
00245 bool tulip_controller_class::capture()
00246 {
00247   /* Record the starting time. */
00248   ros::Time started = ros::Time::now();
00249 
00250   /* Now wait until the buffer is full. */
00251   while (storage_index_ < StoreLen)
00252     {
00253       /* Sleep for 1ms as not to hog the CPU. */
00254       ros::Duration(0.001).sleep();
00255 
00256       /* Make sure we don't hang here forever. */
00257       if (ros::Time::now() - started > ros::Duration(20.0))
00258         {
00259           ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
00260           return false;
00261         }
00262     }
00263 
00264   /* Then we can publish the buffer contents. */
00265   int  index;
00266   for (index = 0 ; index < StoreLen ; index++)
00267     joint_state_pub_.publish(storage_[index]);
00268 
00269   ROS_INFO("Published joint data");
00270   return true;
00271 }
00272 
00273 } // namespace
00274 
00276 PLUGINLIB_EXPORT_CLASS(tulip_controller_namespace::tulip_controller_class, pr2_controller_interface::Controller)


tulip_gazebo
Author(s): Tim Assman, Pieter van Zutven
autogenerated on Sun Jan 26 2014 21:00:10