signal-cast.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #include <dynamic-graph/eigen-io.h>
13 
14 #include <Eigen/Core>
15 #include <iomanip>
17 #include <sot/core/flags.hh>
19 #include <sot/core/multi-bound.hh>
20 #include <sot/core/pool.hh>
21 #include <sot/core/trajectory.hh>
22 
23 #ifdef WIN32
24 #include <Windows.h>
25 #endif
26 
27 /* Implements a set of caster/displayer for the main types of sot-core. */
28 
29 namespace dynamicgraph {
30 using namespace std;
31 using namespace dynamicgraph::sot;
32 namespace dgsot = dynamicgraph::sot;
33 
34 /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
35 /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
36 /* --- CASTER IMPLEMENTATION ------------------------------------------------ */
37 
38 // DG_SIGNAL_CAST_DEFINITION(sot::Flags);
39 // DG_ADD_CASTER(sot::Flags, flags);
40 
41 /* --- TIMEVAL -------------------------------------------------------------- */
42 /* --- TIMEVAL -------------------------------------------------------------- */
43 /* --- TIMEVAL -------------------------------------------------------------- */
44 /*
45 DG_SIGNAL_CAST_DEFINITION_HPP(struct timeval);
46 
47 struct timeval SignalCast<struct timeval>::cast(std::istringstream &iss) {
48  int u, s;
49  iss >> s >> u;
50  struct timeval t;
51  t.tv_sec = s;
52  t.tv_usec = u;
53  return t;
54 } void SignalCast<struct timeval>::disp(const struct timeval &t,
55  std::ostream &os) {
56  os << t.tv_sec << "s " << t.tv_usec << "ms";
57 }
58 
59 DG_ADD_CASTER(struct timeval, tv);
60 */
61 
62 /* --- Trajectory --------------------------------------------------------------
63  */
64 /* --- Trajectory --------------------------------------------------------------
65  */
66 /* --- Trajectory --------------------------------------------------------------
67  */
68 /*
69 DG_SIGNAL_CAST_DEFINITION_HPP(dgsot::Trajectory);
70 
71 dgsot::Trajectory SignalCast<dgsot::Trajectory>::cast(std::istringstream &iss) {
72  dgsot::Trajectory aTraj;
73 
74  // Read joint names.
75  std::vector<std::string>::size_type nb_joints;
76  iss >> nb_joints;
77  aTraj.joint_names_.resize(nb_joints);
78  for (std::vector<std::string>::size_type idJoints = 0; idJoints < nb_joints;
79  idJoints++)
80  iss >> aTraj.joint_names_[idJoints];
81 
82  // Read nb of points
83  std::vector<JointTrajectoryPoint>::size_type nb_points;
84  iss >> nb_points;
85 
86  // Read points
87  for (std::vector<JointTrajectoryPoint>::size_type idPoint = 0;
88  idPoint < nb_points; idPoint++) {
89  // Read positions.
90  for (std::vector<double>::size_type idPos = 0; idPos < nb_joints; idPos++)
91  iss >> aTraj.points_[idPoint].positions_[idPos];
92  // TODO: read velocities and accelerations.
93  }
94  return aTraj;
95 }
96 void SignalCast<dgsot::Trajectory>::disp(const dgsot::Trajectory &aTraj,
97  std::ostream &os) {
98  // Display joint names.
99  os << "{ Number of joints: " << aTraj.joint_names_.size() << std::endl;
100  for (std::vector<std::string>::size_type idJoints = 0;
101  idJoints < aTraj.joint_names_.size(); idJoints++) {
102  os << idJoints << " - " << aTraj.joint_names_[idJoints] << std::endl;
103  }
104  // Display points
105  os << "Number of points: " << aTraj.points_.size() << std::endl;
106  for (std::vector<JointTrajectoryPoint>::size_type idPoint = 0;
107  idPoint < aTraj.points_.size(); idPoint++) {
108  if (aTraj.points_[idPoint].positions_.size() != 0) {
109  os << " Point " << idPoint << " - Pos: [";
110  // Read positions.
111  for (std::vector<double>::size_type idPos = 0;
112  idPos < aTraj.points_[idPoint].positions_.size(); idPos++) {
113  os << "(" << idPos << " : " << aTraj.points_[idPoint].positions_[idPos]
114  << ") ";
115  }
116  os << "] ";
117  }
118  if (aTraj.points_[idPoint].velocities_.size() != 0) {
119  os << " Velocities " << idPoint << " - Pos: [";
120  // Read positions.
121  for (std::vector<double>::size_type idPos = 0;
122  idPos < aTraj.points_[idPoint].velocities_.size(); idPos++) {
123  os << "(" << idPos << " : " << aTraj.points_[idPoint].velocities_[idPos]
124  << ") ";
125  }
126  os << "] ";
127  }
128  if (aTraj.points_[idPoint].accelerations_.size() != 0) {
129  os << " Velocities " << idPoint << " - Pos: [";
130  // Read positions.
131  for (std::vector<double>::size_type idPos = 0;
132  idPos < aTraj.points_[idPoint].accelerations_.size(); idPos++) {
133  os << "(" << idPos << " : "
134  << aTraj.points_[idPoint].accelerations_[idPos] << ") ";
135  }
136  os << "] ";
137  }
138 
139  // TODO: read velocities and accelerations.
140  }
141  os << "}" << std::endl;
142 }
143 
144 DG_ADD_CASTER(Trajectory, Traject);
145 */
146 
147 /* --- MULTI BOUND ---------------------------------------------------------- */
148 /* --- MULTI BOUND ---------------------------------------------------------- */
149 /* --- MULTI BOUND ---------------------------------------------------------- */
150 
151 /*
152 DG_SIGNAL_CAST_DEFINITION_TRACE(sot::VectorMultiBound);
153 
154 void SignalCast<VectorMultiBound>::trace(const VectorMultiBound &t,
155  std::ostream &os) {
156  for (VectorMultiBound::const_iterator iter = t.begin(); t.end() != iter;
157  ++iter) {
158  switch (iter->mode) {
159  case MultiBound::MODE_SINGLE:
160  os << iter->getSingleBound() << "\t";
161  break;
162  case MultiBound::MODE_DOUBLE:
163  if (iter->getDoubleBoundSetup(MultiBound::BOUND_INF))
164  os << iter->getDoubleBound(MultiBound::BOUND_INF) << "\t";
165  else
166  os << "-inf\t";
167  if (iter->getDoubleBoundSetup(MultiBound::BOUND_SUP))
168  os << iter->getDoubleBound(MultiBound::BOUND_SUP) << "\t";
169  else
170  os << "+inf\t";
171  break;
172  }
173  }
174 }
175 DG_ADD_CASTER(sot::VectorMultiBound, sotVMB);
176 */
177 
178 } // namespace dynamicgraph


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26