JointTrajectoryController.cpp
Go to the documentation of this file.
1 /****************************************************************
2  *
3  * Copyright (c) 2011
4  * All rights reserved.
5  *
6  * Hochschule Bonn-Rhein-Sieg
7  * University of Applied Sciences
8  * Computer Science Department
9  *
10  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11  *
12  * Author:
13  * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
14  * Supervised by:
15  * Gerhard K. Kraetzschmar
16  *
17  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18  *
19  * This sofware is published under a dual-license: GNU Lesser General Public
20  * License LGPL 2.1 and BSD license. The dual-license implies that users of this
21  * code may choose which terms they prefer.
22  *
23  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24  *
25  * Redistribution and use in source and binary forms, with or without
26  * modification, are permitted provided that the following conditions are met:
27  *
28  * * Redistributions of source code must retain the above copyright
29  * notice, this list of conditions and the following disclaimer.
30  * * Redistributions in binary form must reproduce the above copyright
31  * notice, this list of conditions and the following disclaimer in the
32  * documentation and/or other materials provided with the distribution.
33  * * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
34  * contributors may be used to endorse or promote products derived from
35  * this software without specific prior written permission.
36  *
37  * This program is free software: you can redistribute it and/or modify
38  * it under the terms of the GNU Lesser General Public License LGPL as
39  * published by the Free Software Foundation, either version 2.1 of the
40  * License, or (at your option) any later version or the BSD license.
41  *
42  * This program is distributed in the hope that it will be useful,
43  * but WITHOUT ANY WARRANTY; without even the implied warranty of
44  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
45  * GNU Lesser General Public License LGPL and the BSD license for more details.
46  *
47  * You should have received a copy of the GNU Lesser General Public
48  * License LGPL and BSD license along with this program.
49  *
50  ****************************************************************/
53 
54 namespace youbot {
55 
57 
58  this->pid.initPid(80.0, 1, 0, 1000, -1000);
59  time = boost::posix_time::microsec_clock::local_time();
60  last_time = boost::posix_time::microsec_clock::local_time();
61 
62  this->isControllerActive = false;
63  this->targetPosition = 0;
64  this->targetVelocity = 0;
65  this->targetAcceleration = 0;
66  this->encoderTicksPerRound = 1;
67  this->gearRatio = 1;
68  this->inverseDirection = false;
69  actualpose = 0;
70  actualvel = 0;
71 
72  // Creates a dummy trajectory
74  SpecifiedTrajectory &traj = *traj_ptr;
75  traj[0].start_time = boost::posix_time::microsec_clock::local_time();
76  traj[0].duration = boost::posix_time::microseconds(0);
77  //traj[0].splines.coef[0] = 0.0;
78  current_trajectory_box_.Set(traj_ptr);
79 
80 
81  }
82 
84 
85 
86  }
87 
88  void JointTrajectoryController::getConfigurationParameter(double& PParameter, double& IParameter, double& DParameter, double& IClippingMax, double& IClippingMin) {
89 
90  if (this->isControllerActive)
91  throw JointParameterException("The trajectory controller is running");
92  this->pid.getGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);
93 
94  }
95 
96  void JointTrajectoryController::setConfigurationParameter(const double PParameter, const double IParameter, const double DParameter, const double IClippingMax, const double IClippingMin) {
97 
98  if (this->isControllerActive)
99  throw JointParameterException("The trajectory controller is running");
100  this->pid.setGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);
101 
102  }
103 
105 
106 
107  if (input_traj.segments.size() == 0)
108  throw std::runtime_error("Invalid trajectory");
109 
110  boost::posix_time::ptime time_now = boost::posix_time::microsec_clock::local_time();
111 
113  SpecifiedTrajectory &new_traj = *new_traj_ptr;
114 
115  // ------ Grabs the trajectory that we're currently following.
116 
118  current_trajectory_box_.Get(prev_traj_ptr);
119  if (!prev_traj_ptr) {
120  throw std::runtime_error("The current trajectory can never be null");
121  return;
122  }
123  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;
124 
125  // ------ Copies over the segments from the previous trajectory that are still useful.
126 
127  // Useful segments are still relevant after the current time.
128  int first_useful = -1;
129  while (first_useful + 1 < (int) prev_traj.size() && prev_traj[first_useful + 1].start_time <= time_now) {
130  ++first_useful;
131  }
132 
133  // Useful segments are not going to be completely overwritten by the message's splines.
134  int last_useful = -1;
135  while (last_useful + 1 < (int) prev_traj.size() && prev_traj[last_useful + 1].start_time < time_now) {
136  ++last_useful;
137  }
138 
139  if (last_useful < first_useful)
140  first_useful = last_useful;
141 
142  // Copies over the old segments that were determined to be useful.
143  for (int i = std::max(first_useful, 0); i <= last_useful; ++i) {
144  new_traj.push_back(prev_traj[i]);
145  }
146 
147  // We always save the last segment so that we know where to stop if
148  // there are no new segments.
149  if (new_traj.size() == 0)
150  new_traj.push_back(prev_traj[prev_traj.size() - 1]);
151 
152  // ------ Determines when and where the new segments start
153 
154  // Finds the end conditions of the final segment
155  Segment &last = new_traj[new_traj.size() - 1];
156  double prev_positions;
157  double prev_velocities;
158  double prev_accelerations;
159 
160  LOG(debug) << "Initial conditions for new set of splines:";
161 
162  sampleSplineWithTimeBounds(last.spline.coef, (double)last.duration.total_microseconds()/1000.0/1000.0,
163  (double)last.start_time.time_of_day().total_microseconds()/1000.0/1000.0,
164  prev_positions, prev_velocities, prev_accelerations);
165  // ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
166  // prev_accelerations[i], joints_[i]->joint_->name.c_str());
167 
168 
169  // ------ Tacks on the new segments
170 
171 
172  double positions;
173  double velocities;
174  double accelerations;
175 
176 
177 
178  std::vector<boost::posix_time::time_duration> durations;
179  durations.push_back(input_traj.segments[0].time_from_start);
180 
181  for (size_t i = 1; i < input_traj.segments.size(); ++i)
182  durations.push_back(input_traj.segments[i].time_from_start - input_traj.segments[i - 1].time_from_start);
183 
184  for (unsigned int i = 0; i < input_traj.segments.size(); ++i) {
185  Segment seg;
186 
187  seg.start_time = input_traj.start_time + input_traj.segments[i].time_from_start;
188  seg.duration = durations[i];
189 
190 
191  positions = input_traj.segments[i].positions.value();
192  velocities = input_traj.segments[i].velocities.value();
193  accelerations = input_traj.segments[i].accelerations.value();
194 
195  // Converts the boundary conditions to splines.
196 
197  // if (prev_accelerations.size() > 0 && accelerations.size() > 0)
198  // {
200  prev_positions, prev_velocities, prev_accelerations,
201  positions, velocities, accelerations,
202  (double)durations[i].total_microseconds()/1000.0/1000.0,
203  seg.spline.coef);
204  /*
205  }
206  else if (prev_velocities.size() > 0 && velocities.size() > 0)
207  {
208  getCubicSplineCoefficients(
209  prev_positions[j], prev_velocities[j],
210  positions[j], velocities[j],
211  durations[i],
212  seg.splines[j].coef);
213  seg.splines[j].coef.resize(6, 0.0);
214  }
215  else
216  {
217  seg.splines[j].coef[0] = prev_positions[j];
218  if (durations[i] == 0.0)
219  seg.splines[j].coef[1] = 0.0;
220  else
221  seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
222  seg.splines[j].coef[2] = 0.0;
223  seg.splines[j].coef[3] = 0.0;
224  seg.splines[j].coef[4] = 0.0;
225  seg.splines[j].coef[5] = 0.0;
226  }
227  */
228 
229  // Pushes the splines onto the end of the new trajectory.
230 
231  new_traj.push_back(seg);
232 
233  // Computes the starting conditions for the next segment
234 
235  prev_positions = positions;
236  prev_velocities = velocities;
237  prev_accelerations = accelerations;
238  }
239 
240  // ------ Commits the new trajectory
241 
242  if (!new_traj_ptr) {
243  throw std::runtime_error("The new trajectory was null!");
244  return;
245  }
246 
247  current_trajectory_box_.Set(new_traj_ptr);
248  LOG(debug) << "The new trajectory has " << new_traj.size() << " segments";
249  this->isControllerActive = true;
250 
251  }
252 
254  // Creates a dummy trajectory
256  SpecifiedTrajectory &traj = *traj_ptr;
257  traj[0].start_time = boost::posix_time::microsec_clock::local_time();
258  traj[0].duration = boost::posix_time::microseconds(0);
259  //traj[0].splines.coef[0] = 0.0;
260  current_trajectory_box_.Set(traj_ptr);
261  LOG(trace) << "Trajectory has been canceled";
262  }
263 
265  return this->isControllerActive;
266  }
267 
269 
270  time = boost::posix_time::microsec_clock::local_time();
271  boost::posix_time::time_duration dt = time - last_time;
272  last_time = time;
273 
275  current_trajectory_box_.Get(traj_ptr);
276  if (!traj_ptr || !this->isControllerActive) {
277  this->isControllerActive = false;
278  // LOG(error) << "The current trajectory can never be null";
279  return false;
280  }
281 
282  // Only because this is what the code originally looked like.
283  const SpecifiedTrajectory &traj = *traj_ptr;
284 
285 
286  // Determines which segment of the trajectory to use. (Not particularly realtime friendly).
287  int seg = -1;
288  while (seg + 1 < (int) traj.size() && traj[seg + 1].start_time < time) {
289  ++seg;
290  }
291 
292  if (seg == -1) {
293  if (traj.size() == 0)
294  LOG(error) << "No segments in the trajectory";
295  else
296  LOG(error) << "No earlier segments.";
297  return false;
298  }
299  if(seg == (int) traj.size()-1 && (traj[seg].start_time + traj[seg].duration) < time){
300  LOG(trace) << "trajectory finished.";
301  this->isControllerActive = false;
302  velocity.value = 0;
303  velocity.controllerMode = VELOCITY_CONTROL;
304  return true;
305  }
306 
307  // ------ Trajectory Sampling
308  duration = (double)traj[seg].duration.total_microseconds()/1000.0/1000.0; //convert to seconds
309  time_till_seg_start = (double)(time - traj[seg].start_time).total_microseconds()/1000.0/1000.0;
310 
312 
313 
314  if(inverseDirection){
315  actualpose = -actual.actualPosition;
316  actualvel = -actual.actualVelocity;
317  }else{
318  actualpose = actual.actualPosition;
319  actualvel = actual.actualVelocity;
320  }
321  // ------ Trajectory Following
323  velocity_error = ((actualvel/ 60.0) * gearRatio * 2.0 * M_PI) - targetVelocity ;
324 
326 
327  velocity.value = (int32) boost::math::round((velsetpoint / (gearRatio * 2.0 * M_PI)) * 60.0);
328 
329  velocity.controllerMode = VELOCITY_CONTROL;
330 
331  if(inverseDirection){
332  velocity.value = -velocity.value;
333  }
334 
335  return true;
336 
337  }
338 
340 
341  position.angle = targetPosition* radian;
342 
343  }
344 
346  velocity.angularVelocity = targetVelocity *radian_per_second;
347  }
348 
349  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) {
350 
351  coefficients.resize(6);
352 
353  if (time == 0.0) {
354  coefficients[0] = end_pos;
355  coefficients[1] = end_vel;
356  coefficients[2] = 0.5 * end_acc;
357  coefficients[3] = 0.0;
358  coefficients[4] = 0.0;
359  coefficients[5] = 0.0;
360  } else {
361  double T[6];
362  generatePowers(5, time, T);
363 
364  coefficients[0] = start_pos;
365  coefficients[1] = start_vel;
366  coefficients[2] = 0.5 * start_acc;
367  coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2] -
368  12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]);
369  coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2] +
370  16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]);
371  coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] -
372  6.0 * start_vel * T[1] - 6.0 * end_vel * T[1]) / (2.0 * T[5]);
373  }
374 
375  }
376 
377  void JointTrajectoryController::sampleQuinticSpline(const std::vector<double>& coefficients, const double time, double& position, double& velocity, double& acceleration) {
378 
379  // create powers of time:
380  double t[6];
381  generatePowers(5, time, t);
382 
383  position = t[0] * coefficients[0] +
384  t[1] * coefficients[1] +
385  t[2] * coefficients[2] +
386  t[3] * coefficients[3] +
387  t[4] * coefficients[4] +
388  t[5] * coefficients[5];
389 
390  velocity = t[0] * coefficients[1] +
391  2.0 * t[1] * coefficients[2] +
392  3.0 * t[2] * coefficients[3] +
393  4.0 * t[3] * coefficients[4] +
394  5.0 * t[4] * coefficients[5];
395 
396  acceleration = 2.0 * t[0] * coefficients[2] +
397  6.0 * t[1] * coefficients[3] +
398  12.0 * t[2] * coefficients[4] +
399  20.0 * t[3] * coefficients[5];
400 
401  }
402 
403  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) {
404 
405  coefficients.resize(4);
406 
407  if (time == 0.0) {
408  coefficients[0] = end_pos;
409  coefficients[1] = end_vel;
410  coefficients[2] = 0.0;
411  coefficients[3] = 0.0;
412  } else {
413  double T[4];
414  generatePowers(3, time, T);
415 
416  coefficients[0] = start_pos;
417  coefficients[1] = start_vel;
418  coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
419  coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
420  }
421 
422  }
423 
424  void JointTrajectoryController::generatePowers(const int n, const double x, double* powers) {
425 
426  powers[0] = 1.0;
427  for (int i = 1; i <= n; i++) {
428  powers[i] = powers[i - 1] * x;
429  }
430 
431  }
432 
433  void JointTrajectoryController::sampleSplineWithTimeBounds(const std::vector<double>& coefficients, const double duration, const double time, double& position, double& velocity, double& acceleration) {
434 
435  if (time < 0) {
436  double _;
437  sampleQuinticSpline(coefficients, 0.0, position, _, _);
438  velocity = 0;
439  acceleration = 0;
440  } else if (time > duration) {
441  double _;
442  sampleQuinticSpline(coefficients, duration, position, _, _);
443  velocity = 0;
444  acceleration = 0;
445  } else {
446  sampleQuinticSpline(coefficients, time,
447  position, velocity, acceleration);
448  }
449 
450  }
451 
452 
453 } // namespace youbot
DataObjectLockFree< boost::shared_ptr< const SpecifiedTrajectory > > current_trajectory_box_
std::vector< TrajectorySegment > segments
void getCubicSplineCoefficients(const double start_pos, const double start_vel, const double end_pos, const double end_vel, const double time, std::vector< double > &coefficients)
quantity< angular_velocity > angularVelocity
Definition: JointData.hpp:184
boost::posix_time::ptime start_time
boost::posix_time::ptime start_time
Set-point velocity of the joint.
Definition: JointData.hpp:182
Output part from the EtherCat message of the youBot EtherCat slaves.
void setGains(double P, double I, double D, double i_max, double i_min)
Set PID gains for the controller.
virtual DataType Get() const
bool updateTrajectoryController(const SlaveMessageInput &actual, SlaveMessageOutput &velocity)
void setConfigurationParameter(const double PParameter, const double IParameter, const double DParameter, const double IClippingMax, const double IClippingMin)
std::vector< double > coef
Joint Trajectory representation.
double updatePid(double p_error, boost::posix_time::time_duration dt)
Update the Pid loop with nonuniform time step size.
void getLastTargetVelocity(JointVelocitySetpoint &velocity)
#define LOG(level)
Definition: Logger.hpp:102
void sampleQuinticSpline(const std::vector< double > &coefficients, const double time, double &position, double &velocity, double &acceleration)
Joint parameter exception.
Definition: Exceptions.hpp:104
int32_t int32
Definition: osal.h:32
virtual void Set(const DataType &push)
Joint trajectory segment.
void getGains(double &p, double &i, double &d, double &i_max, double &i_min)
Get PID gains for the controller.
Input part from the EtherCat message of the youBot EtherCat slaves.
void getConfigurationParameter(double &PParameter, double &IParameter, double &DParameter, double &IClippingMax, double &IClippingMin)
void setTrajectory(const JointTrajectory &input_traj)
void generatePowers(const int n, const double x, double *powers)
void 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)
void initPid(double P, double I, double D, double I1, double I2)
Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2].
boost::posix_time::time_duration duration
void sampleSplineWithTimeBounds(const std::vector< double > &coefficients, const double duration, const double time, double &position, double &velocity, double &acceleration)
quantity< plane_angle > angle
Definition: JointData.hpp:172
Set-point angle / position of the joint.
Definition: JointData.hpp:170
void getLastTargetPosition(JointAngleSetpoint &position)


youbot_driver
Author(s): Jan Paulus
autogenerated on Mon Jun 10 2019 15:46:24