srh_example_controller.cpp
Go to the documentation of this file.
00001 
00027 #include "../example/srh_example_controller.hpp"
00028 #include <angles/angles.h>
00029 #include <pluginlib/class_list_macros.h>
00030 
00031 #include <std_msgs/Float64.h>
00032 
00033 //Register the controller to be able to load it with the controller manager.
00034 PLUGINLIB_DECLARE_CLASS(sr_mechanism_controllers, SrhExampleController, controller::SrhExampleController, pr2_controller_interface::Controller)
00035 
00036 using namespace std;
00037 
00038 namespace controller {
00039 
00040   SrhExampleController::SrhExampleController()
00041     : SrController()
00042   {
00043     //This constructor is kept empty: the init functions
00044     // are called by the controller manager when the controllers are started.
00045   }
00046 
00047   SrhExampleController::~SrhExampleController()
00048   {
00049     //Stop subscribing to the command topic.
00050     sub_command_.shutdown();
00051   }
00052 
00053   bool SrhExampleController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00054   {
00055     assert(robot);
00056     node_ = n;
00057 
00058     //read the joint we're controlling from the parameter server.
00059     std::string joint_name;
00060     if (!node_.getParam("joint", joint_name)) {
00061       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00062       return false;
00063     }
00064 
00065     //set the state publisher for this controller:
00066     // This publisher publishes interesting data for the current controller.
00067     // (useful for debugging / tuning)
00068     // Feel free to create a different message type, to publish more meaningful
00069     // information for your controller (cf srh_mixed_position_velocity_controller.cpp)
00070     controller_state_publisher_.reset(
00071       new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointControllerState>
00072       (node_, "state", 1));
00073 
00074     //Calls the 2nd init function to finish initializing
00075     return init(robot, joint_name);
00076   }
00077 
00078   bool SrhExampleController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
00079   {
00080     assert(robot);
00081     robot_ = robot;
00082     last_time_ = robot->getTime();
00083 
00084     //We need to store 2 different joint states for the joint 0s:
00085     // They control the distal and the middle joint with the same control.
00086     if( joint_name.substr(3,1).compare("0") == 0)
00087     {
00088       has_j2 = true;
00089 
00090       //The joint 0 is name *FJ0, and is controlling *J1 + *J2.
00091       std::string j1 = joint_name.substr(0,3) + "1";
00092       std::string j2 = joint_name.substr(0,3) + "2";
00093 
00094       //Get the pointer to the joint state for *J1
00095       joint_state_ = robot_->getJointState(j1);
00096       if (!joint_state_)
00097       {
00098         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00099                   joint_name.c_str());
00100         return false;
00101       }
00102 
00103       //Get the pointer to the joint state for *J2
00104       joint_state_2 = robot_->getJointState(j2);
00105       if (!joint_state_2)
00106       {
00107         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00108                   joint_name.c_str());
00109         return false;
00110       }
00111     }
00112     else //"normal" joints: one controller controls one joint
00113     {
00114       has_j2 = false;
00115 
00116       //get the pointer to the joint state
00117       joint_state_ = robot_->getJointState(joint_name);
00118       if (!joint_state_)
00119       {
00120         ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00121                   joint_name.c_str());
00122         return false;
00123       }
00124     }
00125 
00126     // after init creates the subscriber to the /command topic
00127     after_init();
00128     return true;
00129   }
00130 
00131 
00132   void SrhExampleController::starting()
00133   {
00134     //Here we set the command to be = to the current position
00135     if( has_j2 ) //if it's *J0, then pos = *J1->pos + *J2->pos
00136       command_ = joint_state_->position_ + joint_state_2->position_;
00137     else
00138       command_ = joint_state_->position_;
00139   }
00140 
00141   void SrhExampleController::update()
00142   {
00143     assert(robot_ != NULL);
00144     assert(joint_state_->joint_);
00145 
00146     //compute the time difference since last iteration
00147     ros::Time time = robot_->getTime();
00148     dt_= time - last_time_;
00149 
00150     //make sure the controller has been initialised,
00151     // to avoid sending a crazy command.
00152     if (!initialized_)
00153     {
00154       starting();
00155 
00156       initialized_ = true;
00157     }
00158 
00159     //compute the commanded effort you want to send
00160     // to the motor: you can use whatever algorithm
00161     // you want. To see more complex examples on how
00162     // to use a pid loop / more than one loop, just
00163     // go to the src directory, and have a look at
00164     // srh_mixed_position_velocity_controller.cpp
00165 
00166     //we start by computing the position error
00167     double error_position = 0.0;
00168     if( has_j2 )
00169     {
00170       //For *J0, the position error is equal to the command - (*J1 + *J2)
00171       error_position = command_ - (joint_state_->position_ + joint_state_2->position_);
00172     }
00173     else
00174       error_position = command_ - joint_state_->position_;
00175 
00176     //Here I'm simply doing a dummy P controller, with a fixed gain.
00177     // It can't be used in the real life obviously. That's where you
00178     // should WRITE YOUR ALGORITHM
00179     commanded_effort = 10* error_position;
00180 
00181     //Update the commanded effort.
00182     if( has_j2 ) //The motor in *J0 is attached to the *J2
00183       joint_state_2->commanded_effort_ = commanded_effort;
00184     else
00185       joint_state_->commanded_effort_ = commanded_effort;
00186 
00187     if(loop_count_ % 10 == 0) //publishes the joint state at 100Hz
00188     {
00189       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00190       {
00191         controller_state_publisher_->msg_.header.stamp = time;
00192         controller_state_publisher_->msg_.set_point = command_;
00193 
00194         if( has_j2 )
00195         {
00196           controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
00197           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
00198         }
00199         else
00200         {
00201           controller_state_publisher_->msg_.process_value = joint_state_->position_;
00202           controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00203         }
00204 
00205         controller_state_publisher_->msg_.error = error_position;
00206         controller_state_publisher_->msg_.time_step = dt_.toSec();
00207         controller_state_publisher_->msg_.command = commanded_effort;
00208 
00209         controller_state_publisher_->unlockAndPublish();
00210       }
00211     }
00212     loop_count_++;
00213 
00214     last_time_ = time;
00215   }
00216 }
00217 
00218 /* For the emacs weenies in the crowd.
00219 Local Variables:
00220    c-basic-offset: 2
00221 End:
00222 */
00223 
00224 


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:06:50