00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "joint_qualification_controllers/wrist_difference_controller.h"
00036 #include "urdf_model/joint.h"
00037
00038 PLUGINLIB_EXPORT_CLASS(joint_qualification_controllers::WristDifferenceController,
00039 pr2_controller_interface::Controller)
00040
00041 #define MAX_DATA_POINTS 120000
00042
00043 using namespace std;
00044 using namespace joint_qualification_controllers;
00045
00046 WristDifferenceController::WristDifferenceController()
00047 : flex_joint_(NULL),
00048 roll_joint_(NULL),
00049 robot_(NULL),
00050 data_sent_(false),
00051 wrist_data_pub_(NULL)
00052 {
00053 wrist_test_data_.left_turn.time.resize(MAX_DATA_POINTS);
00054 wrist_test_data_.left_turn.flex_position.resize(MAX_DATA_POINTS);
00055 wrist_test_data_.left_turn.flex_effort.resize(MAX_DATA_POINTS);
00056 wrist_test_data_.left_turn.flex_cmd.resize(MAX_DATA_POINTS);
00057 wrist_test_data_.left_turn.roll_position.resize(MAX_DATA_POINTS);
00058 wrist_test_data_.left_turn.roll_effort.resize(MAX_DATA_POINTS);
00059 wrist_test_data_.left_turn.roll_cmd.resize(MAX_DATA_POINTS);
00060 wrist_test_data_.left_turn.roll_velocity.resize(MAX_DATA_POINTS);
00061
00062 wrist_test_data_.right_turn.time.resize(MAX_DATA_POINTS);
00063 wrist_test_data_.right_turn.flex_position.resize(MAX_DATA_POINTS);
00064 wrist_test_data_.right_turn.flex_effort.resize(MAX_DATA_POINTS);
00065 wrist_test_data_.right_turn.flex_cmd.resize(MAX_DATA_POINTS);
00066 wrist_test_data_.right_turn.roll_position.resize(MAX_DATA_POINTS);
00067 wrist_test_data_.right_turn.roll_effort.resize(MAX_DATA_POINTS);
00068 wrist_test_data_.right_turn.roll_cmd.resize(MAX_DATA_POINTS);
00069 wrist_test_data_.right_turn.roll_velocity.resize(MAX_DATA_POINTS);
00070
00071 wrist_test_data_.flex_pid.resize(4);
00072 wrist_test_data_.roll_pid.resize(4);
00073
00074 wrist_test_data_.arg_name.resize(10);
00075 wrist_test_data_.arg_value.resize(10);
00076 wrist_test_data_.arg_name[0] = "Flex Position";
00077 wrist_test_data_.arg_name[1] = "Roll Velocity";
00078 wrist_test_data_.arg_name[2] = "Roll Tolerance (%)";
00079 wrist_test_data_.arg_name[3] = "Roll SD Max (%)";
00080 wrist_test_data_.arg_name[4] = "Timeout";
00081 wrist_test_data_.arg_name[5] = "Left Effort";
00082 wrist_test_data_.arg_name[6] = "Right Effort";
00083 wrist_test_data_.arg_name[7] = "Flex Tolerance";
00084 wrist_test_data_.arg_name[8] = "Flex Max Value";
00085 wrist_test_data_.arg_name[9] = "Flex SD";
00086
00087 wrist_test_data_.timeout = false;
00088
00089 state_ = STARTING;
00090 starting_count = 0;
00091 roll_velocity_ = 0;
00092 flex_position_ = 0;
00093 initial_time_ = ros::Time(0);
00094 left_count_ = 0;
00095 right_count_ = 0;
00096 start_count_ = 0;
00097
00098 timeout_ = MAX_DATA_POINTS / 1000;
00099 }
00100
00101 WristDifferenceController::~WristDifferenceController()
00102 {
00103 delete flex_controller_;
00104 delete roll_controller_;
00105 }
00106
00107 bool WristDifferenceController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00108 {
00109 ROS_ASSERT(robot);
00110 robot_ = robot;
00111
00112 std::string roll_name;
00113 if (!n.getParam("roll_velocity_controller/joint", roll_name)){
00114 ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
00115 n.getNamespace().c_str());
00116 return false;
00117 }
00118 if (!(roll_joint_ = robot->getJointState(roll_name)))
00119 {
00120 ROS_ERROR("WristDifferenceController could not find joint named \"%s\"\n", roll_name.c_str());
00121 return false;
00122 }
00123 ROS_DEBUG("Roll joint: %s", roll_name.c_str());
00124 if (roll_joint_->joint_->type != urdf::Joint::CONTINUOUS)
00125 {
00126 ROS_ERROR("Wrist roll joint must be continuous. Unable to check wrist symmetry. Roll joint: %s", roll_name.c_str());
00127 return false;
00128 }
00129
00130 if (!n.getParam("roll_velocity", roll_velocity_)){
00131 ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
00132 n.getNamespace().c_str());
00133 return false;
00134 }
00135 roll_velocity_ = fabs(roll_velocity_);
00136
00137 std::string flex_name;
00138 if (!n.getParam("flex_position_controller/joint", flex_name)){
00139 ROS_ERROR("Hysteresis Controller: No joint name found on parameter namespace: %s)",
00140 n.getNamespace().c_str());
00141 return false;
00142 }
00143 if (!(flex_joint_ = robot->getJointState(flex_name)))
00144 {
00145 ROS_ERROR("WristDifferenceController could not find joint named \"%s\"\n", flex_name.c_str());
00146 return false;
00147 }
00148
00149 if (!n.getParam("flex_position", flex_position_)) {
00150 ROS_ERROR("Hysteresis Controller: No velocity found on parameter namespace: %s)",
00151 n.getNamespace().c_str());
00152 return false;
00153 }
00154
00155 if (!n.getParam("timeout", timeout_)){
00156 ROS_ERROR("Hysteresis Controller: No timeout found on parameter namespace: %s)",
00157 n.getNamespace().c_str());
00158 return false;
00159 }
00160
00161 if (!n.getParam("tolerance", tolerance_)){
00162 ROS_WARN("Parameter 'tolerance' is not set on namespace: %s.",
00163 n.getNamespace().c_str());
00164 return false;
00165 }
00166
00167 if (!n.getParam("sd_max", sd_max_)) {
00168 ROS_WARN("Parameter 'sd_max' is not set on namespace: %s.",
00169 n.getNamespace().c_str());
00170 return false;
00171 }
00172
00173 double left_effort, right_effort, flex_tolerance, flex_max, flex_sd;
00174 if (!n.getParam("left_effort", left_effort)) {
00175 ROS_WARN("Parameter 'left_effort' is not set on namespace: %s.",
00176 n.getNamespace().c_str());
00177 return false;
00178 }
00179
00180 if (!n.getParam("right_effort", right_effort)) {
00181 ROS_WARN("Parameter 'right_effort' is not set on namespace: %s.",
00182 n.getNamespace().c_str());
00183 return false;
00184 }
00185
00186 if (!n.getParam("flex_tolerance", flex_tolerance)) {
00187 ROS_WARN("Parameter 'flex_tolerance' is not set on namespace: %s.",
00188 n.getNamespace().c_str());
00189 return false;
00190 }
00191
00192
00193 if (!n.getParam("flex_max", flex_max)) {
00194 ROS_WARN("Parameter 'flex_max' is not set on namespace: %s.",
00195 n.getNamespace().c_str());
00196 return false;
00197 }
00198
00199 if (!n.getParam("flex_sd", flex_sd)) {
00200 ROS_WARN("Parameter 'flex_sd' is not set on namespace: %s.",
00201 n.getNamespace().c_str());
00202 return false;
00203 }
00204
00205 initial_time_ = robot_->getTime();
00206
00207
00208 wrist_test_data_.flex_joint = flex_name;
00209 wrist_test_data_.roll_joint = roll_name;
00210 wrist_test_data_.arg_value[0] = flex_position_;
00211 wrist_test_data_.arg_value[1] = roll_velocity_;
00212 wrist_test_data_.arg_value[2] = tolerance_;
00213 wrist_test_data_.arg_value[3] = sd_max_;
00214 wrist_test_data_.arg_value[4] = timeout_;
00215 wrist_test_data_.arg_value[5] = left_effort;
00216 wrist_test_data_.arg_value[6] = right_effort;
00217 wrist_test_data_.arg_value[7] = flex_tolerance;
00218 wrist_test_data_.arg_value[8] = flex_max;
00219 wrist_test_data_.arg_value[9] = flex_sd;
00220
00221 flex_controller_ = new controller::JointPositionController();
00222 ros::NodeHandle n_flex(n, "flex_position_controller");
00223 if (!flex_controller_->init(robot, n_flex)) return false;
00224
00225 roll_controller_ = new controller::JointVelocityController();
00226 ros::NodeHandle n_roll(n, "roll_velocity_controller");
00227 if (!roll_controller_->init(robot, n_roll)) return false;
00228
00229
00230 double p, i, d, iClamp, imin;
00231 roll_controller_->getGains(p, i, d, iClamp, imin);
00232 wrist_test_data_.roll_pid[0] = p;
00233 wrist_test_data_.roll_pid[1] = i;
00234 wrist_test_data_.roll_pid[2] = d;
00235 wrist_test_data_.roll_pid[3] = iClamp;
00236
00237 flex_controller_->getGains(p, i, d, iClamp, imin);
00238 wrist_test_data_.flex_pid[0] = p;
00239 wrist_test_data_.flex_pid[1] = i;
00240 wrist_test_data_.flex_pid[2] = d;
00241 wrist_test_data_.flex_pid[3] = iClamp;
00242
00243 wrist_data_pub_.reset(new realtime_tools::RealtimePublisher<joint_qualification_controllers::WristDiffData>(n, "/test_data", 1, true));
00244
00245 return true;
00246 }
00247
00248 void WristDifferenceController::starting()
00249 {
00250 roll_controller_->starting();
00251 flex_controller_->starting();
00252
00253 initial_time_ = robot_->getTime();
00254 }
00255
00256 void WristDifferenceController::update()
00257 {
00258
00259 if(!flex_joint_->calibrated_ || !roll_joint_->calibrated_)
00260 {
00261 return;
00262 }
00263
00264 ros::Time time = robot_->getTime();
00265 flex_controller_->update();
00266 roll_controller_->update();
00267
00268
00269 if ((time - initial_time_).toSec() > timeout_ && state_ != ANALYZING && state_ != DONE)
00270 {
00271 state_ = ANALYZING;
00272 wrist_test_data_.timeout = true;
00273 roll_controller_->setCommand(0.0);
00274 }
00275
00276 switch (state_)
00277 {
00278 case STARTING:
00279 roll_controller_->setCommand(roll_velocity_);
00280 flex_controller_->setCommand(flex_position_);
00281 start_count_++;
00282
00283 if (start_count_++ > 3000)
00284 {
00285 initial_position_ = roll_joint_->position_;
00286 state_ = LEFT;
00287 }
00288 break;
00289 case LEFT:
00290 if (left_count_ < MAX_DATA_POINTS)
00291 {
00292 wrist_test_data_.left_turn.time [left_count_] = time.toSec();
00293 wrist_test_data_.left_turn.flex_position[left_count_] = flex_joint_->position_;
00294 wrist_test_data_.left_turn.flex_effort [left_count_] = flex_joint_->measured_effort_;
00295 wrist_test_data_.left_turn.flex_cmd [left_count_] = flex_joint_->commanded_effort_;
00296
00297 wrist_test_data_.left_turn.roll_position[left_count_] = roll_joint_->position_;
00298 wrist_test_data_.left_turn.roll_effort [left_count_] = roll_joint_->measured_effort_;
00299 wrist_test_data_.left_turn.roll_cmd [left_count_] = roll_joint_->commanded_effort_;
00300 wrist_test_data_.left_turn.roll_velocity[left_count_] = roll_joint_->velocity_;
00301 left_count_++;
00302 }
00303
00304 if(fabs(roll_joint_->position_ - initial_position_) > 6.28 || left_count_ >= MAX_DATA_POINTS)
00305 {
00306 double right_vel = -1 * roll_velocity_;
00307 roll_controller_->setCommand(right_vel);
00308 initial_position_ = roll_joint_->position_;
00309 state_ = RIGHT;
00310 }
00311 break;
00312 case RIGHT:
00313 if (right_count_ < MAX_DATA_POINTS)
00314 {
00315 wrist_test_data_.right_turn.time [right_count_] = time.toSec();
00316 wrist_test_data_.right_turn.flex_position[right_count_] = flex_joint_->position_;
00317 wrist_test_data_.right_turn.flex_effort [right_count_] = flex_joint_->measured_effort_;
00318 wrist_test_data_.right_turn.flex_cmd [right_count_] = flex_joint_->commanded_effort_;
00319
00320 wrist_test_data_.right_turn.roll_position[right_count_] = roll_joint_->position_;
00321 wrist_test_data_.right_turn.roll_effort [right_count_] = roll_joint_->measured_effort_;
00322 wrist_test_data_.right_turn.roll_cmd [right_count_] = roll_joint_->commanded_effort_;
00323 wrist_test_data_.right_turn.roll_velocity[right_count_] = roll_joint_->velocity_;
00324 right_count_++;
00325 }
00326
00327 if(fabs(roll_joint_->position_ - initial_position_) > 6.28 || right_count_ >= MAX_DATA_POINTS)
00328 {
00329 roll_controller_->setCommand(0.0);
00330 state_ = ANALYZING;
00331 }
00332 break;
00333 case ANALYZING:
00334 roll_controller_->setCommand(0.0);
00335 analysis();
00336 state_ = DONE;
00337 break;
00338 case DONE:
00339 roll_controller_->setCommand(0.0);
00340 if (!data_sent_)
00341 data_sent_ = sendData();
00342 break;
00343 }
00344 }
00345
00346 void WristDifferenceController::analysis()
00347 {
00348
00349 if (left_count_ == 0)
00350 left_count_ = 1;
00351
00352 if (right_count_ == 0)
00353 right_count_ = 1;
00354
00355 wrist_test_data_.left_turn.time.resize(left_count_);
00356 wrist_test_data_.left_turn.flex_position.resize(left_count_);
00357 wrist_test_data_.left_turn.flex_effort.resize(left_count_);
00358 wrist_test_data_.left_turn.roll_cmd.resize(left_count_);
00359 wrist_test_data_.left_turn.roll_position.resize(left_count_);
00360 wrist_test_data_.left_turn.roll_effort.resize(left_count_);
00361 wrist_test_data_.left_turn.roll_cmd.resize(left_count_);
00362 wrist_test_data_.left_turn.roll_velocity.resize(left_count_);
00363
00364 wrist_test_data_.right_turn.time.resize(right_count_);
00365 wrist_test_data_.right_turn.flex_position.resize(right_count_);
00366 wrist_test_data_.right_turn.flex_effort.resize(right_count_);
00367 wrist_test_data_.right_turn.roll_cmd.resize(right_count_);
00368 wrist_test_data_.right_turn.roll_position.resize(right_count_);
00369 wrist_test_data_.right_turn.roll_effort.resize(right_count_);
00370 wrist_test_data_.right_turn.roll_cmd.resize(right_count_);
00371 wrist_test_data_.right_turn.roll_velocity.resize(right_count_);
00372
00373 return;
00374 }
00375
00376 bool WristDifferenceController::sendData()
00377 {
00378 if (wrist_data_pub_->trylock())
00379 {
00380 joint_qualification_controllers::WristDiffData *out = &wrist_data_pub_->msg_;
00381 out->flex_joint = wrist_test_data_.flex_joint;
00382 out->roll_joint = wrist_test_data_.roll_joint;
00383 out->flex_pid = wrist_test_data_.flex_pid;
00384 out->roll_pid = wrist_test_data_.roll_pid;
00385 out->arg_name = wrist_test_data_.arg_name;
00386 out->arg_value = wrist_test_data_.arg_value;
00387 out->left_turn = wrist_test_data_.left_turn;
00388 out->right_turn = wrist_test_data_.right_turn;
00389 out->timeout = wrist_test_data_.timeout;
00390
00391 wrist_data_pub_->unlockAndPublish();
00392 return true;
00393 }
00394 return false;
00395 }
00396
00397
00398