waypoint.hpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Ifdefs
00012 *****************************************************************************/
00013 
00014 #ifndef ECL_MANIPULATORS_WAYPOINT_HPP_
00015 #define ECL_MANIPULATORS_WAYPOINT_HPP_
00016 
00017 /*****************************************************************************
00018 ** Includes
00019 *****************************************************************************/
00020 
00021 #include <string>
00022 #include "types.hpp"
00023 #include <ecl/containers/array.hpp>
00024 #include <ecl/utilities/parameter.hpp>
00025 
00026 /*****************************************************************************
00027 ** Namespaces
00028 *****************************************************************************/
00029 
00030 namespace ecl {
00031 
00032 /*****************************************************************************
00033 ** Class [WayPoint<ManipulatorAngleType>]
00034 *****************************************************************************/
00045 template<enum ManipulatorAngleType Type = JointAngles>
00046 class WayPoint {
00047     private:
00054         WayPoint() {};
00055 };
00056 
00057 /*****************************************************************************
00058 ** Class [WayPoint<unsigned int N,JointAngles>]
00059 *****************************************************************************/
00072 template <>
00073 class WayPoint<JointAngles>
00074 {
00075     public:
00076         typedef Array<double> JointDataArray; 
00079         /******************************************
00080         ** Constructors
00081         *******************************************/
00088         WayPoint() :
00089             name(""),
00090             duration(0.0),
00091             rates_configured(false),
00092             accelerations_configured(false)
00093         {}
00094 
00120         WayPoint(const unsigned int &dimension) :
00121             name(""),
00122             duration(0.0),
00123             rates_configured(false),
00124             accelerations_configured(false),
00125             angle_positions(JointDataArray::Constant(dimension,0.0)),
00126             angle_rates(JointDataArray::Constant(dimension,0.0)),
00127             angle_accelerations(JointDataArray::Constant(dimension,0.0)),
00128             nominal_rates(JointDataArray::Constant(dimension,1.00))
00129         {}
00130 
00137         WayPoint( const WayPoint<JointAngles> &waypoint) :
00138                 name(waypoint.name()),
00139                         duration(waypoint.duration()),
00140                         rates_configured(waypoint.rates_configured()),
00141                         accelerations_configured(waypoint.accelerations_configured()),
00142                         angle_positions(waypoint.angle_positions),
00143                         angle_rates(waypoint.angle_rates),
00144                         angle_accelerations(waypoint.angle_accelerations),
00145                         nominal_rates(waypoint.nominal_rates)
00146                 {}
00147 
00148         virtual ~WayPoint() {}
00149 
00150         /******************************************
00151         ** Configuration
00152         *******************************************/
00160         unsigned int dimension() const { return angle_positions.size(); }
00161 
00166         void redimension(const unsigned int &dimension) {
00167                 angle_positions = JointDataArray::Constant(dimension,0.0);
00168                 angle_rates = JointDataArray::Constant(dimension,0.0);
00169                 angle_accelerations = JointDataArray::Constant(dimension,0.0);
00170                 nominal_rates = JointDataArray::Constant(dimension,0.0);
00171         }
00186         Array<double>& angles() { return angle_positions; }
00191         const Array<double>& angles() const { return angle_positions; }
00192 
00219         Array<double>& rates() {
00220             rates_configured(true);
00221             return angle_rates;
00222         }
00227         const Array<double>& rates() const{
00228             return angle_rates;
00229         }
00230 
00254         Array<double>& accelerations() {
00255             accelerations_configured(true);
00256             return angle_accelerations;
00257         }
00262         const Array<double>& accelerations() const {
00263             return angle_accelerations;
00264         }
00265 
00284         Array<double>& nominalRates() { return nominal_rates; }
00293         void nominalRates(double nom_rate) {
00294                 for ( unsigned int i = 0; i < nominal_rates.size(); ++i) {
00295                         nominal_rates[i] = nom_rate;
00296                 }
00297         }
00298 
00308         bool operator==(const WayPoint<JointAngles>& other) const {
00309                 for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
00310                         if ( angle_positions[i] != other.angle_positions[i] ) {
00311                                 return false;
00312                         }
00313                 }
00314                 return true;
00315         }
00316 
00326         bool operator!=(const WayPoint<JointAngles>& other) const {
00327                 for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
00328                         if ( angle_positions[i] != other.angle_positions[i] ) {
00329                                 return true;
00330                         }
00331                 }
00332                 return false;
00333         }
00334 
00346         bool approx(const WayPoint<JointAngles> &other, const double& epsilon = 0.01) {
00347                 for ( unsigned int i = 0; i < angle_positions.size(); ++i ) {
00348                         if ( std::fabs(angle_positions[i] - other.angle_positions[i]) > epsilon ) {
00349                                 return false;
00350                         }
00351                 }
00352                 return true;
00353         }
00354 
00355         /******************************************
00356         ** Streaming
00357         *******************************************/
00369         template <typename OutputStream>
00370         friend OutputStream& operator << (OutputStream &ostream, WayPoint<JointAngles> &waypoint);
00371 
00372         /******************************************
00373         ** Parameters
00374         *******************************************/
00375         Parameter<std::string> name; 
00376         Parameter<double> duration; 
00377         Parameter<bool> rates_configured; 
00378         Parameter<bool> accelerations_configured; 
00380     private:
00381         JointDataArray angle_positions;
00382         JointDataArray angle_rates; // this is rates *at* the point, currently unused by ycs stuff, but might be useful
00383         JointDataArray angle_accelerations;
00384         JointDataArray nominal_rates; // this is rough rates between points (i.e. slopes)
00385 };
00386 
00387 /*****************************************************************************
00388 ** Implementation [WayPoint][Streaming]
00389 *****************************************************************************/
00390 
00391 template <typename OutputStream>
00392 OutputStream& operator << (OutputStream &ostream, WayPoint<JointAngles> &waypoint) {
00393 
00394         Format< Array<double> > format; format.precision(2); format.width(6);
00395     if ( waypoint.name() != "" ) {
00396         ostream << "  Name                 : " << waypoint.name() << "\n";
00397     }
00398     ostream << "  Angles               : " << format(waypoint.angle_positions) << "\n";
00399     if (waypoint.rates_configured()) {
00400         ostream << "  Rates                : " << format(waypoint.angle_rates) << "\n";
00401     }
00402     if (waypoint.accelerations_configured()) {
00403         ostream << "  Accelerations        : " << format(waypoint.angle_accelerations) << "\n";
00404     }
00405     ostream << "  Nominal Rates        : " << format(waypoint.nominal_rates) << "\n";
00406     ostream.flush();
00407     return ostream;
00408 }
00409 
00410 /*****************************************************************************
00411 ** Typedefs
00412 *****************************************************************************/
00413 
00414 //typedef Waypoint<JointAngles> JointAngleWaypoint; /**< @brief Convenient typedef for joint angle waypoints. **/
00415 
00416 }; // namespace ecl
00417 
00418 #endif /* ECL_MANIPULATORS_WAYPOINT_HPP_ */


ecl_manipulators
Author(s): Daniel Stonier (d.stonier@gmail.com)
autogenerated on Thu Jan 2 2014 11:11:43