waypoint.hpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Ifdefs
12 *****************************************************************************/
13 
14 #ifndef ECL_MANIPULATORS_WAYPOINT_HPP_
15 #define ECL_MANIPULATORS_WAYPOINT_HPP_
16 
17 /*****************************************************************************
18 ** Includes
19 *****************************************************************************/
20 
21 #include <string>
22 #include "types.hpp"
23 #include <ecl/containers/array.hpp>
25 
26 /*****************************************************************************
27 ** Namespaces
28 *****************************************************************************/
29 
30 namespace ecl {
31 
32 /*****************************************************************************
33 ** Class [WayPoint<ManipulatorAngleType>]
34 *****************************************************************************/
45 template<enum ManipulatorAngleType Type = JointAngles>
46 class WayPoint {
47  private:
54  WayPoint() {};
55 };
56 
57 /*****************************************************************************
58 ** Class [WayPoint<unsigned int N,JointAngles>]
59 *****************************************************************************/
72 template <>
74 {
75  public:
79  /******************************************
80  ** Constructors
81  *******************************************/
89  name(""),
90  duration(0.0),
91  rates_configured(false),
92  accelerations_configured(false)
93  {}
94 
120  WayPoint(const unsigned int &dimension) :
121  name(""),
122  duration(0.0),
123  rates_configured(false),
124  accelerations_configured(false),
125  angle_positions(JointDataArray::Constant(dimension,0.0)),
126  angle_rates(JointDataArray::Constant(dimension,0.0)),
127  angle_accelerations(JointDataArray::Constant(dimension,0.0)),
128  nominal_rates(JointDataArray::Constant(dimension,1.00))
129  {}
130 
137  WayPoint( const WayPoint<JointAngles> &waypoint) :
138  name(waypoint.name()),
139  duration(waypoint.duration()),
140  rates_configured(waypoint.rates_configured()),
141  accelerations_configured(waypoint.accelerations_configured()),
142  angle_positions(waypoint.angle_positions),
143  angle_rates(waypoint.angle_rates),
144  angle_accelerations(waypoint.angle_accelerations),
145  nominal_rates(waypoint.nominal_rates)
146  {}
147 
148  virtual ~WayPoint() {}
149 
150  /******************************************
151  ** Configuration
152  *******************************************/
160  unsigned int dimension() const { return angle_positions.size(); }
161 
166  void redimension(const unsigned int &dimension) {
167  angle_positions = JointDataArray::Constant(dimension,0.0);
168  angle_rates = JointDataArray::Constant(dimension,0.0);
169  angle_accelerations = JointDataArray::Constant(dimension,0.0);
170  nominal_rates = JointDataArray::Constant(dimension,0.0);
171  }
186  Array<double>& angles() { return angle_positions; }
191  const Array<double>& angles() const { return angle_positions; }
192 
220  rates_configured(true);
221  return angle_rates;
222  }
227  const Array<double>& rates() const{
228  return angle_rates;
229  }
230 
255  accelerations_configured(true);
256  return angle_accelerations;
257  }
262  const Array<double>& accelerations() const {
263  return angle_accelerations;
264  }
265 
284  Array<double>& nominalRates() { return nominal_rates; }
293  void nominalRates(double nom_rate) {
294  for ( unsigned int i = 0; i < nominal_rates.size(); ++i) {
295  nominal_rates[i] = nom_rate;
296  }
297  }
298 
308  bool operator==(const WayPoint<JointAngles>& other) const {
309  for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
310  if ( angle_positions[i] != other.angle_positions[i] ) {
311  return false;
312  }
313  }
314  return true;
315  }
316 
326  bool operator!=(const WayPoint<JointAngles>& other) const {
327  for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
328  if ( angle_positions[i] != other.angle_positions[i] ) {
329  return true;
330  }
331  }
332  return false;
333  }
334 
346  bool approx(const WayPoint<JointAngles> &other, const double& epsilon = 0.01) {
347  for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
348  if ( std::fabs(angle_positions[i] - other.angle_positions[i]) > epsilon ) {
349  return false;
350  }
351  }
352  return true;
353  }
354 
355  /******************************************
356  ** Streaming
357  *******************************************/
369  template <typename OutputStream>
370  friend OutputStream& operator << (OutputStream &ostream, WayPoint<JointAngles> &waypoint);
371 
372  /******************************************
373  ** Parameters
374  *******************************************/
380  private:
381  JointDataArray angle_positions;
382  JointDataArray angle_rates; // this is rates *at* the point, currently unused by ycs stuff, but might be useful
383  JointDataArray angle_accelerations;
384  JointDataArray nominal_rates; // this is rough rates between points (i.e. slopes)
385 };
386 
387 /*****************************************************************************
388 ** Implementation [WayPoint][Streaming]
389 *****************************************************************************/
390 
391 template <typename OutputStream>
392 OutputStream& operator << (OutputStream &ostream, WayPoint<JointAngles> &waypoint) {
393 
394  Format< Array<double> > format; format.precision(2); format.width(6);
395  if ( waypoint.name() != "" ) {
396  ostream << " Name : " << waypoint.name() << "\n";
397  }
398  ostream << " Angles : " << format(waypoint.angle_positions) << "\n";
399  if (waypoint.rates_configured()) {
400  ostream << " Rates : " << format(waypoint.angle_rates) << "\n";
401  }
402  if (waypoint.accelerations_configured()) {
403  ostream << " Accelerations : " << format(waypoint.angle_accelerations) << "\n";
404  }
405  ostream << " Nominal Rates : " << format(waypoint.nominal_rates) << "\n";
406  ostream.flush();
407  return ostream;
408 }
409 
410 /*****************************************************************************
411 ** Typedefs
412 *****************************************************************************/
413 
414 //typedef Waypoint<JointAngles> JointAngleWaypoint; /**< @brief Convenient typedef for joint angle waypoints. **/
415 
416 }; // namespace ecl
417 
418 #endif /* ECL_MANIPULATORS_WAYPOINT_HPP_ */
void nominalRates(double nom_rate)
Configures the nominal rates with a common value.
Definition: waypoint.hpp:293
const Array< double > & rates() const
Definition: waypoint.hpp:227
JointDataArray nominal_rates
Definition: waypoint.hpp:384
Array< double > & nominalRates()
Handle to the nominal angle rates array.
Definition: waypoint.hpp:284
Primary template for waypoints.
Definition: waypoint.hpp:46
Parameter< bool > accelerations_configured
Accelerations have been configured for this waypoint.
Definition: waypoint.hpp:378
Type definitions for manipulators.
Parameter< std::string > name
String name identifier.
Definition: waypoint.hpp:375
const Array< double > & accelerations() const
Definition: waypoint.hpp:262
bool approx(const WayPoint< JointAngles > &other, const double &epsilon=0.01)
Approximately operator for waypoints.
Definition: waypoint.hpp:346
WayPoint(const WayPoint< JointAngles > &waypoint)
Copy constructor.
Definition: waypoint.hpp:137
WayPoint()
Default constructor.
Definition: waypoint.hpp:88
WayPoint(const unsigned int &dimension)
Configures the waypoint for a specific dimension.
Definition: waypoint.hpp:120
void redimension(const unsigned int &dimension)
Definition: waypoint.hpp:166
WayPoint()
Unreachable constructor.
Definition: waypoint.hpp:54
Parameter< bool > rates_configured
Rates have been configured for this waypoint.
Definition: waypoint.hpp:377
Parameter< double > duration
Timestamp for this waypoint.
Definition: waypoint.hpp:376
Array< double > & angles()
Handle to the joint angle array.
Definition: waypoint.hpp:186
Array< double > JointDataArray
A convenient typedef to the underlying storage array.
Definition: waypoint.hpp:76
bool operator!=(const WayPoint< JointAngles > &other) const
Inequality operator for waypoints.
Definition: waypoint.hpp:326
const Array< double > & angles() const
Definition: waypoint.hpp:191
JointDataArray angle_accelerations
Definition: waypoint.hpp:383
unsigned int dimension() const
Return the waypoint dimension.
Definition: waypoint.hpp:160
Array< double > & rates()
Handle to the joint angle rates array.
Definition: waypoint.hpp:219
JointDataArray angle_rates
Definition: waypoint.hpp:382
JointDataArray angle_positions
Definition: waypoint.hpp:381
WayPoint specialisation with joint (motor) angle storage format.
Definition: waypoint.hpp:73
Joint angle representations are being used.
Definition: types.hpp:33
bool operator==(const WayPoint< JointAngles > &other) const
Equality operator for waypoints.
Definition: waypoint.hpp:308
Array< double > & accelerations()
Handle to the joint angle accelerations array.
Definition: waypoint.hpp:254


ecl_manipulators
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:15:42