trajectory.hh

Go to the documentation of this file.

/*
 * Copyright 2013,
 * Olivier Stasse,
 *
 * CNRS
 *
 */
 
#ifndef SOT_TRAJECTORY_H__
#define SOT_TRAJECTORY_H__
 
// Matrix
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-caster.h>
 
#include <boost/array.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/regex.hpp>
#include <sot/core/api.hh>
 
namespace dg = dynamicgraph;
namespace ba = boost::assign;
 
namespace dynamicgraph {
namespace sot {
 
class Trajectory;
 
class RulesJointTrajectory {
 protected:
  Trajectory &TrajectoryToFill_;
 
 public:
  unsigned int dbg_level;
 
  std::string float_str_re, seq_str_re, timestamp_str_re, frame_id_str_re,
      header_str_re, joint_name_str_re, list_of_jn_str_re, point_value_str_re,
      list_of_pv_str_re, bg_pt_str_re, end_pt_str_re, comma_pt_str_re,
      bg_liste_of_pts_str_re;
  boost::regex header_re, list_of_jn_re, list_of_pv_re, bg_pt_re, end_pt_re,
      comma_pt_re, bg_liste_of_pts_re;
  std::vector<std::string> joint_names;
 
  RulesJointTrajectory(Trajectory &TrajectoryToFill);
 
  void parse_string(std::string &atext);
 
 protected:
  bool search_exp_sub_string(
      std::string &text,
      boost::match_results<std::string::const_iterator> &what, boost::regex &e,
      std::string &sub_text);
  void parse_header(std::string &text, std::string &sub_text1);
 
  void parse_joint_names(std::string &text, std::string &sub_text1,
                         std::vector<std::string> &joint_names);
 
  bool parse_seq(std::string &text, std::string &sub_text1,
                 std::vector<double> &seq);
 
  bool parse_point(std::string &trajectory, std::string &sub_text1);
 
  bool parse_points(std::string &trajectory, std::string &sub_text1);
};
 
class SOT_CORE_EXPORT timestamp {
 public:
  unsigned long int secs_;
  unsigned long int nsecs_;
  timestamp() : secs_(0), nsecs_(0) {}
  timestamp(const timestamp &ats) {
    secs_ = ats.secs_;
    nsecs_ = ats.nsecs_;
  }
  timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
    secs_ = lsecs;
    nsecs_ = lnsecs;
  }
  bool operator==(const timestamp &other) const {
    if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_)) return false;
    return true;
  }
  friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
    stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
    return stream;
  }
};
 
class SOT_CORE_EXPORT Header {
 public:
  unsigned int seq_;
  timestamp stamp_;
  std::string frame_id_;
  Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
};
 
class SOT_CORE_EXPORT JointTrajectoryPoint {
 public:
  std::vector<double> positions_;
  std::vector<double> velocities_;
  std::vector<double> accelerations_;
  std::vector<double> efforts_;
 
  typedef std::vector<double> vec_ref;
 
  void display(std::ostream &os) const {
    boost::array<std::string, 4> names = boost::assign::list_of("Positions")(
        "Velocities")("Accelerations")("Effort");
 
    const std::vector<double> *points = 0;
 
    for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
      switch (arrayId) {
        case (0):
          points = &positions_;
          break;
        case (1):
          points = &velocities_;
          break;
        case (2):
          points = &accelerations_;
          break;
        case (3):
          points = &efforts_;
          break;
        default:
          assert(0);
      }
 
      std::vector<double>::const_iterator it_db;
      os << names[arrayId] << std::endl << "---------" << std::endl;
      for (it_db = points->begin(); it_db != points->end(); it_db++) {
        os << *it_db << std::endl;
      }
    }
  }
 
  void transfer(const std::vector<double> &src, unsigned int vecId) {
    switch (vecId) {
      case (0):
        positions_ = src;
        break;
      case (1):
        velocities_ = src;
        break;
      case (2):
        accelerations_ = src;
        break;
      case (3):
        efforts_ = src;
        break;
      default:
        assert(0);
    }
  }
};
 
class SOT_CORE_EXPORT Trajectory {
 public:
  Trajectory();
  Trajectory(const Trajectory &copy);
  virtual ~Trajectory();
 
  std::vector<std::string> joint_names_;
 
  Header header_;
  double time_from_start_;
 
  std::vector<JointTrajectoryPoint> points_;
 
  int deserialize(std::istringstream &is);
  void display(std::ostream &) const;
};
}  // namespace sot
 
template <>
struct signal_io<sot::Trajectory> : signal_io_unimplemented<sot::Trajectory> {};
 
}  // namespace dynamicgraph
 
#endif /* #ifndef SOT_TRAJECTORY_H__ */