JointTrajectoryController.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public 
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/
#include <youbot_driver/youbot/JointTrajectoryController.hpp>
namespace youbot
{

JointTrajectoryController::JointTrajectoryController()
{

  this->pid.initPid(80.0, 1, 0, 1000, -1000);
  time = boost::posix_time::microsec_clock::local_time();
  last_time = boost::posix_time::microsec_clock::local_time();

  this->isControllerActive = false;
  this->targetPosition = 0;
  this->targetVelocity = 0;
  this->targetAcceleration = 0;
  this->encoderTicksPerRound = 1;
  this->gearRatio = 1;
  this->inverseDirection = false;
  actualpose = 0;
  actualvel = 0;

  // Creates a dummy trajectory
  boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &traj = *traj_ptr;
  traj[0].start_time = boost::posix_time::microsec_clock::local_time();
  traj[0].duration = boost::posix_time::microseconds(0);
  //traj[0].splines.coef[0] = 0.0;
  current_trajectory_box_.Set(traj_ptr);

}

JointTrajectoryController::~JointTrajectoryController()
{

}

void JointTrajectoryController::getConfigurationParameter(double& PParameter, double& IParameter, double& DParameter,
                                                          double& IClippingMax, double& IClippingMin)
{

  if (this->isControllerActive)
    throw JointParameterException("The trajectory controller is running");
  this->pid.getGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);

}

void JointTrajectoryController::setConfigurationParameter(const double PParameter, const double IParameter,
                                                          const double DParameter, const double IClippingMax,
                                                          const double IClippingMin)
{

  if (this->isControllerActive)
    throw JointParameterException("The trajectory controller is running");
  this->pid.setGains(PParameter, IParameter, DParameter, IClippingMax, IClippingMin);

}

