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