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
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include "youbot/JointTrajectoryController.hpp"
00052 namespace youbot {
00053
00054 JointTrajectoryController::JointTrajectoryController() {
00055
00056 this->pid.initPid(80.0, 1, 0, 1000, -1000);
00057 time = boost::posix_time::microsec_clock::local_time();
00058 last_time = boost::posix_time::microsec_clock::local_time();
00059
00060 this->isControllerActive = false;
00061 this->targetPosition = 0;
00062 this->targetVelocity = 0;
00063 this->targetAcceleration = 0;
00064 this->encoderTicksPerRound = 1;
00065 this->gearRatio = 1;
00066 this->inverseDirection = false;
00067 actualpose = 0;
00068 actualvel = 0;
00069
00070
00071 boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
00072 SpecifiedTrajectory &traj = *traj_ptr;
00073 traj[0].start_time = boost::posix_time::microsec_clock::local_time();
00074 traj[0].duration = boost::posix_time::microseconds(0);
00075
00076 current_trajectory_box_.Set(traj_ptr);
00077
00078
00079 }
00080
00081 JointTrajectoryController::~JointTrajectoryController() {
00082
00083
00084 }
00085
00086 void JointTrajectoryController::getConfigurationParameter(double& PParameter, double& IParameter, double& DParameter, double& IClippingMax, double& IClippingMin) {
00087
00088 if (this->isControllerActive)
00089 throw JointParameterException("The trajectory controller is running");
00090 this->pid.getGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);
00091
00092 }
00093
00094 void JointTrajectoryController::setConfigurationParameter(const double PParameter, const double IParameter, const double DParameter, const double IClippingMax, const double IClippingMin) {
00095
00096 if (this->isControllerActive)
00097 throw JointParameterException("The trajectory controller is running");
00098 this->pid.setGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);
00099
00100 }
00101
00102 void JointTrajectoryController::setTrajectory(const JointTrajectory& input_traj) {
00103
00104
00105 if (input_traj.segments.size() == 0)
00106 throw std::runtime_error("Invalid trajectory");
00107
00108 boost::posix_time::ptime time_now = boost::posix_time::microsec_clock::local_time();
00109
00110 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00111 SpecifiedTrajectory &new_traj = *new_traj_ptr;
00112
00113
00114
00115 boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
00116 current_trajectory_box_.Get(prev_traj_ptr);
00117 if (!prev_traj_ptr) {
00118 throw std::runtime_error("The current trajectory can never be null");
00119 return;
00120 }
00121 const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
00122
00123
00124
00125
00126 int first_useful = -1;
00127 while (first_useful + 1 < (int) prev_traj.size() && prev_traj[first_useful + 1].start_time <= time_now) {
00128 ++first_useful;
00129 }
00130
00131
00132 int last_useful = -1;
00133 while (last_useful + 1 < (int) prev_traj.size() && prev_traj[last_useful + 1].start_time < time_now) {
00134 ++last_useful;
00135 }
00136
00137 if (last_useful < first_useful)
00138 first_useful = last_useful;
00139
00140
00141 for (int i = std::max(first_useful, 0); i <= last_useful; ++i) {
00142 new_traj.push_back(prev_traj[i]);
00143 }
00144
00145
00146
00147 if (new_traj.size() == 0)
00148 new_traj.push_back(prev_traj[prev_traj.size() - 1]);
00149
00150
00151
00152
00153 Segment &last = new_traj[new_traj.size() - 1];
00154 double prev_positions;
00155 double prev_velocities;
00156 double prev_accelerations;
00157
00158 LOG(debug) << "Initial conditions for new set of splines:";
00159
00160 sampleSplineWithTimeBounds(last.spline.coef, (double)last.duration.total_microseconds()/1000.0/1000.0,
00161 (double)last.start_time.time_of_day().total_microseconds()/1000.0/1000.0,
00162 prev_positions, prev_velocities, prev_accelerations);
00163
00164
00165
00166
00167
00168
00169
00170 double positions;
00171 double velocities;
00172 double accelerations;
00173
00174
00175
00176 std::vector<boost::posix_time::time_duration> durations;
00177 durations.push_back(input_traj.segments[0].time_from_start);
00178
00179 for (size_t i = 1; i < input_traj.segments.size(); ++i)
00180 durations.push_back(input_traj.segments[i].time_from_start - input_traj.segments[i - 1].time_from_start);
00181
00182 for (unsigned int i = 0; i < input_traj.segments.size(); ++i) {
00183 Segment seg;
00184
00185 seg.start_time = input_traj.start_time + input_traj.segments[i].time_from_start;
00186 seg.duration = durations[i];
00187
00188
00189 positions = input_traj.segments[i].positions.value();
00190 velocities = input_traj.segments[i].velocities.value();
00191 accelerations = input_traj.segments[i].accelerations.value();
00192
00193
00194
00195
00196
00197 getQuinticSplineCoefficients(
00198 prev_positions, prev_velocities, prev_accelerations,
00199 positions, velocities, accelerations,
00200 (double)durations[i].total_microseconds()/1000.0/1000.0,
00201 seg.spline.coef);
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 new_traj.push_back(seg);
00230
00231
00232
00233 prev_positions = positions;
00234 prev_velocities = velocities;
00235 prev_accelerations = accelerations;
00236 }
00237
00238
00239
00240 if (!new_traj_ptr) {
00241 throw std::runtime_error("The new trajectory was null!");
00242 return;
00243 }
00244
00245 current_trajectory_box_.Set(new_traj_ptr);
00246 LOG(debug) << "The new trajectory has " << new_traj.size() << " segments";
00247 this->isControllerActive = true;
00248
00249 }
00250
00251 void JointTrajectoryController::cancelCurrentTrajectory() {
00252
00253 boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
00254 SpecifiedTrajectory &traj = *traj_ptr;
00255 traj[0].start_time = boost::posix_time::microsec_clock::local_time();
00256 traj[0].duration = boost::posix_time::microseconds(0);
00257
00258 current_trajectory_box_.Set(traj_ptr);
00259 LOG(trace) << "Trajectory has been canceled";
00260 }
00261
00262 bool JointTrajectoryController::isTrajectoryControllerActive() {
00263 return this->isControllerActive;
00264 }
00265
00266 bool JointTrajectoryController::updateTrajectoryController(const SlaveMessageInput& actual, SlaveMessageOutput& velocity) {
00267
00268 time = boost::posix_time::microsec_clock::local_time();
00269 boost::posix_time::time_duration dt = time - last_time;
00270 last_time = time;
00271
00272 boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
00273 current_trajectory_box_.Get(traj_ptr);
00274 if (!traj_ptr || !this->isControllerActive) {
00275 this->isControllerActive = false;
00276
00277 return false;
00278 }
00279
00280
00281 const SpecifiedTrajectory &traj = *traj_ptr;
00282
00283
00284
00285 int seg = -1;
00286 while (seg + 1 < (int) traj.size() && traj[seg + 1].start_time < time) {
00287 ++seg;
00288 }
00289
00290 if (seg == -1) {
00291 if (traj.size() == 0)
00292 LOG(error) << "No segments in the trajectory";
00293 else
00294 LOG(error) << "No earlier segments.";
00295 return false;
00296 }
00297 if(seg == (int) traj.size()-1 && (traj[seg].start_time + traj[seg].duration) < time){
00298 LOG(trace) << "trajectory finished.";
00299 this->isControllerActive = false;
00300 velocity.value = 0;
00301 velocity.controllerMode = VELOCITY_CONTROL;
00302 return true;
00303 }
00304
00305
00306 duration = (double)traj[seg].duration.total_microseconds()/1000.0/1000.0;
00307 time_till_seg_start = (double)(time - traj[seg].start_time).total_microseconds()/1000.0/1000.0;
00308
00309 sampleSplineWithTimeBounds(traj[seg].spline.coef, duration, time_till_seg_start, targetPosition, targetVelocity, targetAcceleration);
00310
00311
00312 if(inverseDirection){
00313 actualpose = -actual.actualPosition;
00314 actualvel = -actual.actualVelocity;
00315 }else{
00316 actualpose = actual.actualPosition;
00317 actualvel = actual.actualVelocity;
00318 }
00319
00320 pose_error = ((actualpose / encoderTicksPerRound) * gearRatio * (2.0 * M_PI)) - targetPosition;
00321 velocity_error = ((actualvel/ 60.0) * gearRatio * 2.0 * M_PI) - targetVelocity ;
00322
00323 velsetpoint = pid.updatePid(pose_error, velocity_error, dt);
00324
00325 velocity.value = (int32) round((velsetpoint / (gearRatio * 2.0 * M_PI)) * 60.0);
00326
00327 velocity.controllerMode = VELOCITY_CONTROL;
00328
00329 if(inverseDirection){
00330 velocity.value = -velocity.value;
00331 }
00332
00333 return true;
00334
00335 }
00336
00337 void JointTrajectoryController::getLastTargetPosition(JointAngleSetpoint& position) {
00338
00339 position.angle = targetPosition* radian;
00340
00341 }
00342
00343 void JointTrajectoryController::getLastTargetVelocity(JointVelocitySetpoint& velocity) {
00344 velocity.angularVelocity = targetVelocity *radian_per_second;
00345 }
00346
00347 void JointTrajectoryController::getQuinticSplineCoefficients(const double start_pos, const double start_vel, const double start_acc, const double end_pos, const double end_vel, const double end_acc, const double time, std::vector<double>& coefficients) {
00348
00349 coefficients.resize(6);
00350
00351 if (time == 0.0) {
00352 coefficients[0] = end_pos;
00353 coefficients[1] = end_vel;
00354 coefficients[2] = 0.5 * end_acc;
00355 coefficients[3] = 0.0;
00356 coefficients[4] = 0.0;
00357 coefficients[5] = 0.0;
00358 } else {
00359 double T[6];
00360 generatePowers(5, time, T);
00361
00362 coefficients[0] = start_pos;
00363 coefficients[1] = start_vel;
00364 coefficients[2] = 0.5 * start_acc;
00365 coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] -
00366 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]);
00367 coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2] +
00368 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]);
00369 coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] -
00370 6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]);
00371 }
00372
00373 }
00374
00375 void JointTrajectoryController::sampleQuinticSpline(const std::vector<double>& coefficients, const double time, double& position, double& velocity, double& acceleration) {
00376
00377
00378 double t[6];
00379 generatePowers(5, time, t);
00380
00381 position = t[0] * coefficients[0] +
00382 t[1] * coefficients[1] +
00383 t[2] * coefficients[2] +
00384 t[3] * coefficients[3] +
00385 t[4] * coefficients[4] +
00386 t[5] * coefficients[5];
00387
00388 velocity = t[0] * coefficients[1] +
00389 2.0 * t[1] * coefficients[2] +
00390 3.0 * t[2] * coefficients[3] +
00391 4.0 * t[3] * coefficients[4] +
00392 5.0 * t[4] * coefficients[5];
00393
00394 acceleration = 2.0 * t[0] * coefficients[2] +
00395 6.0 * t[1] * coefficients[3] +
00396 12.0 * t[2] * coefficients[4] +
00397 20.0 * t[3] * coefficients[5];
00398
00399 }
00400
00401 void JointTrajectoryController::getCubicSplineCoefficients(const double start_pos, const double start_vel, const double end_pos, const double end_vel, const double time, std::vector<double>& coefficients) {
00402
00403 coefficients.resize(4);
00404
00405 if (time == 0.0) {
00406 coefficients[0] = end_pos;
00407 coefficients[1] = end_vel;
00408 coefficients[2] = 0.0;
00409 coefficients[3] = 0.0;
00410 } else {
00411 double T[4];
00412 generatePowers(3, time, T);
00413
00414 coefficients[0] = start_pos;
00415 coefficients[1] = start_vel;
00416 coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
00417 coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
00418 }
00419
00420 }
00421
00422 void JointTrajectoryController::generatePowers(const int n, const double x, double* powers) {
00423
00424 powers[0] = 1.0;
00425 for (int i = 1; i <= n; i++) {
00426 powers[i] = powers[i - 1] * x;
00427 }
00428
00429 }
00430
00431 void JointTrajectoryController::sampleSplineWithTimeBounds(const std::vector<double>& coefficients, const double duration, const double time, double& position, double& velocity, double& acceleration) {
00432
00433 if (time < 0) {
00434 double _;
00435 sampleQuinticSpline(coefficients, 0.0, position, _, _);
00436 velocity = 0;
00437 acceleration = 0;
00438 } else if (time > duration) {
00439 double _;
00440 sampleQuinticSpline(coefficients, duration, position, _, _);
00441 velocity = 0;
00442 acceleration = 0;
00443 } else {
00444 sampleQuinticSpline(coefficients, time,
00445 position, velocity, acceleration);
00446 }
00447
00448 }
00449
00450
00451 }