void JointTrajectoryController::setTrajectory(const JointTrajectory& input_traj)
{

  if (input_traj.segments.size() == 0)
    throw std::runtime_error("Invalid trajectory");

  boost::posix_time::ptime time_now = boost::posix_time::microsec_clock::local_time();

  boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
  SpecifiedTrajectory &new_traj = *new_traj_ptr;

  // ------ Grabs the trajectory that we're currently following.

  boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
  current_trajectory_box_.Get(prev_traj_ptr);
  if (!prev_traj_ptr)
  {
    throw std::runtime_error("The current trajectory can never be null");
    return;
  }
  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;

  // ------ Copies over the segments from the previous trajectory that are still useful.

  // Useful segments are still relevant after the current time.
  int first_useful = -1;
  while (first_useful + 1 < (int)prev_traj.size() && prev_traj[first_useful + 1].start_time <= time_now)
  {
    ++first_useful;
  }

  // Useful segments are not going to be completely overwritten by the message's splines.
  int last_useful = -1;
  while (last_useful + 1 < (int)prev_traj.size() && prev_traj[last_useful + 1].start_time < time_now)
  {
    ++last_useful;
  }

  if (last_useful < first_useful)
    first_useful = last_useful;

  // Copies over the old segments that were determined to be useful.
  for (int i = std::max(first_useful, 0); i <= last_useful; ++i)
  {
    new_traj.push_back(prev_traj[i]);
  }

  // We always save the last segment so that we know where to stop if
  // there are no new segments.
  if (new_traj.size() == 0)
    new_traj.push_back(prev_traj[prev_traj.size() - 1]);

  // ------ Determines when and where the new segments start

  // Finds the end conditions of the final segment
  Segment &last = new_traj[new_traj.size() - 1];
  double prev_positions;
  double prev_velocities;
  double prev_accelerations;

  LOG(debug) << "Initial conditions for new set of splines:";

  sampleSplineWithTimeBounds(last.spline.coef, (double)last.duration.total_microseconds() / 1000.0 / 1000.0,
                             (double)last.start_time.time_of_day().total_microseconds() / 1000.0 / 1000.0,
                             prev_positions, prev_velocities, prev_accelerations);
  //  ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
  //            prev_accelerations[i], joints_[i]->joint_->name.c_str());

  // ------ Tacks on the new segments

  double positions;
  double velocities;
  double accelerations;

  std::vector<boost::posix_time::time_duration> durations;
  durations.push_back(input_traj.segments[0].time_from_start);

  for (size_t i = 1; i < input_traj.segments.size(); ++i)
    durations.push_back(input_traj.segments[i].time_from_start - input_traj.segments[i - 1].time_from_start);

  for (unsigned int i = 0; i < input_traj.segments.size(); ++i)
  {
    Segment seg;

    seg.start_time = input_traj.start_time + input_traj.segments[i].time_from_start;
    seg.duration = durations[i];

    positions = input_traj.segments[i].positions.value();
    velocities = input_traj.segments[i].velocities.value();
    accelerations = input_traj.segments[i].accelerations.value();

    // Converts the boundary conditions to splines.

    //   if (prev_accelerations.size() > 0 && accelerations.size() > 0)
    //   {
    getQuinticSplineCoefficients(prev_positions, prev_velocities, prev_accelerations, positions, velocities,
                                 accelerations, (double)durations[i].total_microseconds() / 1000.0 / 1000.0,
                                 seg.spline.coef);
    /*
     }
     else if (prev_velocities.size() > 0 && velocities.size() > 0)
     {
     getCubicSplineCoefficients(
     prev_positions[j], prev_velocities[j],
     positions[j], velocities[j],
     durations[i],
     seg.splines[j].coef);
     seg.splines[j].coef.resize(6, 0.0);
     }
     else
     {
     seg.splines[j].coef[0] = prev_positions[j];
     if (durations[i] == 0.0)
     seg.splines[j].coef[1] = 0.0;
     else
     seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
     seg.splines[j].coef[2] = 0.0;
     seg.splines[j].coef[3] = 0.0;
     seg.splines[j].coef[4] = 0.0;
     seg.splines[j].coef[5] = 0.0;
     }
     */

    // Pushes the splines onto the end of the new trajectory.
    new_traj.push_back(seg);

    // Computes the starting conditions for the next segment

    prev_positions = positions;
    prev_velocities = velocities;
    prev_accelerations = accelerations;
  }

  // ------ Commits the new trajectory

  if (!new_traj_ptr)
  {
    throw std::runtime_error("The new trajectory was null!");
    return;
  }

  current_trajectory_box_.Set(new_traj_ptr);
  LOG(debug) << "The new trajectory has " << new_traj.size() << " segments";
  this->isControllerActive = true;

}

void JointTrajectoryController::cancelCurrentTrajectory()
{
  // Creates a dummy trajectory
  boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &traj = *traj_ptr;
  traj[0].start_time = boost::posix_time::microsec_clock::local_time();
  traj[0].duration = boost::posix_time::microseconds(0);
  //traj[0].splines.coef[0] = 0.0;
  current_trajectory_box_.Set(traj_ptr);
  LOG(trace) << "Trajectory has been canceled";
}

bool JointTrajectoryController::isTrajectoryControllerActive()
{
  return this->isControllerActive;
}

