joint_trajectory_segment.h

Go to the documentation of this file.

// Copyright (C) 2013, PAL Robotics S.L.
//
// 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 PAL Robotics S.L. nor the names of its
//     contributors may be used to endorse or promote products derived from
//     this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.


#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H
#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H

// C++ standard
#include <cmath>
#include <stdexcept>
#include <string>
#include <vector>

// angles
#include <angles/angles.h>

// ROS messages
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

// ros_controls
#include <realtime_tools/realtime_server_goal_handle.h>

// Project
#include <joint_trajectory_controller/joint_trajectory_msg_utils.h>
#include <joint_trajectory_controller/tolerances.h>

namespace joint_trajectory_controller
{

template <class Segment>
class JointTrajectorySegment : public Segment
{
public:
  typedef typename Segment::Scalar Scalar;
  typedef typename Segment::Time   Time;

  typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
  typedef boost::shared_ptr<RealtimeGoalHandle>                                               RealtimeGoalHandlePtr;

  struct State : public Segment::State
  {
    typedef typename Segment::State::Scalar Scalar;
    State() : Segment::State() {}
    State(unsigned int size) : Segment::State(size) {}

    State(const trajectory_msgs::JointTrajectoryPoint& point,
          const std::vector<unsigned int>&             permutation     = std::vector<unsigned int>(),
          const std::vector<Scalar>&                   position_offset = std::vector<Scalar>())
    {
      init(point, permutation, position_offset);
    }

    void init(const trajectory_msgs::JointTrajectoryPoint& point,
              const std::vector<unsigned int>&             permutation     = std::vector<unsigned int>(),
              const std::vector<Scalar>&                   position_offset = std::vector<Scalar>())
    {
      using std::invalid_argument;

      const unsigned int joint_dim = point.positions.size();

      // Preconditions
      if (!isValid(point, joint_dim))
      {
        throw(invalid_argument("Size mismatch in trajectory point position, velocity or acceleration data."));
      }
      if (!permutation.empty() && joint_dim != permutation.size())
      {
        throw(invalid_argument("Size mismatch between trajectory point and permutation vector."));
      }
      for (unsigned int i = 0; i < permutation.size(); ++i)
      {
        if (permutation[i] >= joint_dim)
        {
          throw(invalid_argument("Permutation vector contains out-of-range indices."));
        }
      }
      if (!position_offset.empty() && joint_dim != position_offset.size())
      {
        throw(invalid_argument("Size mismatch between trajectory point "
                               "and vector specifying whether joints wrap around."));
      }

      // Initialize state
      if (!point.positions.empty())     {this->position.resize(joint_dim);}
      if (!point.velocities.empty())    {this->velocity.resize(joint_dim);}
      if (!point.accelerations.empty()) {this->acceleration.resize(joint_dim);}

      for (unsigned int i = 0; i < joint_dim; ++i)
      {
        // Apply permutation only if it was specified, otherwise preserve original message order
        const unsigned int id = permutation.empty() ? i : permutation[i];

        // Apply position offset only if it was specified
        const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i];

        if (!point.positions.empty())     {this->position[i]     = point.positions[id] + offset;}
        if (!point.velocities.empty())    {this->velocity[i]     = point.velocities[id];}
        if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[id];}
      }
    }
  };

  JointTrajectorySegment(const Time&  start_time,
                         const State& start_state,
                         const Time&  end_time,
                         const State& end_state)
    : rt_goal_handle_(),
      tolerances_(start_state.position.size())
  {
    Segment::init(start_time, start_state, end_time, end_state);
  }

  JointTrajectorySegment(const ros::Time&                             traj_start_time,
                         const trajectory_msgs::JointTrajectoryPoint& start_point,
                         const trajectory_msgs::JointTrajectoryPoint& end_point,
                         const std::vector<unsigned int>&             permutation     = std::vector<unsigned int>(),
                         const std::vector<Scalar>&                   position_offset = std::vector<Scalar>())
    : rt_goal_handle_(),
      tolerances_(start_point.positions.size())
  {
    if (start_point.positions.size() != end_point.positions.size())
    {
      throw(std::invalid_argument("Can't construct segment from ROS message: "
                                  "Start/end points data size mismatch."));
    }

    const Time start_time = (traj_start_time + start_point.time_from_start).toSec();
    const Time end_time   = (traj_start_time + end_point.time_from_start).toSec();

    try
    {
      const State start_state(start_point, permutation, position_offset);
      const State end_state(end_point,     permutation, position_offset);

      this->init(start_time, start_state,
                 end_time,   end_state);
    }
    catch(const std::invalid_argument& ex)
    {
      std::string msg = "Can't construct segment from ROS message: " + std::string(ex.what());
      throw(std::invalid_argument(msg));
    }
  }

  RealtimeGoalHandlePtr getGoalHandle() const {return rt_goal_handle_;}

  void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle) {rt_goal_handle_ = rt_goal_handle;}

  const SegmentTolerances<Scalar>& getTolerances() const {return tolerances_;}

  void setTolerances(const SegmentTolerances<Scalar>& tolerances) {tolerances_ = tolerances;}

private:
  RealtimeGoalHandlePtr     rt_goal_handle_;
  SegmentTolerances<Scalar> tolerances_;
};

template <class Scalar>
std::vector<Scalar> wraparoundOffset(const std::vector<Scalar>& prev_position,
                                     const std::vector<Scalar>& next_position,
                                     const std::vector<bool>&   angle_wraparound)
{
  // Preconditions
  const unsigned int n_joints = angle_wraparound.size();
  if (n_joints != prev_position.size() || n_joints != next_position.size()) {return std::vector<Scalar>();}

  // Return value
  std::vector<Scalar> pos_offset(n_joints, 0.0);

  for (unsigned int i = 0; i < angle_wraparound.size(); ++i)
  {
    if (angle_wraparound[i])
    {
      Scalar dist = angles::shortest_angular_distance(prev_position[i], next_position[i]);

      // Deal with singularity at M_PI shortest distance
      if (std::abs(dist) - M_PI < 1e-9)
      {
        dist = next_position[i] > prev_position[i] ? std::abs(dist) : -std::abs(dist);
      }
      pos_offset[i] = (prev_position[i] + dist) - next_position[i];
    }
  }
  return pos_offset;
}

} // namespace

#endif // header guard