trajectory.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2013,
3  * Olivier Stasse,
4  *
5  * CNRS
6  *
7  */
8 
9 #ifndef SOT_TRAJECTORY_H__
10 #define SOT_TRAJECTORY_H__
11 
12 // Matrix
13 #include <dynamic-graph/linear-algebra.h>
14 #include <sot/core/api.hh>
15 
16 #include <boost/array.hpp>
17 #include <boost/assign/list_of.hpp>
18 #include <boost/regex.hpp>
19 
20 namespace dg = dynamicgraph;
21 namespace ba = boost::assign;
22 
23 namespace dynamicgraph {
24 namespace sot {
25 
26 class Trajectory;
27 
29 protected:
31 
32 public:
33  unsigned int dbg_level;
34 
43  std::vector<std::string> joint_names;
44 
47  RulesJointTrajectory(Trajectory &TrajectoryToFill);
48 
50  void parse_string(std::string &atext);
51 
52 protected:
55  bool
56  search_exp_sub_string(std::string &text,
57  boost::match_results<std::string::const_iterator> &what,
58  boost::regex &e, std::string &sub_text);
64  void parse_header(std::string &text, std::string &sub_text1);
65 
68  void parse_joint_names(std::string &text, std::string &sub_text1,
69  std::vector<std::string> &joint_names);
70 
73  bool parse_seq(std::string &text, std::string &sub_text1,
74  std::vector<double> &seq);
75 
77  bool parse_point(std::string &trajectory, std::string &sub_text1);
78 
80  bool parse_points(std::string &trajectory, std::string &sub_text1);
81 };
82 
84 public:
85  unsigned long int secs_;
86  unsigned long int nsecs_;
87  timestamp() : secs_(0), nsecs_(0) {}
88  timestamp(const timestamp &ats) {
89  secs_ = ats.secs_;
90  nsecs_ = ats.nsecs_;
91  }
92  timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
93  secs_ = lsecs;
94  nsecs_ = lnsecs;
95  }
96  bool operator==(const timestamp &other) const {
97  if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_))
98  return false;
99  return true;
100  }
101  friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
102  stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
103  return stream;
104  }
105 };
106 
108 public:
109  unsigned int seq_;
111  std::string frame_id_;
112  Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
113 };
114 
116 
117 public:
118  std::vector<double> positions_;
119  std::vector<double> velocities_;
120  std::vector<double> accelerations_;
121  std::vector<double> efforts_;
122 
123  typedef std::vector<double> vec_ref;
124 
125  void display(std::ostream &os) const {
126  boost::array<std::string, 4> names =
127  ba::list_of("Positions")("Velocities")("Accelerations")("Effort");
128 
129  const std::vector<double> *points = 0;
130 
131  for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
132  switch (arrayId) {
133  case (0):
134  points = &positions_;
135  break;
136  case (1):
137  points = &velocities_;
138  break;
139  case (2):
140  points = &accelerations_;
141  break;
142  case (3):
143  points = &efforts_;
144  break;
145  default:
146  assert(0);
147  }
148 
149  std::vector<double>::const_iterator it_db;
150  os << names[arrayId] << std::endl << "---------" << std::endl;
151  for (it_db = points->begin(); it_db != points->end(); it_db++) {
152  os << *it_db << std::endl;
153  }
154  }
155  }
156 
157  void transfer(const std::vector<double> &src, unsigned int vecId) {
158  switch (vecId) {
159  case (0):
160  positions_ = src;
161  break;
162  case (1):
163  velocities_ = src;
164  break;
165  case (2):
166  accelerations_ = src;
167  break;
168  case (3):
169  efforts_ = src;
170  break;
171  default:
172  assert(0);
173  }
174  }
175 };
176 
178 
179 public:
180  Trajectory();
181  Trajectory(const Trajectory &copy);
182  virtual ~Trajectory();
183 
184  std::vector<std::string> joint_names_;
185 
188 
189  std::vector<JointTrajectoryPoint> points_;
190 
191  int deserialize(std::istringstream &is);
192  void display(std::ostream &) const;
193 };
194 } // namespace sot
195 } // namespace dynamicgraph
196 
197 #endif /* #ifndef SOT_TRAJECTORY_H__ */
dynamicgraph::sot::RulesJointTrajectory::seq_str_re
std::string seq_str_re
Definition: trajectory.hh:36
dynamicgraph::sot::Header::Header
Header()
Definition: trajectory.hh:112
dynamicgraph::sot::RulesJointTrajectory::parse_header
void parse_header(std::string &text, std::string &sub_text1)
Find and store the header. This method is looking for: unsigned int seq. unsigned int sec,...
dynamicgraph::sot::RulesJointTrajectory
Definition: trajectory.hh:28
dynamicgraph::sot::JointTrajectoryPoint::accelerations_
std::vector< double > accelerations_
Definition: trajectory.hh:120
dynamicgraph::sot::timestamp::timestamp
timestamp()
Definition: trajectory.hh:87
dynamicgraph::sot::RulesJointTrajectory::bg_liste_of_pts_re
boost::regex bg_liste_of_pts_re
Definition: trajectory.hh:42
SOT_CORE_EXPORT
#define SOT_CORE_EXPORT
Definition: api.hh:20
dynamicgraph::sot::Header::seq_
unsigned int seq_
Definition: trajectory.hh:109
dynamicgraph::sot::RulesJointTrajectory::point_value_str_re
std::string point_value_str_re
Definition: trajectory.hh:37
dynamicgraph
Definition: abstract-sot-external-interface.hh:17
dynamicgraph::sot::RulesJointTrajectory::end_pt_str_re
std::string end_pt_str_re
Definition: trajectory.hh:38
dynamicgraph::sot::RulesJointTrajectory::joint_name_str_re
std::string joint_name_str_re
Definition: trajectory.hh:37
dynamicgraph::sot::RulesJointTrajectory::search_exp_sub_string
bool search_exp_sub_string(std::string &text, boost::match_results< std::string::const_iterator > &what, boost::regex &e, std::string &sub_text)
General parsing method of text with regexp e. The results are given in what. The remaining text is le...
dynamicgraph::sot::timestamp::operator==
bool operator==(const timestamp &other) const
Definition: trajectory.hh:96
dynamicgraph::sot::timestamp
Definition: trajectory.hh:83
dynamicgraph::sot::JointTrajectoryPoint::display
void display(std::ostream &os) const
Definition: trajectory.hh:125
dynamicgraph::sot::JointTrajectoryPoint::transfer
void transfer(const std::vector< double > &src, unsigned int vecId)
Definition: trajectory.hh:157
dynamicgraph::sot::RulesJointTrajectory::RulesJointTrajectory
RulesJointTrajectory(Trajectory &TrajectoryToFill)
Constructor TrajectoryToFill is the structure where to store the parsed information.
dynamicgraph::sot::RulesJointTrajectory::list_of_pv_re
boost::regex list_of_pv_re
Definition: trajectory.hh:41
dynamicgraph::sot::JointTrajectoryPoint::positions_
std::vector< double > positions_
Definition: trajectory.hh:118
dynamicgraph::sot::timestamp::secs_
unsigned long int secs_
Definition: trajectory.hh:85
dynamicgraph::sot::timestamp::nsecs_
unsigned long int nsecs_
Definition: trajectory.hh:86
dynamicgraph::sot::RulesJointTrajectory::header_re
boost::regex header_re
Boost regular expressions implementing the grammar.
Definition: trajectory.hh:41
dynamicgraph::sot::Trajectory::time_from_start_
double time_from_start_
Definition: trajectory.hh:187
dynamicgraph::sot::Header::frame_id_
std::string frame_id_
Definition: trajectory.hh:111
dynamicgraph::sot::RulesJointTrajectory::parse_joint_names
void parse_joint_names(std::string &text, std::string &sub_text1, std::vector< std::string > &joint_names)
Understand joint_names. Extract a list of strings.
dynamicgraph::sot::RulesJointTrajectory::comma_pt_str_re
std::string comma_pt_str_re
Definition: trajectory.hh:38
api.hh
dynamicgraph::sot::RulesJointTrajectory::parse_point
bool parse_point(std::string &trajectory, std::string &sub_text1)
Extract a point description.
dynamicgraph::sot::RulesJointTrajectory::TrajectoryToFill_
Trajectory & TrajectoryToFill_
Definition: trajectory.hh:30
dynamicgraph::sot::Header
Definition: trajectory.hh:107
dynamicgraph::sot::RulesJointTrajectory::parse_seq
bool parse_seq(std::string &text, std::string &sub_text1, std::vector< double > &seq)
Extract a sequence of doubles. To be used for position, velocities, accelerations and effort.
dynamicgraph::sot::JointTrajectoryPoint::efforts_
std::vector< double > efforts_
Definition: trajectory.hh:121
dynamicgraph::sot::Trajectory::joint_names_
std::vector< std::string > joint_names_
Definition: trajectory.hh:184
dynamicgraph::sot::JointTrajectoryPoint::velocities_
std::vector< double > velocities_
Definition: trajectory.hh:119
dynamicgraph::sot::RulesJointTrajectory::parse_points
bool parse_points(std::string &trajectory, std::string &sub_text1)
Extract a sequence of points.
dynamicgraph::sot::RulesJointTrajectory::bg_liste_of_pts_str_re
std::string bg_liste_of_pts_str_re
Definition: trajectory.hh:39
dynamicgraph::sot::Trajectory
Definition: trajectory.hh:177
dynamicgraph::sot::JointTrajectoryPoint::vec_ref
std::vector< double > vec_ref
Definition: trajectory.hh:123
dynamicgraph::sot::RulesJointTrajectory::joint_names
std::vector< std::string > joint_names
Definition: trajectory.hh:43
dynamicgraph::sot::timestamp::operator<<
friend std::ostream & operator<<(std::ostream &stream, const timestamp &ats)
Definition: trajectory.hh:101
dynamicgraph::sot::RulesJointTrajectory::list_of_jn_str_re
std::string list_of_jn_str_re
Definition: trajectory.hh:37
dynamicgraph::sot::JointTrajectoryPoint
Definition: trajectory.hh:115
dynamicgraph::sot::RulesJointTrajectory::dbg_level
unsigned int dbg_level
Definition: trajectory.hh:33
dynamicgraph::sot::Trajectory::header_
Header header_
Definition: trajectory.hh:186
dynamicgraph::sot::RulesJointTrajectory::end_pt_re
boost::regex end_pt_re
Definition: trajectory.hh:41
dynamicgraph::sot::RulesJointTrajectory::parse_string
void parse_string(std::string &atext)
parse_string will fill TrajectoryToFill with string atext.
dynamicgraph::sot::RulesJointTrajectory::comma_pt_re
boost::regex comma_pt_re
Definition: trajectory.hh:42
dynamicgraph::sot::timestamp::timestamp
timestamp(const timestamp &ats)
Definition: trajectory.hh:88
dynamicgraph::sot::Trajectory::points_
std::vector< JointTrajectoryPoint > points_
Definition: trajectory.hh:189
dynamicgraph::sot::Header::stamp_
timestamp stamp_
Definition: trajectory.hh:110
dynamicgraph::sot::RulesJointTrajectory::list_of_pv_str_re
std::string list_of_pv_str_re
Definition: trajectory.hh:38
dynamicgraph::sot::RulesJointTrajectory::float_str_re
std::string float_str_re
Strings specifying the grammar of the structure.
Definition: trajectory.hh:36
dynamicgraph::sot::RulesJointTrajectory::bg_pt_str_re
std::string bg_pt_str_re
Definition: trajectory.hh:38
dynamicgraph::sot::RulesJointTrajectory::bg_pt_re
boost::regex bg_pt_re
Definition: trajectory.hh:41
dynamicgraph::sot::RulesJointTrajectory::timestamp_str_re
std::string timestamp_str_re
Definition: trajectory.hh:36
dynamicgraph::sot::RulesJointTrajectory::frame_id_str_re
std::string frame_id_str_re
Definition: trajectory.hh:36
dynamicgraph::sot::RulesJointTrajectory::list_of_jn_re
boost::regex list_of_jn_re
Definition: trajectory.hh:41
dynamicgraph::sot::timestamp::timestamp
timestamp(unsigned long int lsecs, unsigned long int lnsecs)
Definition: trajectory.hh:92
dynamicgraph::sot::RulesJointTrajectory::header_str_re
std::string header_str_re
Definition: trajectory.hh:37