srh_syntouch_controllers.cpp
Go to the documentation of this file.
00001 
00026 #include "../example/srh_syntouch_controllers.hpp"
00027 #include "angles/angles.h"
00028 #include "pluginlib/class_list_macros.h"
00029 #include <sstream>
00030 #include <math.h>
00031 #include <sr_utilities/sr_math_utils.hpp>
00032 #include <std_msgs/Float64.h>
00033 
00034 PLUGINLIB_DECLARE_CLASS(sr_mechanism_controllers, SrhSyntouchController, controller::SrhSyntouchController, pr2_controller_interface::Controller)
00035 
00036 using namespace std;
00037 
00038 namespace controller {
00039 
00040   SrhSyntouchController::SrhSyntouchController()
00041     : SrController()
00042   {
00043   }
00044 
00045   SrhSyntouchController::~SrhSyntouchController()
00046   {
00047     sub_command_.shutdown();
00048   }
00049 
00050   bool SrhSyntouchController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
00051   {
00052     ROS_DEBUG(" --------- ");
00053     ROS_DEBUG_STREAM("Init: " << joint_name);
00054 
00055     assert(robot);
00056     robot_ = robot;
00057     last_time_ = robot->getTime();
00058 
00059     joint_state_ = robot_->getJointState(joint_name);
00060     if (!joint_state_)
00061     {
00062       ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
00063                 joint_name.c_str());
00064       return false;
00065     }
00066     if (!joint_state_->calibrated_)
00067     {
00068       ROS_ERROR("Joint %s not calibrated for SrhSyntouchController", joint_name.c_str());
00069       return false;
00070     }
00071 
00072     //init the pointer to the biotacs data, updated at 1kHz
00073     actuator_ = static_cast<sr_actuator::SrActuator*>( robot->model_->getActuator( joint_name ) );
00074 
00075     after_init();
00076     return true;
00077   }
00078 
00079   bool SrhSyntouchController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00080   {
00081     assert(robot);
00082     node_ = n;
00083 
00084     std::string joint_name;
00085     if (!node_.getParam("joint", joint_name)) {
00086       ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00087       return false;
00088     }
00089 
00090     return init(robot, joint_name);
00091   }
00092 
00093 
00094   void SrhSyntouchController::starting()
00095   {
00096     command_ = joint_state_->position_;
00097 
00098     ROS_WARN("Reseting PID");
00099   }
00100 
00101   void SrhSyntouchController::update()
00102   {
00103     if (!joint_state_->calibrated_)
00104       return;
00105 
00106     assert(robot_ != NULL);
00107     ros::Time time = robot_->getTime();
00108     assert(joint_state_->joint_);
00109     dt_= time - last_time_;
00110 
00111     if (!initialized_)
00112     {
00113       initialized_ = true;
00114       command_ = joint_state_->position_;
00115     }
00116 
00118     // POSITION
00120     //Compute position error:
00121     double error_position = command_ - joint_state_->position_;
00122 
00124     // TACTILES
00126     //you have access here to the whole data coming from the 5 tactiles at full speed.
00127     double my_first_finger_tactile_pac0 = actuator_->state_.tactiles_->at(0).biotac.pac0;
00128     if(loop_count_ % 10 == 0)
00129     {
00130       ROS_ERROR_STREAM("PAC0, tactile " << my_first_finger_tactile_pac0);
00131     }
00132 
00134     // EFFORT
00136     //Compute the commanded effort to send to the motor
00137     double commanded_effort = 0.0;
00138     //TODO: compute the force demand by combining the information you
00139     // want. You can have a look at the mixed controller to see a
00140     // working implementation of a controller using different pid loops.
00141 
00142     joint_state_->commanded_effort_ = commanded_effort;
00143 
00144     if(loop_count_ % 10 == 0)
00145     {
00146       if(controller_state_publisher_ && controller_state_publisher_->trylock())
00147       {
00148         controller_state_publisher_->msg_.header.stamp = time;
00149         controller_state_publisher_->msg_.set_point = command_;
00150 
00151         controller_state_publisher_->msg_.process_value = joint_state_->position_;
00152         controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
00153 
00154         controller_state_publisher_->msg_.error = error_position;
00155         controller_state_publisher_->msg_.time_step = dt_.toSec();
00156 
00157         controller_state_publisher_->msg_.command = commanded_effort;
00158         controller_state_publisher_->msg_.measured_effort = joint_state_->measured_effort_;
00159 
00160         controller_state_publisher_->unlockAndPublish();
00161       }
00162     }
00163     loop_count_++;
00164 
00165     last_time_ = time;
00166   }
00167 }
00168 
00169 /* For the emacs weenies in the crowd.
00170 Local Variables:
00171    c-basic-offset: 2
00172 End:
00173 */
00174 
00175 


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