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_EXPORT_CLASS( controller::SrhSyntouchController, controller_interface::ControllerBase) 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(ros_ethercat_model::RobotState *robot, ros::NodeHandle &n) 00051 { 00052 ROS_ASSERT(robot); 00053 robot_ = robot; 00054 node_ = n; 00055 00056 if (!node_.getParam("joint", joint_name_)) { 00057 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str()); 00058 return false; 00059 } 00060 00061 ROS_DEBUG(" --------- "); 00062 ROS_DEBUG_STREAM("Init: " << joint_name_); 00063 00064 joint_state_ = robot_->getJointState(joint_name_); 00065 if (!joint_state_) 00066 { 00067 ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n", 00068 joint_name_.c_str()); 00069 return false; 00070 } 00071 if (!joint_state_->calibrated_) 00072 { 00073 ROS_ERROR("Joint %s not calibrated for SrhSyntouchController", joint_name_.c_str()); 00074 return false; 00075 } 00076 00077 //init the pointer to the biotacs data, updated at 1kHz 00078 actuator_ = static_cast<sr_actuator::SrMotorActuator*>( robot->getActuator( joint_name_ ) ); 00079 00080 after_init(); 00081 return true; 00082 } 00083 00084 00085 void SrhSyntouchController::starting(const ros::Time& time) 00086 { 00087 command_ = joint_state_->position_; 00088 00089 ROS_WARN_STREAM("Reseting PID for joint " << joint_state_->joint_->name); 00090 } 00091 00092 void SrhSyntouchController::update(const ros::Time& time, const ros::Duration& period) 00093 { 00094 if (!joint_state_->calibrated_) 00095 return; 00096 00097 ROS_ASSERT(robot_); 00098 ROS_ASSERT(joint_state_->joint_); 00099 00100 if (initialized_) 00101 command_ = joint_state_->commanded_position_; 00102 else 00103 { 00104 initialized_ = true; 00105 command_ = joint_state_->position_; 00106 } 00107 00109 // POSITION 00111 //Compute position error: 00112 double error_position = command_ - joint_state_->position_; 00113 00115 // TACTILES 00117 //you have access here to the whole data coming from the 5 tactiles at full speed. 00118 double my_first_finger_tactile_pac0 = actuator_->motor_state_.tactiles_->at(0).biotac.pac0; 00119 if(loop_count_ % 10 == 0) 00120 { 00121 ROS_ERROR_STREAM("PAC0, tactile " << my_first_finger_tactile_pac0); 00122 } 00123 00125 // EFFORT 00127 //Compute the commanded effort to send to the motor 00128 double commanded_effort = 0.0; 00129 //TODO: compute the force demand by combining the information you 00130 // want. You can have a look at the mixed controller to see a 00131 // working implementation of a controller using different pid loops. 00132 00133 joint_state_->commanded_effort_ = commanded_effort; 00134 00135 if(loop_count_ % 10 == 0) 00136 { 00137 if(controller_state_publisher_ && controller_state_publisher_->trylock()) 00138 { 00139 controller_state_publisher_->msg_.header.stamp = time; 00140 controller_state_publisher_->msg_.set_point = command_; 00141 00142 controller_state_publisher_->msg_.process_value = joint_state_->position_; 00143 controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_; 00144 00145 controller_state_publisher_->msg_.error = error_position; 00146 controller_state_publisher_->msg_.time_step = period.toSec(); 00147 00148 controller_state_publisher_->msg_.command = commanded_effort; 00149 controller_state_publisher_->msg_.measured_effort = joint_state_->effort_; 00150 00151 controller_state_publisher_->unlockAndPublish(); 00152 } 00153 } 00154 loop_count_++; 00155 } 00156 } 00157 00158 /* For the emacs weenies in the crowd. 00159 Local Variables: 00160 c-basic-offset: 2 00161 End: 00162 */ 00163 00164