Go to the documentation of this file.00001 #include <pluginlib/class_list_macros.h>
00002 #include "robotican_controllers/posvel_gripper_controller.h"
00003 namespace gripper_controllers {
00004
00005
00007 bool PosVelGripperController::init(hardware_interface::PosVelJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
00008 {
00009
00010
00011 controller_nh_.param<double>("/gripper_controller/goal_tolerance", goal_tolerance_, 0.005);
00012 goal_tolerance_ = fabs(goal_tolerance_);
00013
00014 controller_nh_.param<double>("/gripper_controller/max_effort", default_max_effort_, 0.0);
00015 default_max_effort_ = fabs(default_max_effort_);
00016
00017 controller_nh_.param<double>("/gripper_controller/stall_velocity_threshold", stall_velocity_threshold_, 0.001);
00018 controller_nh_.param<double>("/gripper_controller/stall_timeout", stall_timeout_, 1.0);
00019
00020 controller_nh_.param<double>("/gripper_controller/joints_vel", joints_vel_, 0.05);
00021
00022
00023 std::string left_finger_joint_name="left_finger_joint";
00024 std::string right_finger_joint_name="right_finger_joint";
00025 controller_nh_.getParam("/gripper_controller/left_joint", left_finger_joint_name);
00026 controller_nh_.getParam("/gripper_controller/right_joint", right_finger_joint_name);
00027 leftjoint = hw->getHandle(left_finger_joint_name);
00028 rightjoint = hw->getHandle(right_finger_joint_name);
00029
00030
00031 controller_nh_ = controller_nh;
00032
00033
00034 double action_monitor_rate = 20.0;
00035 controller_nh_.getParam("/gripper_controller/action_monitor_rate", action_monitor_rate);
00036 action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
00037 ROS_DEBUG("Action status changes will be monitored at %f Hz",action_monitor_rate);
00038
00039
00040 pre_alloc_result_.reset(new control_msgs::GripperCommandResult());
00041 pre_alloc_result_->position = command_struct_.gap;
00042 pre_alloc_result_->reached_goal = false;
00043 pre_alloc_result_->stalled = false;
00044
00045 action_server_.reset(new ActionServer(controller_nh, "gripper_cmd",
00046 boost::bind(&PosVelGripperController::goalCB, this, _1),
00047 boost::bind(&PosVelGripperController::cancelCB, this, _1),
00048 false));
00049
00050 action_server_->start();
00051
00052 return true;
00053 }
00054
00055
00057 void PosVelGripperController::starting(const ros::Time& time)
00058 {
00059 command_struct_rt_.gap = pos2Gap(rightjoint.getPosition());
00060 command_struct_rt_.max_effort = default_max_effort_;
00061 command_.initRT(command_struct_rt_);
00062
00063 _lastGap = command_struct_rt_.gap;
00064
00065
00066
00067 leftjoint.setCommand(leftjoint.getPosition(),joints_vel_);
00068 rightjoint.setCommand(rightjoint.getPosition(),joints_vel_);
00069
00070 last_movement_time_ = time;
00071 }
00072
00073
00075 void PosVelGripperController::update(const ros::Time& time, const ros::Duration& period)
00076 {
00077 command_struct_rt_ = *(command_.readFromRT());
00078
00079 double current_gap = pos2Gap(rightjoint.getPosition());
00080
00081 double current_gap_velocity = current_gap - _lastGap / period.toSec();
00082 double current_effort = (fabs(leftjoint.getEffort())>fabs(rightjoint.getEffort())) ? fabs(leftjoint.getEffort()):fabs(rightjoint.getEffort());
00083 double error_gap = command_struct_rt_.gap - current_gap;
00084 double error_velocity = - current_gap_velocity;
00085 _lastGap = current_gap;
00086 if (command_struct_rt_.max_effort==0) current_effort=-1;
00087 checkForSuccess(time, error_gap, current_gap, current_gap_velocity, current_effort,command_struct_rt_.max_effort);
00088
00089
00090 double jointsPos = gap2Pos(command_struct_rt_.gap);
00091
00092
00093 leftjoint.setCommand(-jointsPos,joints_vel_);
00094 rightjoint.setCommand(jointsPos,joints_vel_);
00095
00096
00097 }
00098
00099
00101 void PosVelGripperController::stopping()
00102 {
00103
00104 }
00105
00106 void PosVelGripperController::preemptActiveGoal()
00107 {
00108 RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00109
00110
00111 if (current_active_goal)
00112 {
00113
00114 rt_active_goal_.reset();
00115 if(current_active_goal->gh_.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE)
00116 current_active_goal->gh_.setCanceled();
00117 }
00118 }
00119
00120
00121 void PosVelGripperController::goalCB(GoalHandle gh)
00122 {
00123 ROS_INFO("Recieved new action goal");
00124
00125
00126 if (!this->isRunning())
00127 {
00128 ROS_ERROR("Can't accept new action goals. Controller is not running.");
00129 control_msgs::GripperCommandResult result;
00130 gh.setRejected(result);
00131 return;
00132 }
00133
00134
00135 RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
00136
00137
00138 preemptActiveGoal();
00139 gh.setAccepted();
00140
00141
00142
00143
00144 command_struct_.gap = gh.getGoal()->command.position;
00145 command_struct_.max_effort = gh.getGoal()->command.max_effort;
00146 command_.writeFromNonRT(command_struct_);
00147 pre_alloc_result_->reached_goal = false;
00148 pre_alloc_result_->stalled = false;
00149 last_movement_time_ = ros::Time::now();
00150
00151 goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
00152 &RealtimeGoalHandle::runNonRealtime,
00153 rt_goal);
00154 goal_handle_timer_.start();
00155 rt_active_goal_ = rt_goal;
00156 ROS_INFO("GRIPPER: Got new goal");
00157 }
00158
00159
00160 void PosVelGripperController::cancelCB(GoalHandle gh)
00161 {
00162 RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
00163
00164
00165 if (current_active_goal && current_active_goal->gh_ == gh)
00166 {
00167
00168 rt_active_goal_.reset();
00169
00170
00171 setHoldPosition();
00172 ROS_DEBUG( "Canceling active action goal because cancel callback recieved from actionlib.");
00173
00174
00175 current_active_goal->gh_.setCanceled();
00176 }
00177 }
00178
00179 double PosVelGripperController::gap2Pos(double gap) {
00180 double max_gap=0.15;
00181 double rad_when_stright=0.144;
00182 double half_gap_on_zero_rad=0.021;
00183 double tip_r=0.09;
00184
00185 if (gap <0) gap=0;
00186 else if (gap>max_gap) gap=max_gap;
00187
00188
00189 double computeGap = ((gap / 2.0f) - half_gap_on_zero_rad) / tip_r;
00190 const double gap2Pos = asin(computeGap) - rad_when_stright;
00191 return gap2Pos;
00192
00193 }
00194
00195 double PosVelGripperController::pos2Gap(double pos) {
00196 double rad_when_stright=0.144;
00197 double half_gap_on_zero_rad=0.021;
00198 double tip_r=0.09;
00199 return 2 * (half_gap_on_zero_rad + tip_r * sin(pos + rad_when_stright));
00200 }
00201
00202 void PosVelGripperController::setHoldPosition()
00203 {
00204
00205 command_struct_.gap = pos2Gap(rightjoint.getPosition());
00206
00207 command_.writeFromNonRT(command_struct_);
00208
00209 }
00210
00211
00212 void PosVelGripperController::checkForSuccess(const ros::Time &time, double error_gap, double current_gap, double current_gap_velocity,
00213 double current_effort,double max_effort)
00214 {
00215
00216
00217 if(!rt_active_goal_) {
00218 return;
00219 }
00220
00221 if(rt_active_goal_->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE){
00222 return;
00223 }
00224 if(fabs(error_gap) < goal_tolerance_)
00225 {
00226 pre_alloc_result_->effort = current_effort;
00227 pre_alloc_result_->position = current_gap;
00228 pre_alloc_result_->reached_goal = true;
00229 pre_alloc_result_->stalled = false;
00230 rt_active_goal_->setSucceeded(pre_alloc_result_);
00231 ROS_INFO("GRIPPER: Reached Goal");
00232 }
00233 else
00234 {
00235 if (current_effort >= fabs(max_effort)) {
00236 ROS_WARN("GRIPPER: MAX EFFORT");
00237 pre_alloc_result_->effort = current_effort;
00238 pre_alloc_result_->position = current_gap;
00239 pre_alloc_result_->reached_goal = false;
00240 pre_alloc_result_->stalled = true;
00241
00242 ROS_WARN("pre_alloc_result_->effort: %f", pre_alloc_result_->effort);
00243 ROS_WARN("pre_alloc_result_->position: %f", pre_alloc_result_->position);
00244
00245 rt_active_goal_->setSucceeded(pre_alloc_result_);
00246 setHoldPosition();
00247 return;
00248 }
00249
00250 if(fabs(current_gap_velocity) > stall_velocity_threshold_) {
00251
00252 last_movement_time_ = time;
00253 }
00254 else if( (time - last_movement_time_).toSec() > stall_timeout_)
00255 {
00256 ROS_WARN("GRIPPER: STALLED");
00257 pre_alloc_result_->effort = current_effort;
00258 pre_alloc_result_->position = current_gap;
00259 pre_alloc_result_->reached_goal = false;
00260 pre_alloc_result_->stalled = true;
00261 rt_active_goal_->setAborted(pre_alloc_result_);
00262 setHoldPosition();
00263
00264 }
00265 }
00266 }
00267
00268 }