tulip_controller.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 "tulip_controller.h"
#include <pluginlib/class_list_macros.h>
#include "tulip_controller_functions.cpp"

// http://www.ros.org/wiki/pr2_mechanism
// http://www.ros.org/wiki/pr2_mechanism/Tutorials


namespace tulip_controller_namespace {

 
bool tulip_controller_class::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
{
  ROS_INFO("tulip_controller::init()");
  joint_pos_current.resize(17);
  joint_pos_desired.resize(17);
  joint_pos_feedback.resize(17);
  joint_pos_init.resize(17);
  joint_state.resize(17);
  joint_name.resize(17);
  pid_controller.resize(17);
  published_jointdata = false;

  exp.resize(1);  

  //NOTE: the zeroth element of all joint vectors is not used
  //      -to keep compatible with matlab, where there is no zeroth element in a vector
  //      -to use the conventional joint numbering of TUlip, 
  
  joint_name[0]  = "";
  joint_name[1]  = "right_hip_yaw";
  joint_name[2]  = "right_hip_roll";
  joint_name[3]  = "right_hip_pitch";
  joint_name[4]  = "right_knee_pitch";
  joint_name[5]  = "right_ankle_pitch";
  joint_name[6]  = "right_ankle_roll";
  joint_name[7]  = "left_hip_yaw";
  joint_name[8]  = "left_hip_roll";
  joint_name[9]  = "left_hip_pitch";
  joint_name[10] = "left_knee_pitch";
  joint_name[11] = "left_ankle_pitch";
  joint_name[12] = "left_ankle_roll";
  joint_name[13] = "neck_pitch";
  joint_name[14] = "neck_yaw";
  joint_name[15] = "right_shoulder_pitch";
  joint_name[16] = "left_shoulder_pitch";
  
  //get joints and controller from parameter server
  for (int i = 1; i < 17 ; i++){ // for loop starts at i=1, not i=0
          
    joint_state[i] = robot->getJointState(joint_name[i]);
    if (!joint_state[i] ||
        !pid_controller[i].init(ros::NodeHandle(n, joint_name[i] + "_pid")) ||
        !n.getParam(joint_name[i] + "_pid/r", joint_pos_init[i]))
    {
      ROS_ERROR("Error reading joint parameters, check for typos in yaml file");
      while(1){};
    }
  }
  
 
  //get initial model state from parameter server

  if (!getExpPar("home_posture", exp[0], n))
  {
    ROS_ERROR("Error reading experiment parameters, check for typos in yaml file");
    while(1){};
  }

  
  std::cout << "Succesfully read parameters" << std::endl;

  joint_state_pub_ = n.advertise<tulip_gazebo::joint_state_message>("matlab_joints", StoreLen);
  storage_index_ = 0; // start recording immediately, set to StoreLen to start recording when triggered by capture
  cycle_index_ = 0;
  
  // copy robot pointer so we can access time
  robot_ = robot;

  //set initial posture
  ros::service::waitForService("/gazebo/set_model_configuration");
  ros::ServiceClient client_initpose = n.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazebo/set_model_configuration");
  gazebo_msgs::SetModelConfiguration srv_initpose;
   
  srv_initpose.request.model_name      = "tulip";               // model to set state
  srv_initpose.request.urdf_param_name = "robot_description";   // parameter name that contains the urdf XML.
  srv_initpose.request.joint_names     = joint_name;
  srv_initpose.request.joint_positions = joint_pos_init; 

  if (client_initpose.call(srv_initpose))
     { ROS_INFO("Succesfully set initial posture to ");
        for (int i=1; i<17; i++) {std::cout << joint_pos_init[i] << " ";}
        std::cout << std::endl;
      }
   else
      { ROS_ERROR("Failed to set initial posture: %s",srv_initpose.response.status_message.c_str()); } 
  
  //unpause physics
  ros::service::waitForService("/gazebo/unpause_physics");
  ros::ServiceClient client_unpause = n.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
  std_srvs::Empty srv_unpause;
        
  if (client_unpause.call(srv_unpause)) {
    ROS_INFO("Succesfully unpaused physics");
  } else {
    ROS_ERROR("Failed to unpause physics");
  } 
   
  return true;
}

void tulip_controller_class::starting()
{
   ROS_INFO("tulip_controller::starting()");
   for (int i = 1; i < 17; i++){
     joint_pos_init[i] = joint_state[i]->position_;
     pid_controller[i].reset();
   }
   time_of_first_cycle_    = robot_->getTime();
   time_of_previous_cycle_ = robot_->getTime();
   
   //http://answers.ros.org/question/2009/applying-a-force-to-a-rigid-body
   //https://code.ros.org/svn/wg-ros-pkg/stacks/wg_robots_gazebo/trunk/pr2_arm_gazebo/src/move_arm.cpp
   //http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c%2B%2B)
   ROS_INFO("time is push time!");
   
   // give push prescribed in yaml parameter file
   
   
      ros::NodeHandle n;
      gazebo_msgs::ApplyBodyWrench srv_wrench;
      srv_wrench.request.wrench.force.x  = exp[0].push.force.x;
      srv_wrench.request.wrench.force.y  = exp[0].push.force.y;
      srv_wrench.request.wrench.force.z  = exp[0].push.force.z;
      srv_wrench.request.start_time      = robot_->getTime() + ros::Duration(exp[0].push.time);
      srv_wrench.request.duration        = ros::Duration(exp[0].push.duration);
   
      tulip_controller_class::applyForcetoRobot(srv_wrench, n);
}


void tulip_controller_class::update()
{
  ros::NodeHandle n;
  //ROS_INFO("tulip_controller::update()");
 
  ros::Duration dt = robot_->getTime() - time_of_previous_cycle_;
  time_of_previous_cycle_ = robot_->getTime();
  
  for (int i = 1; i < 17; i++){
    joint_pos_desired[i] = joint_pos_init[i];
    joint_pos_current[i] = joint_state[i]->position_;
    joint_pos_feedback[i] = pid_controller[i].updatePid(joint_pos_current[i]-joint_pos_desired[i], dt);
    joint_state[i]->commanded_effort_ = joint_pos_feedback[i];
  
    //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]);
  }


  cycle_index_ = cycle_index_ + 1;
  if (cycle_index_ % 10 == 1) { //only save every tenth cycle
    for (int i = 1; i < 17; i++){
      int index = storage_index_;
      if ((index >= 0) && (index < StoreLen))
      {
        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
        storage_[index].time             = robot_->getTime().toSec();
        storage_[index].joint_nr         = i;
        storage_[index].desired_position = joint_pos_desired[i];
        storage_[index].position         = joint_state[i]->position_;
        storage_[index].velocity         = joint_state[i]->velocity_;
        storage_[index].commanded_effort = joint_state[i]->commanded_effort_;
        storage_[index].measured_effort  = joint_state[i]->measured_effort_;

        // Increment for the next cycle.
        storage_index_ = index+1;
      }

    }
    if (storage_index_ >= StoreLen && !published_jointdata) {
      tulip_controller_class::capture();
      published_jointdata = true;
    }
  }
}


void tulip_controller_class::stopping()
{}

bool tulip_controller_class::capture()
{
  /* Record the starting time. */
  ros::Time started = ros::Time::now();

  /* Now wait until the buffer is full. */
  while (storage_index_ < StoreLen)
    {
      /* Sleep for 1ms as not to hog the CPU. */
      ros::Duration(0.001).sleep();

      /* Make sure we don't hang here forever. */
      if (ros::Time::now() - started > ros::Duration(20.0))
        {
          ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
          return false;
        }
    }

  /* Then we can publish the buffer contents. */
  int  index;
  for (index = 0 ; index < StoreLen ; index++)
    joint_state_pub_.publish(storage_[index]);

  ROS_INFO("Published joint data");
  return true;
}

} // namespace

PLUGINLIB_DECLARE_CLASS(tulip_controller,tulip_controller_plugin, 
                         tulip_controller_namespace::tulip_controller_class, 
                         pr2_controller_interface::Controller)