sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
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 dynamicgraph {
21 namespace sot {
22 
23 class Trajectory;
24 
26 protected:
28 
29 public:
30  unsigned int dbg_level;
31 
40  std::vector<std::string> joint_names;
41 
44  RulesJointTrajectory(Trajectory &TrajectoryToFill);
45 
47  void parse_string(std::string &atext);
48 
49 protected:
52  bool
53  search_exp_sub_string(std::string &text,
54  boost::match_results<std::string::const_iterator> &what,
55  boost::regex &e, std::string &sub_text);
61  void parse_header(std::string &text, std::string &sub_text1);
62 
65  void parse_joint_names(std::string &text, std::string &sub_text1,
66  std::vector<std::string> &joint_names);
67 
70  bool parse_seq(std::string &text, std::string &sub_text1,
71  std::vector<double> &seq);
72 
74  bool parse_point(std::string &trajectory, std::string &sub_text1);
75 
77  bool parse_points(std::string &trajectory, std::string &sub_text1);
78 };
79 
81 public:
82  unsigned long int secs_;
83  unsigned long int nsecs_;
84  timestamp() : secs_(0), nsecs_(0) {}
85  timestamp(const timestamp &ats) {
86  secs_ = ats.secs_;
87  nsecs_ = ats.nsecs_;
88  }
89  timestamp(unsigned long int lsecs, unsigned long int lnsecs) {
90  secs_ = lsecs;
91  nsecs_ = lnsecs;
92  }
93  bool operator==(const timestamp &other) const {
94  if ((secs_ != other.secs_) || (nsecs_ != other.nsecs_))
95  return false;
96  return true;
97  }
98  friend std::ostream &operator<<(std::ostream &stream, const timestamp &ats) {
99  stream << ats.secs_ + 0.000001 * (long double)ats.nsecs_;
100  return stream;
101  }
102 };
103 
105 public:
106  unsigned int seq_;
108  std::string frame_id_;
109  Header() : seq_(0), stamp_(0, 0), frame_id_("initial_trajectory") {}
110 };
111 
113 
114 public:
115  std::vector<double> positions_;
116  std::vector<double> velocities_;
117  std::vector<double> accelerations_;
118  std::vector<double> efforts_;
119 
120  typedef std::vector<double> vec_ref;
121 
122  void display(std::ostream &os) const {
123  boost::array<std::string, 4> names = boost::assign::list_of("Positions")(
124  "Velocities")("Accelerations")("Effort");
125 
126  const std::vector<double> *points = 0;
127 
128  for (std::size_t arrayId = 0; arrayId < names.size(); ++arrayId) {
129  switch (arrayId) {
130  case (0):
131  points = &positions_;
132  break;
133  case (1):
134  points = &velocities_;
135  break;
136  case (2):
137  points = &accelerations_;
138  break;
139  case (3):
140  points = &efforts_;
141  break;
142  default:
143  assert(0);
144  }
145 
146  std::vector<double>::const_iterator it_db;
147  os << names[arrayId] << std::endl << "---------" << std::endl;
148  for (it_db = points->begin(); it_db != points->end(); it_db++) {
149  os << *it_db << std::endl;
150  }
151  }
152  }
153 
154  void transfer(const std::vector<double> &src, unsigned int vecId) {
155  switch (vecId) {
156  case (0):
157  positions_ = src;
158  break;
159  case (1):
160  velocities_ = src;
161  break;
162  case (2):
163  accelerations_ = src;
164  break;
165  case (3):
166  efforts_ = src;
167  break;
168  default:
169  assert(0);
170  }
171  }
172 };
173 
175 
176 public:
177  Trajectory();
178  Trajectory(const Trajectory &copy);
179  virtual ~Trajectory();
180 
181  std::vector<std::string> joint_names_;
182 
185 
186  std::vector<JointTrajectoryPoint> points_;
187 
188  int deserialize(std::istringstream &is);
189  void display(std::ostream &) const;
190 };
191 } // namespace sot
192 } // namespace dynamicgraph
193 
194 #endif /* #ifndef SOT_TRAJECTORY_H__ */
std::string list_of_pv_str_re
Definition: trajectory.hh:33
Definition: trajectory.hh:104
std::vector< double > efforts_
Definition: trajectory.hh:118
boost::regex end_pt_re
Definition: trajectory.hh:38
unsigned int seq_
Definition: trajectory.hh:106
std::vector< std::string > joint_names
Definition: trajectory.hh:40
bool parse_points(std::string &trajectory, std::string &sub_text1)
Extract a sequence of points.
boost::regex list_of_jn_re
Definition: trajectory.hh:38
friend std::ostream & operator<<(std::ostream &stream, const timestamp &ats)
Definition: trajectory.hh:98
std::vector< double > positions_
Definition: trajectory.hh:115
std::string frame_id_str_re
Definition: trajectory.hh:33
Definition: trajectory.hh:25
std::vector< std::string > joint_names_
Definition: trajectory.hh:181
void parse_string(std::string &atext)
parse_string will fill TrajectoryToFill with string atext.
boost::regex comma_pt_re
Definition: trajectory.hh:38
#define SOT_CORE_EXPORT
Definition: api.hh:20
double time_from_start_
Definition: trajectory.hh:184
std::string seq_str_re
Definition: trajectory.hh:33
std::string float_str_re
Strings specifying the grammar of the structure.
Definition: trajectory.hh:33
boost::regex header_re
Boost regular expressions implementing the grammar.
Definition: trajectory.hh:38
std::string bg_pt_str_re
Definition: trajectory.hh:33
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...
Header header_
Definition: trajectory.hh:183
std::string frame_id_
Definition: trajectory.hh:108
timestamp()
Definition: trajectory.hh:84
std::string timestamp_str_re
Definition: trajectory.hh:33
timestamp(const timestamp &ats)
Definition: trajectory.hh:85
boost::regex list_of_pv_re
Definition: trajectory.hh:38
std::string end_pt_str_re
Definition: trajectory.hh:33
std::vector< double > velocities_
Definition: trajectory.hh:116
std::string header_str_re
Definition: trajectory.hh:33
std::vector< JointTrajectoryPoint > points_
Definition: trajectory.hh:186
std::string point_value_str_re
Definition: trajectory.hh:33
timestamp(unsigned long int lsecs, unsigned long int lnsecs)
Definition: trajectory.hh:89
void transfer(const std::vector< double > &src, unsigned int vecId)
Definition: trajectory.hh:154
bool parse_point(std::string &trajectory, std::string &sub_text1)
Extract a point description.
std::string joint_name_str_re
Definition: trajectory.hh:33
std::string bg_liste_of_pts_str_re
Definition: trajectory.hh:33
Definition: trajectory.hh:80
std::string comma_pt_str_re
Definition: trajectory.hh:33
unsigned int dbg_level
Definition: trajectory.hh:30
std::vector< double > vec_ref
Definition: trajectory.hh:120
Definition: trajectory.hh:174
boost::regex bg_liste_of_pts_re
Definition: trajectory.hh:38
unsigned long int secs_
Definition: trajectory.hh:82
std::string list_of_jn_str_re
Definition: trajectory.hh:33
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...
bool operator==(const timestamp &other) const
Definition: trajectory.hh:93
RulesJointTrajectory(Trajectory &TrajectoryToFill)
Constructor TrajectoryToFill is the structure where to store the parsed information.
Trajectory & TrajectoryToFill_
Definition: trajectory.hh:27
Definition: trajectory.hh:112
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.
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...
std::vector< double > accelerations_
Definition: trajectory.hh:117
Definition: abstract-sot-external-interface.hh:17
boost::regex bg_pt_re
Definition: trajectory.hh:38
timestamp stamp_
Definition: trajectory.hh:107
Header()
Definition: trajectory.hh:109
unsigned long int nsecs_
Definition: trajectory.hh:83
void display(std::ostream &os) const
Definition: trajectory.hh:122