bool JointTrajectoryController::updateTrajectoryController(const SlaveMessageInput& actual,
                                                           SlaveMessageOutput& velocity)
{

  time = boost::posix_time::microsec_clock::local_time();
  boost::posix_time::time_duration dt = time - last_time;
  last_time = time;

  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.Get(traj_ptr);
  if (!traj_ptr || !this->isControllerActive)
  {
    this->isControllerActive = false;
    //   LOG(error) << "The current trajectory can never be null";
    return false;
  }

  // Only because this is what the code originally looked like.
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
  int seg = -1;
  while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time < time)
  {
    ++seg;
  }

  if (seg == -1)
  {
    if (traj.size() == 0)
      LOG(error) << "No segments in the trajectory";
    else
      LOG(error) << "No earlier segments.";
    return false;
  }
  if (seg == (int)traj.size() - 1 && (traj[seg].start_time + traj[seg].duration) < time)
  {
    LOG(trace) << "trajectory finished.";
    this->isControllerActive = false;
    velocity.value = 0;
    velocity.controllerMode = VELOCITY_CONTROL;
    return true;
  }

  // ------ Trajectory Sampling
  duration = (double)traj[seg].duration.total_microseconds() / 1000.0 / 1000.0; //convert to seconds
  time_till_seg_start = (double)(time - traj[seg].start_time).total_microseconds() / 1000.0 / 1000.0;

  sampleSplineWithTimeBounds(traj[seg].spline.coef, duration, time_till_seg_start, targetPosition, targetVelocity,
                             targetAcceleration);

  if (inverseDirection)
  {
    actualpose = -actual.actualPosition;
    actualvel = -actual.actualVelocity;
  }
  else
  {
    actualpose = actual.actualPosition;
    actualvel = actual.actualVelocity;
  }
  // ------ Trajectory Following
  pose_error = ((actualpose / encoderTicksPerRound) * gearRatio * (2.0 * M_PI)) - targetPosition;
  velocity_error = ((actualvel / 60.0) * gearRatio * 2.0 * M_PI) - targetVelocity;

  velsetpoint = pid.updatePid(pose_error, velocity_error, dt);

  velocity.value = (int32)round((velsetpoint / (gearRatio * 2.0 * M_PI)) * 60.0);

  velocity.controllerMode = VELOCITY_CONTROL;

  if (inverseDirection)
  {
    velocity.value = -velocity.value;
  }

  return true;

}

void JointTrajectoryController::getLastTargetPosition(JointAngleSetpoint& position)
{

  position.angle = targetPosition * radian;

}

void JointTrajectoryController::getLastTargetVelocity(JointVelocitySetpoint& velocity)
{
  velocity.angularVelocity = targetVelocity * radian_per_second;
}

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)
{

  coefficients.resize(6);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.5 * end_acc;
    coefficients[3] = 0.0;
    coefficients[4] = 0.0;
    coefficients[5] = 0.0;
  }
  else
  {
    double T[6];
    generatePowers(5, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = 0.5 * start_acc;
    coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2]
        - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]);
    coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2]
        + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]);
    coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - 6.0 * start_vel * T[1]
        - 6.0 * end_vel * T[1]) / (2.0 * T[5]);
  }

}

void JointTrajectoryController::sampleQuinticSpline(const std::vector<double>& coefficients, const double time,
                                                    double& position, double& velocity, double& acceleration)
{

  // create powers of time:
  double t[6];
  generatePowers(5, time, t);

  position = t[0] * coefficients[0] + t[1] * coefficients[1] + t[2] * coefficients[2] + t[3] * coefficients[3]
      + t[4] * coefficients[4] + t[5] * coefficients[5];

  velocity = t[0] * coefficients[1] + 2.0 * t[1] * coefficients[2] + 3.0 * t[2] * coefficients[3]
      + 4.0 * t[3] * coefficients[4] + 5.0 * t[4] * coefficients[5];

  acceleration = 2.0 * t[0] * coefficients[2] + 6.0 * t[1] * coefficients[3] + 12.0 * t[2] * coefficients[4]
      + 20.0 * t[3] * coefficients[5];

}

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)
{

  coefficients.resize(4);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.0;
    coefficients[3] = 0.0;
  }
  else
  {
    double T[4];
    generatePowers(3, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
    coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
  }

}

void JointTrajectoryController::generatePowers(const int n, const double x, double* powers)
{

  powers[0] = 1.0;
  for (int i = 1; i <= n; i++)
  {
    powers[i] = powers[i - 1] * x;
  }

}

void JointTrajectoryController::sampleSplineWithTimeBounds(const std::vector<double>& coefficients,
                                                           const double duration, const double time, double& position,
                                                           double& velocity, double& acceleration)
{

  if (time < 0)
  {
    double _;
    sampleQuinticSpline(coefficients, 0.0, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else if (time > duration)
  {
    double _;
    sampleQuinticSpline(coefficients, duration, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else
  {
    sampleQuinticSpline(coefficients, time, position, velocity, acceleration);
  }

}

} // namespace youbot